X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fcobboard%2Fcs.c;h=386b36024b07d108c2480e044b53cb6546eccaa4;hp=f9c0b7da6ddaa433beba7eb13a77899879dd69b8;hb=f5502f60124c62801685777bb7c699bdfb7e1166;hpb=0c4732c1e255ea56aa766b772dfdb6be49620830 diff --git a/projects/microb2010/cobboard/cs.c b/projects/microb2010/cobboard/cs.c index f9c0b7d..386b360 100644 --- a/projects/microb2010/cobboard/cs.c +++ b/projects/microb2010/cobboard/cs.c @@ -1,7 +1,7 @@ -/* +/* * Copyright Droids Corporation * Olivier Matz - * + * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2 of the License, or @@ -46,8 +46,7 @@ #include "main.h" #include "actuator.h" #include "spickle.h" - -#define DEBUG_CPLD +#include "shovel.h" /* called every 5 ms */ static void do_cs(__attribute__((unused)) void *dummy) @@ -56,11 +55,7 @@ static void do_cs(__attribute__((unused)) void *dummy) if (cobboard.flags & DO_ENCODERS) { encoders_spi_manage(NULL); } -#ifdef DEBUG_CPLD - cobboard.left_spickle.prev = cobboard.left_spickle.cs.filtered_feedback_value; - cobboard.right_spickle.prev = cobboard.right_spickle.cs.filtered_feedback_value; - cobboard.shovel.prev = cobboard.shovel.cs.filtered_feedback_value; -#endif + /* control system */ if (cobboard.flags & DO_CS) { if (cobboard.left_spickle.on) @@ -70,35 +65,21 @@ static void do_cs(__attribute__((unused)) void *dummy) if (cobboard.shovel.on) cs_manage(&cobboard.shovel.cs); } -#ifdef DEBUG_CPLD - { - extern int16_t g_encoders_spi_previous[4]; - int32_t ls, rs, sh; - - ls = (cobboard.left_spickle.prev - cobboard.left_spickle.cs.filtered_feedback_value); - rs = (cobboard.right_spickle.prev - cobboard.right_spickle.cs.filtered_feedback_value); - sh = (cobboard.shovel.prev - cobboard.shovel.cs.filtered_feedback_value); - if (ls < -2000 || ls > 2000 || - rs < -2000 || rs > 2000 || - sh < -2000 || sh > 2000) { - printf_P(PSTR("left_spickle %ld "), ls); - printf_P(PSTR("right_spickle %ld "), rs); - printf_P(PSTR("shovel %ld "), sh); - printf_P(PSTR("/ %d %d %d %d\r\n"), - g_encoders_spi_previous[0], - g_encoders_spi_previous[1], - g_encoders_spi_previous[2], - g_encoders_spi_previous[3]); - BRAKE_ON(); - while (1); - } - } -#endif - if (cobboard.flags & DO_BD) { + if ((cobboard.flags & DO_BD) && (cobboard.flags & DO_POWER)) { bd_manage_from_cs(&cobboard.left_spickle.bd, &cobboard.left_spickle.cs); bd_manage_from_cs(&cobboard.right_spickle.bd, &cobboard.right_spickle.cs); bd_manage_from_cs(&cobboard.shovel.bd, &cobboard.shovel.cs); + + /* urgent case: stop power on blocking */ + if (cobboard.flags & DO_ERRBLOCKING) { +/* if (bd_get(&cobboard.left_spickle.bd) || */ +/* bd_get(&cobboard.right_spickle.bd) || */ +/* bd_get(&cobboard.shovel.bd)) { */ +/* printf_P(PSTR("MOTOR BLOCKED STOP ALL\r\n")); */ +/* cobboard.flags &= ~(DO_POWER | DO_ERRBLOCKING); */ +/* } */ + } } if (cobboard.flags & DO_POWER) BRAKE_OFF(); @@ -139,8 +120,8 @@ void microb_cs_init(void) /* ---- CS left_spickle */ /* PID */ pid_init(&cobboard.left_spickle.pid); - pid_set_gains(&cobboard.left_spickle.pid, 300, 10, 1500); - pid_set_maximums(&cobboard.left_spickle.pid, 0, 10000, 2400); /* max is 12 V */ + pid_set_gains(&cobboard.left_spickle.pid, 400, 10, 1500); + pid_set_maximums(&cobboard.left_spickle.pid, 0, 25000, 4095); pid_set_out_shift(&cobboard.left_spickle.pid, 10); pid_set_derivate_filter(&cobboard.left_spickle.pid, 4); @@ -154,13 +135,13 @@ void microb_cs_init(void) /* Blocking detection */ bd_init(&cobboard.left_spickle.bd); bd_set_speed_threshold(&cobboard.left_spickle.bd, 150); - bd_set_current_thresholds(&cobboard.left_spickle.bd, 500, 8000, 1000000, 40); + bd_set_current_thresholds(&cobboard.left_spickle.bd, 500, 8000, 1000000, 200); /* ---- CS right_spickle */ /* PID */ pid_init(&cobboard.right_spickle.pid); - pid_set_gains(&cobboard.right_spickle.pid, 300, 10, 1500); - pid_set_maximums(&cobboard.right_spickle.pid, 0, 10000, 2400); /* max is 12 V */ + pid_set_gains(&cobboard.right_spickle.pid, 400, 10, 1500); + pid_set_maximums(&cobboard.right_spickle.pid, 0, 25000, 4095); pid_set_out_shift(&cobboard.right_spickle.pid, 10); pid_set_derivate_filter(&cobboard.right_spickle.pid, 4); @@ -174,7 +155,7 @@ void microb_cs_init(void) /* Blocking detection */ bd_init(&cobboard.right_spickle.bd); bd_set_speed_threshold(&cobboard.right_spickle.bd, 150); - bd_set_current_thresholds(&cobboard.right_spickle.bd, 500, 8000, 1000000, 40); + bd_set_current_thresholds(&cobboard.right_spickle.bd, 500, 8000, 1000000, 200); /* ---- CS shovel */ /* PID */ @@ -186,21 +167,21 @@ void microb_cs_init(void) /* quadramp */ quadramp_init(&cobboard.shovel.qr); - quadramp_set_1st_order_vars(&cobboard.shovel.qr, 2000, 2000); /* set speed */ + quadramp_set_1st_order_vars(&cobboard.shovel.qr, 2500, 2500); /* set speed */ quadramp_set_2nd_order_vars(&cobboard.shovel.qr, 50, 20); /* set accel */ /* CS */ cs_init(&cobboard.shovel.cs); cs_set_consign_filter(&cobboard.shovel.cs, quadramp_do_filter, &cobboard.shovel.qr); cs_set_correct_filter(&cobboard.shovel.cs, pid_do_filter, &cobboard.shovel.pid); - cs_set_process_in(&cobboard.shovel.cs, pwm_ng_set, SHOVEL_PWM); + cs_set_process_in(&cobboard.shovel.cs, shovel_set, SHOVEL_PWM); cs_set_process_out(&cobboard.shovel.cs, encoders_spi_get_value, SHOVEL_ENCODER); cs_set_consign(&cobboard.shovel.cs, 0); /* Blocking detection */ bd_init(&cobboard.shovel.bd); bd_set_speed_threshold(&cobboard.shovel.bd, 150); - bd_set_current_thresholds(&cobboard.shovel.bd, 500, 8000, 1000000, 40); + bd_set_current_thresholds(&cobboard.shovel.bd, 500, 8000, 1000000, 200); /* set them on (or not) !! */ cobboard.left_spickle.on = 0;