From 0f87cab7c3b7757e4a6e7641d2492d15bb588bcf Mon Sep 17 00:00:00 2001 From: "Dmitry V. Sokolov" Date: Mon, 17 Feb 2020 23:00:19 +0100 Subject: code cleanup + comments --- main.c | 85 ++++++++++++++++++++++++++++++++++++------------------------------ 1 file changed, 46 insertions(+), 39 deletions(-) diff --git a/main.c b/main.c index 0149164..6ec0eed 100644 --- a/main.c +++ b/main.c @@ -1,21 +1,29 @@ #define F_CPU 8000000L - #include #include #include volatile uint32_t millis = 0; // an approximation of milliseconds elapsed since boot +// these parameters are to tune depending on your actual hardware const uint8_t zero[3] = {45, 50, 40}; // zero position of the servo (degrees) const uint8_t range[3] = {25, 25, 20}; // the servos are allowed to move in the zero[i] +- range[i] interval -const uint16_t distance_threshold = 768; // ADC threshold for obstacle detection +const uint16_t distance_threshold = 768; // ADC threshold for obstacle detection, in principle atmega8 ADC returns [0,1023] values, but the schematics imposes a narrower interval // movement plannifier variables volatile uint8_t pos[3] = {45, 45, 45}; // current servo position (degrees) volatile uint8_t pos_beg[3] = {45, 45, 45}; // starting position for each servo (degrees) volatile uint8_t pos_end[3] = {45, 45, 45}; // goal position for each servo (degrees) volatile uint32_t time_start[3] = {0, 0, 0}; // beginning of the motion timestamp (milliseconds) -volatile float duration[3] = {.1f, .1f, .1f}; // motion duration (sec) +volatile float duration[3] = {.1f, .1f, .1f}; // movement duration (sec) + +// gait sequences +const uint8_t steps_per_sequence = 4; // each gait is a periodic sequence of 4 steps +const float step_time[4] = {.33f, .166f, .33f, .166f}; // duration of each of 4 steps +const int8_t advance_sequence[4][3] = {{-1, -1, 1}, {-1, -1, -1}, { 1, 1, -1}, { 1, 1, 1}}; // for each of 4 steps +const int8_t retreat_sequence[4][3] = {{-1, -1, -1}, {-1, -1, 1}, { 1, 1, 1}, { 1, 1, -1}}; // these numbers determine movement goals, +const int8_t turn_left_sequence[4][3] = {{ 1, -1, 1}, { 1, -1, -1}, {-1, 1, -1}, {-1, 1, 1}}; // whether we have to add or subtract range[i] from zero[i] +const int8_t turn_right_sequence[4][3] = {{-1, 1, 1}, {-1, 1, -1}, { 1, -1, -1}, { 1, -1, 1}}; // i.e. the goal position of the servo i is zero[i] + range[i]*sequence[step][i] // The servos take a 50 Hz PWM signal; 1 ms minimum pulse width (0 deg), 2 ms maximum pulse width (90 deg). // I have three servos, two of them are attached to a 16 bit timer (timer1), and the third one to a 8 bit timer (timer2). @@ -82,29 +90,31 @@ uint16_t adc_read(uint8_t ch) { #define MAX(a, b) (((a) > (b)) ? (a) : (b)) #define CLAMP(x, low, high) (MIN(MAX((x), (low)), (high))) +// for the given servo, this function returns its current step phase [0.f, 1.f] +inline float movement_phase(uint8_t servo) { + return CLAMP((millis - time_start[servo])/(1000.f * duration[servo]), 0.f, 1.f); +} + +// update PWM servo signals according to the movement planning inline void movement_plannifier() { - for (uint8_t i=0; i<3; i++) { - float t = (millis - time_start[i])/1000.f; - pos[i] = CLAMP(pos_beg[i] + (pos_end[i] - pos_beg[i])*t/duration[i], zero[i]-range[i], zero[i]+range[i]); - } + for (uint8_t i=0; i<3; i++) + pos[i] = CLAMP(pos_beg[i] + (pos_end[i] - pos_beg[i])*movement_phase(i), zero[i]-range[i], zero[i]+range[i]); update_servo_timers(); } +// are we done moving? inline uint8_t is_movement_finished() { uint8_t finished = 1; - for (uint8_t i=0; i<3; i++) { - float t = (millis - time_start[i])/1000.f; - finished &= (t/duration[i]>=1.f); - } + for (uint8_t i=0; i<3; i++) + finished &= (movement_phase(i)>=1.f); return finished; } -const int8_t advance_sequence[4][3] = {{-1, -1, 1}, {-1, -1, -1}, { 1, 1, -1}, { 1, 1, 1}}; -const int8_t retreat_sequence[4][3] = {{-1, -1, -1}, {-1, -1, 1}, { 1, 1, 1}, { 1, 1, -1}}; -const int8_t turn_left_sequence[4][3] = {{ 1, -1, 1}, { 1, -1, -1}, {-1, 1, -1}, {-1, 1, 1}}; -const int8_t turn_right_sequence[4][3] = {{-1, 1, 1}, {-1, 1, -1}, { 1, -1, -1}, { 1, -1, 1}}; - -const float step_durations[4] = {.33f, .166f, .33f, .166f}; +// well duh +void go_to_zero_stance() { + for (uint8_t i=0; i<3; i++) pos[i] = zero[i]; + update_servo_timers(); +} int main(void) { DDRC &= ~_BV(4); // input: right phototransistor @@ -116,12 +126,11 @@ int main(void) { PORTC |= _BV(0); // set the LED control pin to HIGH init_servos(); - for (uint8_t i=0; i<3; i++) pos[i] = zero[i]; - update_servo_timers(); - _delay_ms(2000); + go_to_zero_stance(); + _delay_ms(3000); - int8_t (*sequence)[3] = advance_sequence; - uint8_t step = 3; + int8_t (*sequence)[3] = (int8_t(*)[3])advance_sequence; // by default we go forward + uint8_t step = steps_per_sequence-1; // at the initialization stage the (previous) movement is considered to be complete, thus the next movement will be planned starting from the step 0 uint16_t adc_left_eye = adc_read(5); uint16_t adc_right_eye = adc_read(4); @@ -133,34 +142,32 @@ int main(void) { uint8_t robst = adc_right_eye < distance_threshold; // obstacle on the right? if (is_movement_finished()) { - if (!lobst && !robst) { - sequence = advance_sequence; + if (!lobst && !robst && step==steps_per_sequence-1) { // the obstacles are checked at every step with an exception of go forward decision, it is made at the end of each sequence + sequence = (int8_t(*)[3])advance_sequence; // no obstacles => go forward } else if (lobst && robst) { - sequence = retreat_sequence; + sequence = (int8_t(*)[3])retreat_sequence; // obstacles left and right => go backwards } else if (lobst && !robst) { - sequence = turn_right_sequence; - } else { - sequence = turn_left_sequence; + sequence = (int8_t(*)[3])turn_right_sequence; // obstacle on the left => turn right + } else if (!lobst && robst) { + sequence = (int8_t(*)[3])turn_left_sequence; // obstacle on the right => turn left } - step = (step + 1)%4; - for (uint8_t i=0; i<3; i++) { - pos_beg[i] = pos[i]; - pos_end[i] = zero[i] + range[i]*sequence[step][i]; - time_start[i] = millis; - duration[i] = step_durations[step]; + step = (step + 1) % steps_per_sequence; // if previous movement is complete, then perform the next step + for (uint8_t i=0; i<3; i++) { // for each of 3 servos + pos_beg[i] = pos[i]; // we go from pos_beg[i] + pos_end[i] = zero[i] + range[i]*sequence[step][i]; // to pos_end[i], + time_start[i] = millis; // the movement is initiated at time_start[i] (right now) + duration[i] = step_time[step]; // and should last for step_time[step] } } - movement_plannifier(); + movement_plannifier(); // update the servos position according to the planning _delay_ms(1); - if (millis-start>60L*1000L) break; + if (millis-start>60L*1000L) break; // 60 seconds total running time } - for (uint8_t i=0; i<3; i++) pos[i] = zero[i]; - update_servo_timers(); + go_to_zero_stance(); while (1) _delay_ms(10); - return 0; } -- cgit v1.2.3