summaryrefslogtreecommitdiff
path: root/n/line-follower/src/speed.c
diff options
context:
space:
mode:
Diffstat (limited to 'n/line-follower/src/speed.c')
-rw-r--r--n/line-follower/src/speed.c57
1 files changed, 57 insertions, 0 deletions
diff --git a/n/line-follower/src/speed.c b/n/line-follower/src/speed.c
index 54962b3..a2f2c54 100644
--- a/n/line-follower/src/speed.c
+++ b/n/line-follower/src/speed.c
@@ -23,6 +23,14 @@
*
* }}} */
+/** Maximum value the pwm can reach (defined by the avr configuration) */
+#define PWM_MAX 255
+/** Maximum value for the pwm in linefol mode (controls avg speed) */
+int16_t linefol_max_pwm = 80;
+/** Table and index for calculating the derivate term */
+#define N_LINEPOS_DER_TAB 8 // max : 256
+int16_t linepos_der_tab[N_LINEPOS_DER_TAB];
+uint8_t linepos_der_index;
/** Actual speed. */
int8_t speed_left, speed_right;
/** Wanted speed. */
@@ -104,6 +112,55 @@ speed_update (void)
speed_right = speed_right_aim;
}
}
+/** Compute new pwm value for motors in linefol mode. */
+static inline void
+speed_compute_linefol_pwm (void)
+{
+ int16_t e,speed_linepos_der;
+ uint8_t i;
+ int16_t pwm;
+ // reusing left channel variables
+
+ e = linepos; /* 10b = 8b + 9b */
+
+ /* Derivative update. */
+ i=linepos_der_index; // old index
+ linepos_der_index= // updating index by
+ (linepos_der_index+N_LINEPOS_DER_TAB-1) // modulo adressing
+ & (N_LINEPOS_DER_TAB-1);
+ linepos_der_tab[linepos_der_index] = // last der value
+ (linepos_der_tab[i] + e - speed_left_e_old) * K_ATT_LINEPOS_DER;
+
+ speed_linepos_der = 0;
+ for(i=0 ; i < N_LINEPOS_DER_TAB ; i++) // calculating sum
+ speed_linepos_der += linepos_der_tab[i];
+
+ /* Integral update (reusing left channel variables). */
+ speed_left_int += e; /* 12b = 11b + 10b */
+ if (speed_left_int > speed_int_max) /* 11b */
+ speed_left_int = speed_int_max;
+ else if (speed_left_int < -speed_int_max)
+ speed_left_int = -speed_int_max;
+
+ /* Compute PI. */ /* 16b = 15b + 15b */
+ pwm = dsp_mul_i16f88 (e, speed_kp) /* 15b = 10b * 5.8b */
+ + dsp_mul_i16f88 (speed_left_int, speed_ki) /* 15b = 11b * 4.8b */
+ + dsp_mul_i16f88 (speed_linepos_der, speed_kd); /* 15b = 11b * 4.8b */
+
+ /* Save result. */
+ speed_left_e_old = e;
+ if(pwm > 0)
+ {
+ pwm_left = linefol_max_pwm * (PWM_MAX - pwm);
+ pwm_right = linefol_max_pwm * (PWM_MAX);
+ }
+ else
+ {
+ pwm_left = linefol_max_pwm * PWM_MAX;
+ pwm_right = linefol_max_pwm * (PWM_MAX - pwm);
+ }
+}
+
/** Compute new pwm value for left motor. */
static inline void