X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fcs.c;h=6ef8fe934024333b2029c14f93d3f6d649446bdd;hp=ec70f9a2ecebf748b7c6fdb8e6f594ac7f8b1fb5;hb=17aadc4c8c3e60c2b5e6bbba91c8542849addbd7;hpb=5918edd6f4f713ef3c8b0b0020dd30a4fb8222ae diff --git a/projects/microb2010/mainboard/cs.c b/projects/microb2010/mainboard/cs.c index ec70f9a..6ef8fe9 100644 --- a/projects/microb2010/mainboard/cs.c +++ b/projects/microb2010/mainboard/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 @@ -22,6 +22,7 @@ #include #include +#include #include #include @@ -32,7 +33,7 @@ #include #include #include -#include +#include #include #include @@ -51,23 +52,77 @@ #include #include "main.h" +#include "robotsim.h" #include "strat.h" #include "actuator.h" +void dump_cs(const char *name, struct cs *cs); + +#ifndef HOST_VERSION +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; +} +#endif + +#ifndef HOST_VERSION +#define DEBUG_CPLD +#endif + +#ifdef DEBUG_CPLD +extern int16_t g_encoders_spi_previous[4]; +static int32_t ll_prev, rr_prev; +#endif + /* called every 5 ms */ -static void do_cs(void *dummy) +static void do_cs(void *dummy) { static uint16_t cpt = 0; static int32_t old_a = 0, old_d = 0; +#ifdef HOST_VERSION + robotsim_update(); +#else +#ifdef DEBUG_CPLD + int32_t ll, rr; +#endif /* read encoders */ if (mainboard.flags & DO_ENCODERS) { encoders_spi_manage(NULL); } +#ifdef DEBUG_CPLD + ll = encoders_spi_get_value(LEFT_ENCODER); + rr = encoders_spi_get_value(RIGHT_ENCODER); + if ((ll - ll_prev > 3000) || (ll - ll_prev < -3000) || + (rr - rr_prev > 3000) || (rr - rr_prev < -3000)) { + 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); + } + ll_prev = ll; + rr_prev = rr; +#endif +#endif - /* XXX there is an issue which is probably related to avr-libc - * 1.6.2 (debian): this code using fixed_point lib does not - * work with it */ /* robot system, conversion to angle,distance */ if (mainboard.flags & DO_RS) { int16_t a,d; @@ -87,16 +142,34 @@ static void do_cs(void *dummy) cs_manage(&mainboard.angle.cs); if (mainboard.distance.on) cs_manage(&mainboard.distance.cs); +#ifndef HOST_VERSION + if (mainboard.left_cobroller.on) + cs_manage(&mainboard.left_cobroller.cs); + if (mainboard.right_cobroller.on) + cs_manage(&mainboard.right_cobroller.cs); +#endif } if ((cpt & 1) && (mainboard.flags & DO_POS)) { /* about 1.5ms (worst case without centrifugal force * compensation) */ position_manage(&mainboard.pos); } - if (mainboard.flags & DO_BD) { + if ((mainboard.flags & DO_BD) && (mainboard.flags & DO_POWER)) { bd_manage_from_cs(&mainboard.angle.bd, &mainboard.angle.cs); bd_manage_from_cs(&mainboard.distance.bd, &mainboard.distance.cs); +#ifndef HOST_VERSION + 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_ERRBLOCKING) { + if (bd_get(&mainboard.left_cobroller.bd) || + bd_get(&mainboard.left_cobroller.bd)) { + printf_P(PSTR("MOTOR BLOCKED STOP ALL\r\n")); + mainboard.flags &= ~(DO_POWER | DO_ERRBLOCKING); + } + } +#endif } +#ifndef HOST_VERSION if (mainboard.flags & DO_TIMER) { uint8_t second; /* the robot should stop correctly in the strat, but @@ -110,18 +183,26 @@ static void do_cs(void *dummy) while(1); } } +#endif /* brakes */ if (mainboard.flags & DO_POWER) BRAKE_OFF(); else BRAKE_ON(); cpt++; + +#ifdef HOST_VERSION + if ((cpt & 7) == 0) { + // dump_cs("dist", &mainboard.distance.cs); + robotsim_dump(); + } +#endif } 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", + DEBUG(E_USER_CS, "%s cons=% .5"PRIi32" fcons=% .5"PRIi32" err=% .5"PRIi32" " + "in=% .5"PRIi32" out=% .5"PRIi32"", name, cs_get_consign(cs), cs_get_filtered_consign(cs), cs_get_error(cs), cs_get_filtered_feedback(cs), cs_get_out(cs)); @@ -129,8 +210,8 @@ 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"), + printf_P(PSTR("%s cons=% .5"PRIi32" fcons=% .5"PRIi32" err=% .5"PRIi32" " + "in=% .5"PRIi32" out=% .5"PRIi32"\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)); @@ -138,7 +219,7 @@ void dump_cs(const char *name, struct cs *cs) void dump_pid(const char *name, struct pid_filter *pid) { - printf_P(PSTR("%s P=% .8ld I=% .8ld D=% .8ld out=% .8ld\r\n"), + printf_P(PSTR("%s P=% .8"PRIi32" I=% .8"PRIi32" D=% .8"PRIi32" out=% .8"PRIi32"\r\n"), name, pid_get_value_in(pid) * pid_get_gain_P(pid), pid_get_value_I(pid) * pid_get_gain_I(pid), @@ -153,10 +234,17 @@ void microb_cs_init(void) rs_set_left_pwm(&mainboard.rs, pwm_set_and_save, LEFT_PWM); 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); - rs_set_right_ext_encoder(&mainboard.rs, encoders_spi_get_value, - RIGHT_ENCODER, IMP_COEF * -1.006); +#ifdef HOST_VERSION + rs_set_left_ext_encoder(&mainboard.rs, robotsim_encoder_get, + LEFT_ENCODER, IMP_COEF * 1.); + rs_set_right_ext_encoder(&mainboard.rs, robotsim_encoder_get, + RIGHT_ENCODER, IMP_COEF * 1.); +#else + rs_set_left_ext_encoder(&mainboard.rs, encoders_spi_get_value, + LEFT_ENCODER, IMP_COEF * -1.011718); + rs_set_right_ext_encoder(&mainboard.rs, encoders_spi_get_value, + RIGHT_ENCODER, IMP_COEF * 1.012695); +#endif /* rs will use external encoders */ rs_set_flags(&mainboard.rs, RS_USE_EXT); @@ -164,30 +252,31 @@ void microb_cs_init(void) position_init(&mainboard.pos); position_set_physical_params(&mainboard.pos, VIRTUAL_TRACK_MM, DIST_IMP_MM); position_set_related_robot_system(&mainboard.pos, &mainboard.rs); - position_set_centrifugal_coef(&mainboard.pos, 0.000016); + // position_set_centrifugal_coef(&mainboard.pos, 0.000016); position_use_ext(&mainboard.pos); /* TRAJECTORY MANAGER */ - trajectory_init(&mainboard.traj); + trajectory_init(&mainboard.traj, CS_HZ); trajectory_set_cs(&mainboard.traj, &mainboard.distance.cs, &mainboard.angle.cs); trajectory_set_robot_params(&mainboard.traj, &mainboard.rs, &mainboard.pos); trajectory_set_speed(&mainboard.traj, SPEED_DIST_FAST, SPEED_ANGLE_FAST); /* d, a */ + trajectory_set_acc(&mainboard.traj, ACC_DIST, ACC_ANGLE); /* d, a */ /* distance window, angle window, angle start */ trajectory_set_windows(&mainboard.traj, 200., 5.0, 30.); /* ---- CS angle */ /* PID */ pid_init(&mainboard.angle.pid); - pid_set_gains(&mainboard.angle.pid, 500, 10, 7000); - pid_set_maximums(&mainboard.angle.pid, 0, 20000, 4095); + pid_set_gains(&mainboard.angle.pid, 850, 30, 15000); + pid_set_maximums(&mainboard.angle.pid, 0, 10000, 4095); pid_set_out_shift(&mainboard.angle.pid, 10); pid_set_derivate_filter(&mainboard.angle.pid, 4); /* QUADRAMP */ quadramp_init(&mainboard.angle.qr); - quadramp_set_1st_order_vars(&mainboard.angle.qr, 2000, 2000); /* set speed */ - quadramp_set_2nd_order_vars(&mainboard.angle.qr, 13, 13); /* set accel */ + quadramp_set_1st_order_vars(&mainboard.angle.qr, 500, 500); /* set speed */ + quadramp_set_2nd_order_vars(&mainboard.angle.qr, 5, 5); /* set accel */ /* CS */ cs_init(&mainboard.angle.cs); @@ -200,20 +289,20 @@ void microb_cs_init(void) /* Blocking detection */ bd_init(&mainboard.angle.bd); bd_set_speed_threshold(&mainboard.angle.bd, 80); - bd_set_current_thresholds(&mainboard.angle.bd, 500, 8000, 1000000, 50); + bd_set_current_thresholds(&mainboard.angle.bd, 500, 8000, 1000000, 20); /* ---- CS distance */ /* PID */ pid_init(&mainboard.distance.pid); - pid_set_gains(&mainboard.distance.pid, 500, 10, 7000); - pid_set_maximums(&mainboard.distance.pid, 0, 2000, 4095); + pid_set_gains(&mainboard.distance.pid, 850, 30, 15000); + pid_set_maximums(&mainboard.distance.pid, 0, 10000, 4095); pid_set_out_shift(&mainboard.distance.pid, 10); pid_set_derivate_filter(&mainboard.distance.pid, 6); /* QUADRAMP */ quadramp_init(&mainboard.distance.qr); - quadramp_set_1st_order_vars(&mainboard.distance.qr, 2000, 2000); /* set speed */ - quadramp_set_2nd_order_vars(&mainboard.distance.qr, 17, 17); /* set accel */ + quadramp_set_1st_order_vars(&mainboard.distance.qr, 500, 500); /* set speed */ + quadramp_set_2nd_order_vars(&mainboard.distance.qr, 5., 5.); /* set accel */ /* CS */ cs_init(&mainboard.distance.cs); @@ -226,14 +315,63 @@ void microb_cs_init(void) /* Blocking detection */ bd_init(&mainboard.distance.bd); bd_set_speed_threshold(&mainboard.distance.bd, 60); - bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 1000000, 50); + bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 1000000, 20); + +#ifndef HOST_VERSION + /* ---- 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_right_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); +#endif /* !HOST_VERSION */ /* set them on !! */ mainboard.angle.on = 1; mainboard.distance.on = 1; + mainboard.left_cobroller.on = 1; + mainboard.right_cobroller.on = 1; +#ifdef DEBUG_CPLD + ll_prev = encoders_spi_get_value(LEFT_ENCODER); + rr_prev = encoders_spi_get_value(RIGHT_ENCODER); +#endif scheduler_add_periodical_event_priority(do_cs, NULL, 5000L / SCHEDULER_UNIT, CS_PRIO); + }