X-Git-Url: http://git.droids-corp.org/?a=blobdiff_plain;ds=sidebyside;f=projects%2Fmicrob2010%2Fballboard%2Fcs.c;h=35628d2806a23847e3c9ade3f673ddb6b294459c;hb=04f1061f2c5959ce87f632591576097f540dfc14;hp=bab700db0c1cc0a4bcefe78fab4d4d23cd2c75c2;hpb=fa8546ea39c7442ad3bf5a822a72a2b50a41045d;p=aversive.git diff --git a/projects/microb2010/ballboard/cs.c b/projects/microb2010/ballboard/cs.c index bab700d..35628d2 100644 --- a/projects/microb2010/ballboard/cs.c +++ b/projects/microb2010/ballboard/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 @@ -45,6 +45,7 @@ #include "main.h" #include "actuator.h" +#include "beacon.h" int32_t encoders_spi_update_roller_speed(void *number) { @@ -71,11 +72,26 @@ static void do_cs(void *dummy) cs_manage(&ballboard.forktrans.cs); if (ballboard.forkrot.on) cs_manage(&ballboard.forkrot.cs); + if (ballboard.beacon.on) + cs_manage(&ballboard.beacon.cs); } - if (ballboard.flags & DO_BD) { - bd_manage_from_cs(&ballboard.roller.bd, &ballboard.roller.cs); + if ((ballboard.flags & DO_BD) && (ballboard.flags & DO_POWER)) { bd_manage_from_cs(&ballboard.forktrans.bd, &ballboard.forktrans.cs); bd_manage_from_cs(&ballboard.forkrot.bd, &ballboard.forkrot.cs); + bd_manage_from_speed_cmd(&ballboard.roller.bd, + cs_get_filtered_feedback(&ballboard.roller.cs), + cs_get_out(&ballboard.roller.cs)); + +#if 0 + /* urgent case: stop power on blocking */ + if (ballboard.flags & DO_ERRBLOCKING) { + if (bd_get(&ballboard.forktrans.bd) || + bd_get(&ballboard.forkrot.bd)) { + printf_P(PSTR("MOTOR BLOCKED STOP ALL\r\n")); + ballboard.flags &= ~(DO_POWER | DO_ERRBLOCKING); + } + } +#endif } if (ballboard.flags & DO_POWER) BRAKE_OFF(); @@ -86,7 +102,7 @@ static void do_cs(void *dummy) 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)); @@ -112,17 +128,29 @@ void microb_cs_init(void) pid_set_out_shift(&ballboard.roller.pid, 6); pid_set_derivate_filter(&ballboard.roller.pid, 6); + /* QUADRAMP (used as a ramp filter) */ + quadramp_init(&ballboard.roller.qr); + quadramp_set_1st_order_vars(&ballboard.roller.qr, 20, 20); + quadramp_set_2nd_order_vars(&ballboard.roller.qr, 0, 0); + /* CS */ cs_init(&ballboard.roller.cs); + cs_set_consign_filter(&ballboard.roller.cs, quadramp_do_filter, &ballboard.roller.qr); cs_set_correct_filter(&ballboard.roller.cs, pid_do_filter, &ballboard.roller.pid); cs_set_process_in(&ballboard.roller.cs, pwm_ng_set, ROLLER_PWM); cs_set_process_out(&ballboard.roller.cs, encoders_spi_update_roller_speed, ROLLER_ENCODER); cs_set_consign(&ballboard.roller.cs, 0); + /* Blocking detection */ + bd_init(&ballboard.roller.bd); +#define ROLLER_SPEED_THRES (ROLLER_SPEED * 0.75) + bd_set_speed_threshold(&ballboard.roller.bd, ROLLER_SPEED_THRES); + bd_set_current_thresholds(&ballboard.roller.bd, 500, 1500, 1200000, 70); + /* ---- CS forktrans */ /* PID */ pid_init(&ballboard.forktrans.pid); - pid_set_gains(&ballboard.forktrans.pid, 200, 5, 250); + pid_set_gains(&ballboard.forktrans.pid, 30, 5, 0); pid_set_maximums(&ballboard.forktrans.pid, 0, 10000, 2047); pid_set_out_shift(&ballboard.forktrans.pid, 6); pid_set_derivate_filter(&ballboard.forktrans.pid, 6); @@ -143,20 +171,20 @@ void microb_cs_init(void) /* Blocking detection */ bd_init(&ballboard.forktrans.bd); bd_set_speed_threshold(&ballboard.forktrans.bd, 150); - bd_set_current_thresholds(&ballboard.forktrans.bd, 500, 8000, 1000000, 40); + bd_set_current_thresholds(&ballboard.forktrans.bd, 500, 8000, 1000000, 200); /* ---- CS forkrot */ /* PID */ pid_init(&ballboard.forkrot.pid); - pid_set_gains(&ballboard.forkrot.pid, 200, 5, 250); + pid_set_gains(&ballboard.forkrot.pid, 30, 5, 0); pid_set_maximums(&ballboard.forkrot.pid, 0, 10000, 2047); pid_set_out_shift(&ballboard.forkrot.pid, 6); pid_set_derivate_filter(&ballboard.forkrot.pid, 6); /* QUADRAMP */ quadramp_init(&ballboard.forkrot.qr); - quadramp_set_1st_order_vars(&ballboard.forkrot.qr, 200, 200); /* set speed */ - quadramp_set_2nd_order_vars(&ballboard.forkrot.qr, 20, 20); /* set accel */ + quadramp_set_1st_order_vars(&ballboard.forkrot.qr, 1000, 500); /* set speed */ + quadramp_set_2nd_order_vars(&ballboard.forkrot.qr, 100, 20); /* set accel */ /* CS */ cs_init(&ballboard.forkrot.cs); @@ -169,15 +197,32 @@ void microb_cs_init(void) /* Blocking detection */ bd_init(&ballboard.forkrot.bd); bd_set_speed_threshold(&ballboard.forkrot.bd, 150); - bd_set_current_thresholds(&ballboard.forkrot.bd, 500, 8000, 1000000, 40); + bd_set_current_thresholds(&ballboard.forkrot.bd, 500, 8000, 1000000, 200); + + /* BEACON */ + + /* PID */ + pid_init(&ballboard.beacon.pid); + pid_set_gains(&ballboard.beacon.pid, 80, 80, 250); + pid_set_maximums(&ballboard.beacon.pid, 0, 10000, 1500); + pid_set_out_shift(&ballboard.beacon.pid, 6); + pid_set_derivate_filter(&ballboard.beacon.pid, 6); + + /* CS */ + cs_init(&ballboard.beacon.cs); + cs_set_correct_filter(&ballboard.beacon.cs, pid_do_filter, &ballboard.beacon.pid); + cs_set_process_in(&ballboard.beacon.cs, pwm_ng_set, BEACON_PWM); + cs_set_process_out(&ballboard.beacon.cs, encoders_spi_update_beacon_speed, BEACON_ENCODER); + cs_set_consign(&ballboard.beacon.cs, 0); /* set them on !! */ - ballboard.roller.on = 0; + ballboard.roller.on = 1; ballboard.forktrans.on = 1; - ballboard.forkrot.on = 1; + ballboard.forkrot.on = 0; + ballboard.beacon.on = 0; scheduler_add_periodical_event_priority(do_cs, NULL, - 5000L / SCHEDULER_UNIT, + 5000L / SCHEDULER_UNIT, CS_PRIO); }