void dual_quadramp_do_filter(struct dual_quadramp_filter *q) { int32_t x_speed, y_speed; int32_t x_acc, y_acc; int x_distance, y_distance; x_distance = abs(q->x_pos - q->x_consign); y_distance = abs(q->y_pos - q->y_consign); x_speed = (int32_t)(q->speed * x_distance / (float)(x_distance + y_distance)); y_speed = (int32_t)(q->speed * y_distance / (float)(x_distance + y_distance)); x_acc = (int32_t)(q->acc * x_distance / (float)(x_distance + y_distance)); y_acc = (int32_t)(q->acc * y_distance / (float)(x_distance + y_distance)); quadramp_set_1st_order_vars(&q->x_quadramp, x_speed, x_speed); quadramp_set_2nd_order_vars(&q->x_quadramp, x_acc, x_acc); quadramp_set_1st_order_vars(&q->y_quadramp, y_speed, y_speed); quadramp_set_2nd_order_vars(&q->y_quadramp, y_acc, y_acc); q->x_pos = quadramp_do_filter((void *)&q->x_quadramp, q->x_consign); q->y_pos = quadramp_do_filter((void *)&q->y_quadramp, q->y_consign); }
/** set speed consign in quadramp filter */ void set_quadramp_acc(struct trajectory *traj, double d_acc, double a_acc) { struct quadramp_filter * q_d, * q_a; q_d = traj->csm_distance->consign_filter_params; q_a = traj->csm_angle->consign_filter_params; quadramp_set_2nd_order_vars(q_d, ABS(d_acc), ABS(d_acc)); quadramp_set_2nd_order_vars(q_a, ABS(a_acc), ABS(a_acc)); }
int main(void) { struct quadramp_filter q; int32_t in=0, out=0, prev_out=0; int32_t i=0; quadramp_init(&q); for (i=0 ; i<4000 ; i++) { switch(i) { case 0: quadramp_set_1st_order_vars(&q, 50, 100); quadramp_set_2nd_order_vars(&q, 1, 2); in = 10000; break; case 600: in = 9000; break; case 630: in = 10000; break; case 1000: in = -5000; break; case 1500: in = -4000; break; case 2000: quadramp_set_1st_order_vars(&q, 10, 10); quadramp_set_2nd_order_vars(&q, 2, 2); in = 10000; break; case 3000: quadramp_set_1st_order_vars(&q, 100, 100); break; default: break; } out = quadramp_do_filter(&q, in); printf("%" SCNu32 " %" SCNu32 " %" SCNu32 " %" SCNu32 "\n", i, in, out, out-prev_out); prev_out = out; } return 0; }
void robot_cs_init(robot_cs_t* rcs) { // zeroing structures rcs->hrs = NULL; rcs->hpm = NULL; // set CS on rcs->active = 1; // CS not reactivated since last tick rcs->reactivated = 0; // setup PIDs pid_init(&pid_x); pid_init(&pid_y); pid_init(&pid_angle); pid_set_gains(&pid_x, SETTING_PID_X_GAIN_P, SETTING_PID_X_GAIN_I, SETTING_PID_X_GAIN_D); pid_set_maximums(&pid_x, SETTING_PID_X_MAX_IN, SETTING_PID_X_MAX_I, SETTING_PID_X_MAX_OUT); pid_set_out_shift(&pid_x, SETTING_PID_X_SHIFT); pid_set_gains(&pid_y, SETTING_PID_Y_GAIN_P, SETTING_PID_Y_GAIN_I, SETTING_PID_Y_GAIN_D); pid_set_maximums(&pid_y, SETTING_PID_Y_MAX_IN, SETTING_PID_Y_MAX_I, SETTING_PID_Y_MAX_OUT); pid_set_out_shift(&pid_y, SETTING_PID_Y_SHIFT); pid_set_gains(&pid_angle, SETTING_PID_A_GAIN_P, SETTING_PID_A_GAIN_I, SETTING_PID_A_GAIN_D); pid_set_maximums(&pid_angle, SETTING_PID_A_MAX_IN, SETTING_PID_A_MAX_I, SETTING_PID_A_MAX_OUT); pid_set_out_shift(&pid_angle, SETTING_PID_A_SHIFT); // quadramp quadramp_set_1st_order_vars(&qramp_angle, SETTING_QRAMP_A_SPEED, SETTING_QRAMP_A_SPEED); quadramp_set_2nd_order_vars(&qramp_angle, SETTING_QRAMP_A_ACC, SETTING_QRAMP_A_ACC); // setup CSMs cs_init(&csm_x); cs_init(&csm_y); cs_init(&csm_angle); cs_set_consign_filter(&csm_x, NULL, NULL); cs_set_consign_filter(&csm_y, NULL, NULL); cs_set_consign_filter(&csm_angle, &quadramp_do_filter, &qramp_angle); cs_set_correct_filter(&csm_x, &pid_do_filter, &pid_x); cs_set_correct_filter(&csm_y, &pid_do_filter, &pid_y); cs_set_correct_filter(&csm_angle, &pid_do_filter, &pid_angle); cs_set_feedback_filter(&csm_x, NULL, NULL); cs_set_feedback_filter(&csm_y, NULL, NULL); cs_set_feedback_filter(&csm_angle, NULL, NULL); cs_set_process_out(&csm_x, &get_robot_x, rcs); cs_set_process_out(&csm_y, &get_robot_y, rcs); cs_set_process_out(&csm_angle, &get_robot_a, rcs); cs_set_process_in(&csm_x, NULL, NULL); cs_set_process_in(&csm_y, NULL, NULL); cs_set_process_in(&csm_angle, NULL, NULL); }
void microb_cs_init(void) { /* ROBOT_SYSTEM */ rs_init(&mainboard.rs); 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_microb_get_value, LEFT_ENCODER, IMP_COEF * -1.0000); rs_set_right_ext_encoder(&mainboard.rs, encoders_microb_get_value, RIGHT_ENCODER, IMP_COEF * 1.0000); /* rs will use external encoders */ rs_set_flags(&mainboard.rs, RS_USE_EXT); /* POSITION MANAGER */ 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_use_ext(&mainboard.pos); /* TRAJECTORY MANAGER */ trajectory_init(&mainboard.traj); 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, 1500, 1500); /* 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_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 */ /* CS */ cs_init(&mainboard.angle.cs); cs_set_consign_filter(&mainboard.angle.cs, quadramp_do_filter, &mainboard.angle.qr); cs_set_correct_filter(&mainboard.angle.cs, pid_do_filter, &mainboard.angle.pid); cs_set_process_in(&mainboard.angle.cs, rs_set_angle, &mainboard.rs); cs_set_process_out(&mainboard.angle.cs, rs_get_angle, &mainboard.rs); cs_set_consign(&mainboard.angle.cs, 0); /* 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); /* ---- 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_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 */ /* CS */ cs_init(&mainboard.distance.cs); cs_set_consign_filter(&mainboard.distance.cs, quadramp_do_filter, &mainboard.distance.qr); cs_set_correct_filter(&mainboard.distance.cs, pid_do_filter, &mainboard.distance.pid); cs_set_process_in(&mainboard.distance.cs, rs_set_distance, &mainboard.rs); cs_set_process_out(&mainboard.distance.cs, rs_get_distance, &mainboard.rs); cs_set_consign(&mainboard.distance.cs, 0); /* 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); /* ---- CS fessor */ fessor_autopos(); /* PID */ pid_init(&mainboard.fessor.pid); pid_set_gains(&mainboard.fessor.pid, 300, 10, 150); pid_set_maximums(&mainboard.fessor.pid, 0, 10000, 4095); pid_set_out_shift(&mainboard.fessor.pid, 10); pid_set_derivate_filter(&mainboard.fessor.pid, 4); /* CS */ cs_init(&mainboard.fessor.cs); cs_set_correct_filter(&mainboard.fessor.cs, pid_do_filter, &mainboard.fessor.pid); cs_set_process_in(&mainboard.fessor.cs, fessor_set, NULL); cs_set_process_out(&mainboard.fessor.cs, encoders_microb_get_value, FESSOR_ENC); fessor_up(); /* ---- CS elevator */ elevator_autopos(); /* PID */ pid_init(&mainboard.elevator.pid); pid_set_gains(&mainboard.elevator.pid, 300, 10, 150); pid_set_maximums(&mainboard.elevator.pid, 0, 10000, 4095); pid_set_out_shift(&mainboard.elevator.pid, 10); pid_set_derivate_filter(&mainboard.elevator.pid, 4); /* CS */ cs_init(&mainboard.elevator.cs); cs_set_correct_filter(&mainboard.elevator.cs, pid_do_filter, &mainboard.elevator.pid); cs_set_process_in(&mainboard.elevator.cs, elevator_set, NULL); cs_set_process_out(&mainboard.elevator.cs, encoders_microb_get_value, ELEVATOR_ENC); elevator_down(); /* ---- CS wheel */ /* PID */ pid_init(&mainboard.wheel.pid); pid_set_gains(&mainboard.wheel.pid, 100, 100, 0); pid_set_maximums(&mainboard.wheel.pid, 0, 30000, 4095); pid_set_out_shift(&mainboard.wheel.pid, 5); pid_set_derivate_filter(&mainboard.wheel.pid, 4); /* CS */ cs_init(&mainboard.wheel.cs); cs_set_correct_filter(&mainboard.wheel.cs, pid_do_filter, &mainboard.wheel.pid); cs_set_process_in(&mainboard.wheel.cs, wheel_set, NULL); cs_set_process_out(&mainboard.wheel.cs, wheel_get_value, NULL); cs_set_consign(&mainboard.wheel.cs, 1000); /* set them on !! */ mainboard.angle.on = 0; mainboard.distance.on = 0; mainboard.fessor.on = 1; mainboard.elevator.on = 0; mainboard.wheel.on = 1; mainboard.flags |= DO_CS; scheduler_add_periodical_event_priority(do_cs, NULL, 5000L / SCHEDULER_UNIT, CS_PRIO); }
void htrajectory_setASpeed( htrajectory_t *htj, double speed, double acc ) { quadramp_set_1st_order_vars(htj->qramp_a, speed, speed); quadramp_set_2nd_order_vars(htj->qramp_a, acc, acc); }