One big fixed period control loop
Dependencies: FastAnalogIn MODSERIAL PID QEI RPCInterface Servo mbed-rtos mbed telemetry PinDetect
Fork of Everything by
Diff: main.cpp
- Revision:
- 15:db95bb7c7f93
- Parent:
- 14:a0614f48e6ef
- Child:
- 16:3ab3c4670f4f
diff -r a0614f48e6ef -r db95bb7c7f93 main.cpp --- a/main.cpp Mon Apr 11 23:56:23 2016 +0000 +++ b/main.cpp Tue Apr 12 00:13:56 2016 +0000 @@ -117,7 +117,7 @@ e_stop = true; motor = 1.0; } - if (serial->rxGetLastChar() == '=') { + if (serial->rxGetLastChar() == '+') { ref_v = ref_v + 0.05; motor_ctrl.setSetPoint(ref_v); } @@ -153,7 +153,9 @@ bt.printf(" [5] Change Ks\r\n"); bt.printf(" [6] Change reference velocity\r\n"); bt.printf(" [7] EMERGENCY STOP\r\n"); - bt.printf(" [8] Timing\r\n"); +// bt.printf(" [8] Timing\r\n"); + bt.printf(" [8] duty += 0.05\r\n"); + bt.printf(" [9] duty -= 0.05\r\n"); comm_cmd = bt.getc(); while (comm_cmd != 'q') { switch(atoi(&comm_cmd)){ @@ -218,12 +220,25 @@ comm_cmd = 'q'; break; case 7: - e_stop = true; +// e_stop = true; + motor = 1.0; bt.printf("STOPPED\r\n"); comm_cmd = 'q'; break; +// case 8: +// bt.printf("Exposure: %dus, Camera Read: %dus, Steering: %dus, Velocity: %dus, Total: %dus\r\n", t_int, t_cam-t_int, t_steer, t_vel, t_cam+t_steer+t_vel); +// break; case 8: - bt.printf("Exposure: %dus, Camera Read: %dus, Steering: %dus, Velocity: %dus, Total: %dus\r\n", t_int, t_cam-t_int, t_steer, t_vel, t_cam+t_steer+t_vel); + motor_duty = motor_duty + 0.05; + motor = 1.0 - motor_duty; + bt.printf("%f\r\n", motor_duty); + comm_cmd = 'q'; + break; + case 9: + motor_duty = motor_duty - 0.05; + motor = 1.0 - motor_duty; + bt.printf("%f\r\n", motor_duty); + comm_cmd = 'q'; break; } if (bt.readable()) { @@ -338,44 +353,44 @@ // t_steer = t.read_us(); - // Velocity control - // t.reset(); - if (!e_stop) { - curr_pulses = qei.getPulses(); - //for (int i = 0; i < 9; i++) { -// prev_vels[i] = prev_vels[i+1]; -// } -// prev_vels[9] = (curr_pulses - prev_pulses) / t.read() / ppr * c; -// t.reset(); -// if (prev_vels[9] < 0) { -// prev_vels[9] = 0; -// } -// if (prev_vels[0] < 0) { -// velocity = prev_vels[9]; -// } else { +// // Velocity control +// // t.reset(); +// if (!e_stop) { +// curr_pulses = qei.getPulses(); +// //for (int i = 0; i < 9; i++) { +//// prev_vels[i] = prev_vels[i+1]; +//// } +//// prev_vels[9] = (curr_pulses - prev_pulses) / t.read() / ppr * c; +//// t.reset(); +//// if (prev_vels[9] < 0) { +//// prev_vels[9] = 0; +//// } +//// if (prev_vels[0] < 0) { +//// velocity = prev_vels[9]; +//// } else { +//// velocity = 0; +//// for (int i = 0; i < 10; i++) { +//// velocity = velocity + prev_vels[i]; +//// velocity = velocity / 10; +//// } +//// } +// velocity = (curr_pulses - prev_pulses) / interrupt_T / ppr * c; +// if (velocity < 0) { // velocity = 0; -// for (int i = 0; i < 10; i++) { -// velocity = velocity + prev_vels[i]; -// velocity = velocity / 10; -// } // } - velocity = (curr_pulses - prev_pulses) / interrupt_T / ppr * c; - if (velocity < 0) { - velocity = 0; - } -// velocity = (curr_pulses - prev_pulses) / t.read() / ppr * c; - t.reset(); - tele_velocity = velocity; - prev_pulses = curr_pulses; - motor_ctrl.setProcessValue(velocity); - motor_duty = motor_ctrl.compute(); - motor = 1.0 - motor_duty; - tele_pwm = motor_duty; - } else { - motor = 1.0; - } - // t_vel = t.read_us(); - ctrl_flag = false; +//// velocity = (curr_pulses - prev_pulses) / t.read() / ppr * c; +// t.reset(); +// tele_velocity = velocity; +// prev_pulses = curr_pulses; +// motor_ctrl.setProcessValue(velocity); +// motor_duty = motor_ctrl.compute(); +// motor = 1.0 - motor_duty; +// tele_pwm = motor_duty; +// } else { +// motor = 1.0; +// } +// // t_vel = t.read_us(); +// ctrl_flag = false; } void set_control_flag() { @@ -413,12 +428,12 @@ // Initialize communications thread Thread communication_thread(communication); -// control_interrupt.attach(control, interrupt_T); - control_interrupt.attach(set_control_flag, interrupt_T); + control_interrupt.attach(control, interrupt_T); +// control_interrupt.attach(set_control_flag, interrupt_T); while (true) { - if (ctrl_flag) { - control(); - } +// if (ctrl_flag) { +// control(); +// } } } \ No newline at end of file