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.c50
1 files changed, 39 insertions, 11 deletions
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