code cleanup + comments

1 files 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 <avr/io.h>
 #include <util/delay.h>
 #include <avr/interrupt.h>
 
 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;
 }