X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fcs.c;h=516f00b0e27c6266e0c7da03d7d2539d0c773926;hp=81e52457f6af1b27a61a2778f3657241c9e43e53;hb=06813a0566192d90e1ad8c891dfb7cc55dedd525;hpb=ebfaaedd491e61696cc93b353471be15408d23e4 diff --git a/projects/microb2010/mainboard/cs.c b/projects/microb2010/mainboard/cs.c index 81e5245..516f00b 100644 --- a/projects/microb2010/mainboard/cs.c +++ b/projects/microb2010/mainboard/cs.c @@ -56,6 +56,8 @@ #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) { @@ -124,12 +126,19 @@ static void do_cs(void *dummy) * 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 @@ -155,8 +164,10 @@ static void do_cs(void *dummy) cpt++; #ifdef HOST_VERSION - if ((cpt & 7) == 0) + if ((cpt & 7) == 0) { + // dump_cs("dist", &mainboard.distance.cs); robotsim_dump(); + } #endif } @@ -197,14 +208,14 @@ void microb_cs_init(void) /* increase gain to decrease dist, increase left and it will turn more left */ #ifdef HOST_VERSION rs_set_left_ext_encoder(&mainboard.rs, robotsim_encoder_get, - LEFT_ENCODER, IMP_COEF); + LEFT_ENCODER, IMP_COEF * 1.); rs_set_right_ext_encoder(&mainboard.rs, robotsim_encoder_get, - RIGHT_ENCODER, IMP_COEF); + RIGHT_ENCODER, IMP_COEF * 1.); #else rs_set_left_ext_encoder(&mainboard.rs, encoders_spi_get_value, - LEFT_ENCODER, IMP_COEF * -1.00); + LEFT_ENCODER, IMP_COEF * -1.011718); rs_set_right_ext_encoder(&mainboard.rs, encoders_spi_get_value, - RIGHT_ENCODER, IMP_COEF * 1.00); + RIGHT_ENCODER, IMP_COEF * 1.012695); #endif /* rs will use external encoders */ rs_set_flags(&mainboard.rs, RS_USE_EXT); @@ -217,11 +228,12 @@ void microb_cs_init(void) 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.); @@ -235,8 +247,8 @@ void microb_cs_init(void) /* 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); @@ -249,20 +261,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, 100, 7000); + pid_set_gains(&mainboard.distance.pid, 500, 10, 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); /* 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); @@ -275,7 +287,7 @@ 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 */ @@ -310,7 +322,7 @@ void microb_cs_init(void) 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_process_out(&mainboard.right_cobroller.cs, encoders_right_cobroller_speed, RIGHT_COBROLLER_ENCODER); cs_set_consign(&mainboard.right_cobroller.cs, 0); /* Blocking detection */ @@ -320,10 +332,10 @@ void microb_cs_init(void) #endif /* !HOST_VERSION */ /* set them on !! */ - mainboard.angle.on = 0; - mainboard.distance.on = 0; + mainboard.angle.on = 1; + mainboard.distance.on = 1; mainboard.left_cobroller.on = 1; - mainboard.right_cobroller.on = 0; + mainboard.right_cobroller.on = 1; scheduler_add_periodical_event_priority(do_cs, NULL,