summaryrefslogtreecommitdiff
path: root/n/line-follower
diff options
context:
space:
mode:
authorschodet2004-12-19 20:27:11 +0000
committerschodet2004-12-19 20:27:11 +0000
commit8b9bbc5af08793dcf9869703a81381ca1b97884c (patch)
tree5e040ebdb6226c914d77687062d6ec8cda34db6e /n/line-follower
parent006bbbb9bc0909e8d68b8e40289e180a81cb806b (diff)
asservissement vitesse
tests chez nico
Diffstat (limited to 'n/line-follower')
-rw-r--r--n/line-follower/src/linesensor.c16
-rw-r--r--n/line-follower/src/main.c16
-rw-r--r--n/line-follower/src/pwm.c41
-rw-r--r--n/line-follower/src/speed.c50
4 files changed, 97 insertions, 26 deletions
diff --git a/n/line-follower/src/linesensor.c b/n/line-follower/src/linesensor.c
index e425e66..30d70fd 100644
--- a/n/line-follower/src/linesensor.c
+++ b/n/line-follower/src/linesensor.c
@@ -3,7 +3,7 @@
/* linesensor.c */
/* linefol - Line follower robot on a ATmega128. {{{
*
- * Copyright (C) 2004 Nicolas Schodet
+ * Copyright (C) 2004 Pierre Prot
*
* Robot APB Team/Efrei 2005.
* Web: http://assos.efrei.fr/robot/
@@ -48,8 +48,8 @@ static inline void
linesensor_poll (void)
{
uint8_t c, a, i, sum;
- c = PINC;
- a = PINA;
+ c = PINC | 0x01;
+ a = PINA | 0x80;
sum=0;
lineActiveBits = 0;
@@ -83,9 +83,11 @@ static inline void
linesensor_print (void)
{
uint8_t c, a, i;
- c = PINC;
- a = PINA;
+ c = PINC | 0x01;
+ a = PINA | 0x80;
+ linesensor_poll ();
+
for (i = 0; i < 8; i++)
{
if (!(c & 1))
@@ -103,7 +105,9 @@ linesensor_print (void)
rs232_putc ('.');
a >>= 1;
}
- rs232_putc ('\r');
+ rs232_putc (' ');
+
+ proto_send1('P',(linepos + 15));
}
#endif // linesensor_c
diff --git a/n/line-follower/src/main.c b/n/line-follower/src/main.c
index 1b7e857..3e7d84e 100644
--- a/n/line-follower/src/main.c
+++ b/n/line-follower/src/main.c
@@ -42,7 +42,7 @@
* 2 - position control;
* 3 - linefol mode;
* 4 - linefol calibration mode. */
-int8_t motor_mode;
+uint8_t motor_mode;
/** Main loop counter. */
uint8_t motor_loop_cpt;
@@ -87,7 +87,7 @@ int
main (void)
{
DDRD = 0x60;
- pwm_init ();
+ pwm_init (0);
timer_init ();
counter_init ();
speed_init ();
@@ -121,12 +121,17 @@ main_loop (void)
speed_update ();
speed_compute_left_pwm ();
speed_compute_right_pwm ();
+ break;
case 3:
linesensor_poll ();
speed_compute_linefol_pwm ();
+ speed_compute_left_pwm ();
+ speed_compute_right_pwm ();
+ break;
case 4:
delay_ms (5L);
linesensor_print ();
+ break;
}
motor_timer_1 = timer_read ();
/* Pwm setup. */
@@ -223,6 +228,10 @@ 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 = 4;
+ linesensor_reset ();
+ break;
case c ('l', 0):
motor_mode = 3;
linesensor_reset ();
@@ -272,6 +281,9 @@ proto_callback (uint8_t cmd, uint8_t argc, proto_arg_t argv[])
case c ('T', 1):
motor_stat_postrack_cpt = motor_stat_postrack = argv[0];
break;
+ case c ('P', 1):
+ pwm_init (argv[0]);
+ break;
default:
proto_send0 ('e');
return;
diff --git a/n/line-follower/src/pwm.c b/n/line-follower/src/pwm.c
index 883201f..9c5d15e 100644
--- a/n/line-follower/src/pwm.c
+++ b/n/line-follower/src/pwm.c
@@ -30,7 +30,7 @@ int16_t pwm_left, pwm_right;
/** Initialise PWM generator. */
static inline void
-pwm_init (void);
+pwm_init (uint8_t n);
/** Preprocess a PWM value before sending it to hardware. */
static inline uint8_t
@@ -44,16 +44,43 @@ pwm_update (void);
/** Initialise PWM generator. */
static inline void
-pwm_init (void)
+pwm_init (uint8_t n)
{
/* Phase correct PWM, TOP = 0xff, OC1B & OC1C with positive logic.
f_IO without prescaler.
Fpwm = f_IO / (2 * prescaler * TOP) = 28912 Hz. */
- TCCR1A =
- regv (COM1A1, COM1A0, COM1B1, COM1B0, COM1C1, COM1C0, WGM11, WGM10,
- 0, 0, 1, 0, 1, 0, 0, 1);
- TCCR1B = regv (ICNC1, ICES1, 5, WGM13, WGM12, CS12, CS11, CS10,
- 0, 0, 0, 0, 0, 0, 0, 1);
+ switch (n)
+ {
+ case 0:
+ default:
+ TCCR1A =
+ regv (COM1A1, COM1A0, COM1B1, COM1B0, COM1C1, COM1C0, WGM11, WGM10,
+ 0, 0, 1, 0, 1, 0, 0, 1);
+ TCCR1B = regv (ICNC1, ICES1, 5, WGM13, WGM12, CS12, CS11, CS10,
+ 0, 0, 0, 0, 0, 0, 0, 1);
+ break;
+ case 1:
+ TCCR1A =
+ regv (COM1A1, COM1A0, COM1B1, COM1B0, COM1C1, COM1C0, WGM11, WGM10,
+ 0, 0, 1, 0, 1, 0, 1, 0);
+ TCCR1B = regv (ICNC1, ICES1, 5, WGM13, WGM12, CS12, CS11, CS10,
+ 0, 0, 0, 0, 0, 0, 0, 1);
+ break;
+ case 2:
+ TCCR1A =
+ regv (COM1A1, COM1A0, COM1B1, COM1B0, COM1C1, COM1C0, WGM11, WGM10,
+ 0, 0, 1, 0, 1, 0, 1, 1);
+ TCCR1B = regv (ICNC1, ICES1, 5, WGM13, WGM12, CS12, CS11, CS10,
+ 0, 0, 0, 0, 0, 0, 0, 1);
+ break;
+ case 3:
+ TCCR1A =
+ regv (COM1A1, COM1A0, COM1B1, COM1B0, COM1C1, COM1C0, WGM11, WGM10,
+ 0, 0, 1, 0, 1, 0, 0, 1);
+ TCCR1B = regv (ICNC1, ICES1, 5, WGM13, WGM12, CS12, CS11, CS10,
+ 0, 0, 0, 0, 0, 0, 1, 0);
+ break;
+ }
/* Enable pwm and direction outputs in DDRB. */
DDRB |= _BV (7) | _BV (6) | _BV (3) | _BV (2);
}
diff --git a/n/line-follower/src/speed.c b/n/line-follower/src/speed.c
index cafd446..e23c52d 100644
--- a/n/line-follower/src/speed.c
+++ b/n/line-follower/src/speed.c
@@ -25,13 +25,16 @@
#include "linesensor.c"
+// C'est porc :
+extern uint8_t motor_mode;
+
/** Statistics about pid components. */
extern int16_t pid_pid,pid_p,pid_i,pid_d
/** 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;
+int8_t linefol_max_pwm = 80;
/** Attenuation for filtering the derivate term (f88). */
uint16_t K_ATT_LINEPOS_DER = 0x00FE; // ~= 95%
/** Table and index for calculating the derivate term. */
@@ -39,7 +42,7 @@ uint16_t K_ATT_LINEPOS_DER = 0x00FE; // ~= 95%
int16_t linepos_der_tab[N_LINEPOS_DER_TAB];
uint8_t linepos_der_index;
-/** Actual speed. */
+/** Current speed. */
int8_t speed_left, speed_right;
/** Wanted speed. */
int8_t speed_left_aim, speed_right_aim;
@@ -64,10 +67,6 @@ uint16_t speed_kd = 1 * 255;
/* +AutoDec */
-/** Update speeds according to the wanted speeds and the acceleration. */
-static inline void
-speed_update (void);
-
/** Initialise speed parameters. */
static inline void
speed_init (void);
@@ -84,6 +83,9 @@ speed_update (void);
static inline void
speed_compute_linefol_pwm (void);
+static inline int8_t
+linefol_saturate (int16_t input);
+
/** Compute new pwm value for left motor. */
static inline void
speed_compute_left_pwm (void);
@@ -152,6 +154,7 @@ 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)
@@ -195,18 +198,43 @@ speed_compute_linefol_pwm (void)
/* Save result. */
e_old = e;
- if(pwm > 0)
+ if(lineActiveBits > 1)
{
- pwm_left = linefol_max_pwm * (PWM_MAX - pwm);
- pwm_right = linefol_max_pwm * (PWM_MAX);
+ if(pwm > 0)
+ {
+ speed_left = linefol_saturate (((linefol_max_pwm * PWM_MAX) >> 8) - pwm);
+ speed_right = linefol_saturate ((linefol_max_pwm * PWM_MAX) >> 8);
+// pwm_left = ((linefol_max_pwm * PWM_MAX) >> 8) - pwm;
+// pwm_right = ((linefol_max_pwm * PWM_MAX) >> 8);
+ }
+ else
+ {
+ speed_left = linefol_saturate ((linefol_max_pwm * PWM_MAX) >> 8);
+ speed_right = linefol_saturate (((linefol_max_pwm * PWM_MAX) >> 8 )+ pwm);
+// pwm_left = ((linefol_max_pwm * PWM_MAX) >> 8);
+// pwm_right = ((linefol_max_pwm * PWM_MAX) >> 8 )+ pwm;
+ }
}
else
{
- pwm_left = linefol_max_pwm * PWM_MAX;
- pwm_right = linefol_max_pwm * (PWM_MAX - pwm);
+ motor_mode = 2;
+ speed_left_aim = 0;
+ speed_right_aim = 0;
+// pwm_left = 0;
+// pwm_right = 0;
}
}
+static inline int8_t
+linefol_saturate (int16_t input)
+{
+ if (input > 127)
+ return 127;
+ else if (input < -127)
+ return -127;
+ else
+ return input;
+}
/** Compute new pwm value for left motor. */
static inline void