summaryrefslogtreecommitdiff
path: root/n/line-follower/src/speed.c
diff options
context:
space:
mode:
authorprot2004-12-12 00:34:30 +0000
committerprot2004-12-12 00:34:30 +0000
commit82eca8372fdd8c56a136b09b534e4369ce4a2486 (patch)
treea7d9209cbd40cfa5d3e852514b79e25dabdbc919 /n/line-follower/src/speed.c
parent6b1b0588ae906055da621a957e3c24a2dbb3e8a1 (diff)
Added protocol for mode 3 and 4
Renaming variables -> Quite good for test !
Diffstat (limited to 'n/line-follower/src/speed.c')
-rw-r--r--n/line-follower/src/speed.c47
1 files changed, 24 insertions, 23 deletions
diff --git a/n/line-follower/src/speed.c b/n/line-follower/src/speed.c
index ed883a5..99c7023 100644
--- a/n/line-follower/src/speed.c
+++ b/n/line-follower/src/speed.c
@@ -23,17 +23,18 @@
*
* }}} */
+#include "linesensor.c"
-/** Maximum value the pwm can reach (defined by the avr configuration) */
+/** 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) */
+/** 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 */
+/** Attenuation for filtering the derivate term (f88). */
+uint16_t K_ATT_LINEPOS_DER = 0x00FE; // ~= 95%
+/** 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;
-/** Attenuation for filtering the derivate term */
-#define K_ATT_LINEPOS_DER 0.95
/** Actual speed. */
int8_t speed_left, speed_right;
@@ -44,11 +45,12 @@ uint8_t speed_acc = 8;
/** Acceleration counter, speed gets updated when it reachs 0. */
uint8_t speed_acc_cpt;
/** Integral term. */
-int16_t speed_left_int, speed_right_int;
+int16_t speed_left_int, speed_right_int, linepos_int;
/** Integral max value. */
int16_t speed_int_max = 1024;
+int16_t linepos_int_max = 1024;
/** Last error value. */
-int16_t speed_left_e_old, speed_right_e_old;
+int16_t speed_left_e_old, speed_right_e_old, e_old;
/** P coeficients. 5.8 fixed point format. */
uint16_t speed_kp = 2 * 255;
/** I coeficients. 4.8 fixed point format. */
@@ -56,7 +58,6 @@ uint16_t speed_ki = 1 * 255;
/** D coeficient. 4.8 fixed point format. */
uint16_t speed_kd = 1 * 255;
-#include "linesensor.c"
/* +AutoDec */
@@ -124,40 +125,40 @@ speed_update (void)
static inline void
speed_compute_linefol_pwm (void)
{
- int16_t e,speed_linepos_der;
+ int16_t e, linepos_der;
uint8_t i;
int16_t pwm;
// reusing left channel variables
- linesensor_poll (void);
e = linepos; /* 10b = 8b + 9b */
/* Derivative update. */
i=linepos_der_index; // old index
- linepos_der_index= // updating index by
+ linepos_der_index= // updating new index
(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;
+ linepos_der_tab[linepos_der_index] = // new der value
+ dsp_mul_i16f88 ((linepos_der_tab[i] + e - e_old),
+ K_ATT_LINEPOS_DER);
- speed_linepos_der = 0;
+ linepos_der = 0;
for(i=0 ; i < N_LINEPOS_DER_TAB ; i++) // calculating sum
- speed_linepos_der += linepos_der_tab[i];
+ 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;
+ linepos_int += e; /* 12b = 11b + 10b */
+ if (linepos_int > linepos_int_max) /* 11b */
+ linepos_int = linepos_int_max;
+ else if (linepos_int < -linepos_int_max)
+ linepos_int = -linepos_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 */
+ + dsp_mul_i16f88 (linepos_int, speed_ki) /* 15b = 11b * 4.8b */
+ + dsp_mul_i16f88 (linepos_der, speed_kd); /* 15b = 11b * 4.8b */
/* Save result. */
- speed_left_e_old = e;
+ e_old = e;
if(pwm > 0)
{
pwm_left = linefol_max_pwm * (PWM_MAX - pwm);