X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fcobboard%2Fcs.c;h=a71897057388a5d3a5354bfe0e2765abadad4bd8;hp=4d8ccb015f41eb36e69afad300cc595e68c1aa1f;hb=6914527de2ecfef9d790740c71739e7418246b96;hpb=8d6a47e9e21a9a31f4bc12d32fb3d11091a4b305 diff --git a/projects/microb2010/cobboard/cs.c b/projects/microb2010/cobboard/cs.c index 4d8ccb0..a718970 100644 --- a/projects/microb2010/cobboard/cs.c +++ b/projects/microb2010/cobboard/cs.c @@ -45,14 +45,22 @@ #include "main.h" #include "actuator.h" +#include "spickle.h" + +#define DEBUG_CPLD /* called every 5 ms */ -static void do_cs(__attribute__((unused)) void *dummy) +static void do_cs(__attribute__((unused)) void *dummy) { /* read encoders */ 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) @@ -62,10 +70,45 @@ static void do_cs(__attribute__((unused)) void *dummy) if (cobboard.shovel.on) cs_manage(&cobboard.shovel.cs); } - if (cobboard.flags & DO_BD) { +#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) && (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(); @@ -76,7 +119,7 @@ static void do_cs(__attribute__((unused)) void *dummy) void dump_cs_debug(const char *name, struct cs *cs) { DEBUG(E_USER_CS, "%s cons=% .5ld fcons=% .5ld err=% .5ld " - "in=% .5ld out=% .5ld", + "in=% .5ld out=% .5ld", name, cs_get_consign(cs), cs_get_filtered_consign(cs), cs_get_error(cs), cs_get_filtered_feedback(cs), cs_get_out(cs)); @@ -85,7 +128,7 @@ void dump_cs_debug(const char *name, struct cs *cs) void dump_cs(const char *name, struct cs *cs) { printf_P(PSTR("%s cons=% .5ld fcons=% .5ld err=% .5ld " - "in=% .5ld out=% .5ld\r\n"), + "in=% .5ld out=% .5ld\r\n"), name, cs_get_consign(cs), cs_get_filtered_consign(cs), cs_get_error(cs), cs_get_filtered_feedback(cs), cs_get_out(cs)); @@ -106,67 +149,55 @@ void microb_cs_init(void) /* ---- CS left_spickle */ /* PID */ pid_init(&cobboard.left_spickle.pid); - pid_set_gains(&cobboard.left_spickle.pid, 500, 40, 5000); - pid_set_maximums(&cobboard.left_spickle.pid, 0, 5000, 2400); /* max is 12 V */ + 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_out_shift(&cobboard.left_spickle.pid, 10); pid_set_derivate_filter(&cobboard.left_spickle.pid, 4); - /* QUADRAMP */ - quadramp_init(&cobboard.left_spickle.qr); - quadramp_set_1st_order_vars(&cobboard.left_spickle.qr, 2000, 2000); /* set speed */ - quadramp_set_2nd_order_vars(&cobboard.left_spickle.qr, 20, 20); /* set accel */ - /* CS */ cs_init(&cobboard.left_spickle.cs); - cs_set_consign_filter(&cobboard.left_spickle.cs, quadramp_do_filter, &cobboard.left_spickle.qr); cs_set_correct_filter(&cobboard.left_spickle.cs, pid_do_filter, &cobboard.left_spickle.pid); - cs_set_process_in(&cobboard.left_spickle.cs, pwm_ng_set, LEFT_SPICKLE_PWM); + cs_set_process_in(&cobboard.left_spickle.cs, spickle_set, LEFT_SPICKLE_PWM); cs_set_process_out(&cobboard.left_spickle.cs, encoders_spi_get_value, LEFT_SPICKLE_ENCODER); cs_set_consign(&cobboard.left_spickle.cs, 0); /* 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, 500, 40, 5000); - pid_set_maximums(&cobboard.right_spickle.pid, 0, 5000, 2400); /* max is 12 V */ + 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_out_shift(&cobboard.right_spickle.pid, 10); - pid_set_derivate_filter(&cobboard.right_spickle.pid, 6); - - /* QUADRAMP */ - quadramp_init(&cobboard.right_spickle.qr); - quadramp_set_1st_order_vars(&cobboard.right_spickle.qr, 1000, 1000); /* set speed */ - quadramp_set_2nd_order_vars(&cobboard.right_spickle.qr, 20, 20); /* set accel */ + pid_set_derivate_filter(&cobboard.right_spickle.pid, 4); /* CS */ cs_init(&cobboard.right_spickle.cs); - cs_set_consign_filter(&cobboard.right_spickle.cs, quadramp_do_filter, &cobboard.right_spickle.qr); cs_set_correct_filter(&cobboard.right_spickle.cs, pid_do_filter, &cobboard.right_spickle.pid); - cs_set_process_in(&cobboard.right_spickle.cs, pwm_ng_set, RIGHT_SPICKLE_PWM); + cs_set_process_in(&cobboard.right_spickle.cs, spickle_set, RIGHT_SPICKLE_PWM); cs_set_process_out(&cobboard.right_spickle.cs, encoders_spi_get_value, RIGHT_SPICKLE_ENCODER); cs_set_consign(&cobboard.right_spickle.cs, 0); /* 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 */ pid_init(&cobboard.shovel.pid); - pid_set_gains(&cobboard.shovel.pid, 500, 40, 5000); - pid_set_maximums(&cobboard.shovel.pid, 0, 5000, 2400); /* max is 12 V */ + pid_set_gains(&cobboard.shovel.pid, 1000, 10, 1400); + pid_set_maximums(&cobboard.shovel.pid, 0, 10000, 3200); /* max is 18 V */ pid_set_out_shift(&cobboard.shovel.pid, 10); - pid_set_derivate_filter(&cobboard.shovel.pid, 6); + pid_set_derivate_filter(&cobboard.shovel.pid, 4); - /* QUADRAMP */ + /* quadramp */ quadramp_init(&cobboard.shovel.qr); - quadramp_set_1st_order_vars(&cobboard.shovel.qr, 1000, 1000); /* set speed */ - quadramp_set_2nd_order_vars(&cobboard.shovel.qr, 20, 20); /* set accel */ + quadramp_set_1st_order_vars(&cobboard.shovel.qr, 2000, 2000); /* set speed */ + quadramp_set_2nd_order_vars(&cobboard.shovel.qr, 50, 20); /* set accel */ /* CS */ cs_init(&cobboard.shovel.cs); @@ -179,14 +210,14 @@ void microb_cs_init(void) /* 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 !! */ - cobboard.left_spickle.on = 1; - cobboard.right_spickle.on = 1; - cobboard.shovel.on = 1; + /* set them on (or not) !! */ + cobboard.left_spickle.on = 0; + cobboard.right_spickle.on = 0; + cobboard.shovel.on = 0; - scheduler_add_periodical_event_priority(do_cs, NULL, - CS_PERIOD / SCHEDULER_UNIT, + scheduler_add_periodical_event_priority(do_cs, NULL, + CS_PERIOD / SCHEDULER_UNIT, CS_PRIO); }