summaryrefslogtreecommitdiff
path: root/n/line-follower
diff options
context:
space:
mode:
authorprot2004-12-10 02:01:05 +0000
committerprot2004-12-10 02:01:05 +0000
commit1fcc21169ab15064ca0acbc0688d2f75a835a443 (patch)
treee216ca55611516ed1763b0074d8f774aa00c5898 /n/line-follower
parenta92bc5bb18c2fe75e8cc6b22023c61854188f70a (diff)
Adding pid for linefol mode.
(Still under work)
Diffstat (limited to 'n/line-follower')
-rw-r--r--n/line-follower/src/main.c9
-rw-r--r--n/line-follower/src/speed.c57
2 files changed, 64 insertions, 2 deletions
diff --git a/n/line-follower/src/main.c b/n/line-follower/src/main.c
index 72d4b39..f0059f9 100644
--- a/n/line-follower/src/main.c
+++ b/n/line-follower/src/main.c
@@ -38,7 +38,8 @@
/** Motor mode :
* 0 - pwm setup;
* 1 - speed control;
- * 2 - position control. */
+ * 2 - position control;
+ * 3 - linefol mode. */
int8_t motor_mode;
/** Main loop counter. */
@@ -107,7 +108,11 @@ main_loop (void)
postrack_update ();
motor_timer_2 = timer_read ();
/* Speed control. */
- if (motor_mode >= 1)
+ if (motor_mode = 3)
+ {
+ speed_compute_linefol_pwm ();
+ }
+ else if (motor_mode >= 1)
{
speed_update ();
speed_compute_left_pwm ();
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