X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fcs.c;h=39e39efc211e6098fc236ac93d84d8e73a9810d7;hp=ec70f9a2ecebf748b7c6fdb8e6f594ac7f8b1fb5;hb=d1cc3debac68c0012f2c5ca33da9bcf0882f4988;hpb=5918edd6f4f713ef3c8b0b0020dd30a4fb8222ae

diff --git a/projects/microb2010/mainboard/cs.c b/projects/microb2010/mainboard/cs.c
index ec70f9a..39e39ef 100644
--- 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,