X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fcs.c;h=9884bcfbd969ee663255aa3fb2a31e8549f8632c;hp=81e52457f6af1b27a61a2778f3657241c9e43e53;hb=9d056416278ae5d772a0fc1f7d2d11a4461fb673;hpb=ebfaaedd491e61696cc93b353471be15408d23e4 diff --git a/projects/microb2010/mainboard/cs.c b/projects/microb2010/mainboard/cs.c index 81e5245..9884bcf 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); @@ -213,30 +224,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); @@ -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_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); @@ -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,