trajectory: follow a line

[aversive.git] / projects / microb2010 / mainboard / cs.c

diff --git a/projects/microb2010/mainboard/cs.c b/projects/microb2010/mainboard/cs.c
index ec70f9a..39e39ef 100644 (file)
--- a/projects/microb2010/mainboard/cs.c
+++ b/projects/microb2010/mainboard/cs.c
@@ -32,7 +32,7 @@
 #include <pwm_ng.h>
 #include <timer.h>
 #include <scheduler.h>
-#include <time.h>
+#include <clock_time.h>
 #include <adc.h>
 
 #include <pid.h>
@@ -54,6 +54,26 @@
 #include "strat.h"
 #include "actuator.h"
 
+int32_t encoders_left_cobroller_speed(void *number)
+{
+       static volatile int32_t roller_pos;
+       int32_t tmp, speed;
+       tmp = encoders_spi_get_value(number);
+       speed = tmp - roller_pos;
+       roller_pos = tmp;
+       return speed;
+}
+
+int32_t encoders_right_cobroller_speed(void *number)
+{
+       static volatile int32_t roller_pos;
+       int32_t tmp, speed;
+       tmp = encoders_spi_get_value(number);
+       speed = tmp - roller_pos;
+       roller_pos = tmp;
+       return speed;
+}
+
 /* called every 5 ms */
 static void do_cs(void *dummy) 
 {
@@ -87,6 +107,10 @@ static void do_cs(void *dummy)
                        cs_manage(&mainboard.angle.cs);
                if (mainboard.distance.on)
                        cs_manage(&mainboard.distance.cs);
+               if (mainboard.left_cobroller.on)
+                       cs_manage(&mainboard.left_cobroller.cs);
+               if (mainboard.right_cobroller.on)
+                       cs_manage(&mainboard.right_cobroller.cs);
        }
        if ((cpt & 1) && (mainboard.flags & DO_POS)) {
                /* about 1.5ms (worst case without centrifugal force
@@ -96,6 +120,8 @@ static void do_cs(void *dummy)
        if (mainboard.flags & DO_BD) {
                bd_manage_from_cs(&mainboard.angle.bd, &mainboard.angle.cs);
                bd_manage_from_cs(&mainboard.distance.bd, &mainboard.distance.cs);
+               bd_manage_from_cs(&mainboard.left_cobroller.bd, &mainboard.left_cobroller.cs);
+               bd_manage_from_cs(&mainboard.right_cobroller.bd, &mainboard.right_cobroller.cs);
        }
        if (mainboard.flags & DO_TIMER) {
                uint8_t second;
@@ -154,9 +180,9 @@ void microb_cs_init(void)
        rs_set_right_pwm(&mainboard.rs,  pwm_set_and_save, RIGHT_PWM);
        /* increase gain to decrease dist, increase left and it will turn more left */
        rs_set_left_ext_encoder(&mainboard.rs, encoders_spi_get_value, 
-                               LEFT_ENCODER, IMP_COEF * 1.0015);
+                               LEFT_ENCODER, IMP_COEF * -1.00);
        rs_set_right_ext_encoder(&mainboard.rs, encoders_spi_get_value, 
-                                RIGHT_ENCODER, IMP_COEF * -1.006);
+                                RIGHT_ENCODER, IMP_COEF * 1.00);
        /* rs will use external encoders */
        rs_set_flags(&mainboard.rs, RS_USE_EXT);
 
@@ -205,7 +231,7 @@ void microb_cs_init(void)
        /* ---- CS distance */
        /* PID */
        pid_init(&mainboard.distance.pid);
-       pid_set_gains(&mainboard.distance.pid, 500, 10, 7000);
+       pid_set_gains(&mainboard.distance.pid, 500, 100, 7000);
        pid_set_maximums(&mainboard.distance.pid, 0, 2000, 4095);
        pid_set_out_shift(&mainboard.distance.pid, 10);
        pid_set_derivate_filter(&mainboard.distance.pid, 6);
@@ -228,9 +254,51 @@ void microb_cs_init(void)
        bd_set_speed_threshold(&mainboard.distance.bd, 60);
        bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 1000000, 50);
 
+       /* ---- CS left_cobroller */
+       /* PID */
+       pid_init(&mainboard.left_cobroller.pid);
+       pid_set_gains(&mainboard.left_cobroller.pid, 80, 10, 10);
+       pid_set_maximums(&mainboard.left_cobroller.pid, 0, 30000, 4095);
+       pid_set_out_shift(&mainboard.left_cobroller.pid, 5);
+       pid_set_derivate_filter(&mainboard.left_cobroller.pid, 6);
+
+       /* CS */
+       cs_init(&mainboard.left_cobroller.cs);
+       cs_set_correct_filter(&mainboard.left_cobroller.cs, pid_do_filter, &mainboard.left_cobroller.pid);
+       cs_set_process_in(&mainboard.left_cobroller.cs, pwm_ng_set, LEFT_COBROLLER_PWM);
+       cs_set_process_out(&mainboard.left_cobroller.cs, encoders_left_cobroller_speed, LEFT_COBROLLER_ENCODER);
+       cs_set_consign(&mainboard.left_cobroller.cs, 0);
+
+       /* Blocking detection */
+       bd_init(&mainboard.left_cobroller.bd);
+       bd_set_speed_threshold(&mainboard.left_cobroller.bd, 60);
+       bd_set_current_thresholds(&mainboard.left_cobroller.bd, 500, 8000, 1000000, 50);
+
+       /* ---- CS right_cobroller */
+       /* PID */
+       pid_init(&mainboard.right_cobroller.pid);
+       pid_set_gains(&mainboard.right_cobroller.pid, 80, 10, 10);
+       pid_set_maximums(&mainboard.right_cobroller.pid, 0, 30000, 4095);
+       pid_set_out_shift(&mainboard.right_cobroller.pid, 5);
+       pid_set_derivate_filter(&mainboard.right_cobroller.pid, 6);
+
+       /* CS */
+       cs_init(&mainboard.right_cobroller.cs);
+       cs_set_correct_filter(&mainboard.right_cobroller.cs, pid_do_filter, &mainboard.right_cobroller.pid);
+       cs_set_process_in(&mainboard.right_cobroller.cs, pwm_ng_set, RIGHT_COBROLLER_PWM);
+       cs_set_process_out(&mainboard.right_cobroller.cs, encoders_left_cobroller_speed, RIGHT_COBROLLER_ENCODER);
+       cs_set_consign(&mainboard.right_cobroller.cs, 0);
+
+       /* Blocking detection */
+       bd_init(&mainboard.right_cobroller.bd);
+       bd_set_speed_threshold(&mainboard.right_cobroller.bd, 60);
+       bd_set_current_thresholds(&mainboard.right_cobroller.bd, 500, 8000, 1000000, 50);
+
        /* set them on !! */
-       mainboard.angle.on = 1;
-       mainboard.distance.on = 1;
+       mainboard.angle.on = 0;
+       mainboard.distance.on = 0;
+       mainboard.left_cobroller.on = 1;
+       mainboard.right_cobroller.on = 0;
 
 
        scheduler_add_periodical_event_priority(do_cs, NULL,