summaryrefslogtreecommitdiff
path: root/n/line-follower
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
parent6b1b0588ae906055da621a957e3c24a2dbb3e8a1 (diff)
Added protocol for mode 3 and 4
Renaming variables -> Quite good for test !
Diffstat (limited to 'n/line-follower')
-rw-r--r--n/line-follower/src/linesensor.c9
-rw-r--r--n/line-follower/src/main.c42
-rw-r--r--n/line-follower/src/speed.c47
3 files changed, 66 insertions, 32 deletions
diff --git a/n/line-follower/src/linesensor.c b/n/line-follower/src/linesensor.c
index 8daf00f..c43165a 100644
--- a/n/line-follower/src/linesensor.c
+++ b/n/line-follower/src/linesensor.c
@@ -1,3 +1,5 @@
+#ifndef linesensor_c
+#define linesensor_c
/* linesensor.c */
/* linefol - Line follower robot on a ATmega128. {{{
*
@@ -25,7 +27,7 @@
/** The line position between -8 and +8. */
static int8_t linepos;
-/** The number of bits seeing the line. */
+/** The number of bits seeing a line ( ~= width of the line). */
static int8_t lineActiveBits;
/* +AutoDec */
@@ -41,7 +43,7 @@ linesensor_poll (void);
/** Poll linesensor and returns the state on the RS232 output.
* For test, calibration and verification. */
static inline void
-linesensor_poll_human (void);
+linesensor_print (void);
/* -AutoDec */
@@ -86,7 +88,7 @@ linesensor_poll (void)
/** Poll linesensor and returns the state on the RS232 output.
* For test, calibration and verification. */
static inline void
-linesensor_poll_human (void)
+linesensor_print (void)
{
uint8_t c, a, i;
c = PINC;
@@ -112,3 +114,4 @@ linesensor_poll_human (void)
rs232_putc ('\r');
}
+#endif // linesensor_c
diff --git a/n/line-follower/src/main.c b/n/line-follower/src/main.c
index a8aceb2..1c89604 100644
--- a/n/line-follower/src/main.c
+++ b/n/line-follower/src/main.c
@@ -34,12 +34,14 @@
#include "counter.c"
#include "speed.c"
#include "postrack.c"
+#include "linesensor.c"
/** Motor mode :
* 0 - pwm setup;
* 1 - speed control;
* 2 - position control;
- * 3 - linefol mode. */
+ * 3 - linefol mode;
+ * 4 - linefol calibration mode. */
int8_t motor_mode;
/** Main loop counter. */
@@ -108,15 +110,22 @@ main_loop (void)
postrack_update ();
motor_timer_2 = timer_read ();
/* Speed control. */
- if (motor_mode == 3)
- {
- speed_compute_linefol_pwm ();
- }
- else if (motor_mode >= 1)
+ switch (motor_mode)
{
+ case 1:
+ case 2:
speed_update ();
speed_compute_left_pwm ();
speed_compute_right_pwm ();
+ case 3:
+ linesensor_poll ();
+ speed_compute_linefol_pwm ();
+ case 4:
+ delay_ms (5L);
+ linesensor_print ();
+ default:
+ motor_mode = motor_mode; // j'ai pas trouvé comment faire
+ // autrement...
}
motor_timer_1 = timer_read ();
/* Pwm setup. */
@@ -202,6 +211,24 @@ proto_callback (uint8_t cmd, uint8_t argc, proto_arg_t argv[])
speed_left_aim = argv[0];
speed_right_aim = argv[1];
break;
+ case c ('l', 0):
+ motor_mode = 3;
+ linesensor_init ();
+ break;
+ case c ('l', 1):
+ motor_mode = 3;
+ linesensor_init ();
+ linefol_max_pwm = argv[0];
+ break;
+ case c ('l', 5):
+ motor_mode = 3;
+ linesensor_init ();
+ linefol_max_pwm = argv[0];
+ speed_kp = argv[1];
+ speed_ki = argv[2];
+ speed_kd = argv[3];
+ K_ATT_LINEPOS_DER = argv[4];
+ break;
case c ('a', 1):
speed_acc_cpt = speed_acc = argv[0];
break;
@@ -211,6 +238,9 @@ proto_callback (uint8_t cmd, uint8_t argc, proto_arg_t argv[])
case c ('i', 2):
speed_ki = argv[0] << 8 | argv[1];
break;
+ case c ('d', 2):
+ speed_kd = argv[0] << 8 | argv[1];
+ break;
case c ('f', 2):
postrack_set_footing (argv[0] << 8 | argv[1]);
break;
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);