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:
- 13:d6e167698517
- Parent:
- 12:54e7d8ff3a74
- Child:
- 14:a0614f48e6ef
--- a/main.cpp Fri Apr 08 21:40:35 2016 +0000 +++ b/main.cpp Fri Apr 08 23:43:12 2016 +0000 @@ -42,6 +42,9 @@ void communication(void const *args); // Communications +//void kill(Arguments *input, Reply *output); +//RPCFunction rpc_kill(&kill, "kill"); + // ===== // Motor // ===== @@ -67,7 +70,7 @@ float Ki = 0; // Integral factor float Kd = 0; // Derivative factor float interval = 0.01; // Sampling interval -float ref_v = 0.8; // Reference velocity +float ref_v = 0.5; // Reference velocity PID motor_ctrl(Kp, Ki, Kd, interval); // Motor controller // ===== @@ -81,7 +84,7 @@ // Camera // ====== int t_int = 10000; // Exposure time -const int T_INT_MAX = interrupt_T * 1000000 - 10000; // Maximum exposure time +const int T_INT_MAX = interrupt_T * 1000000 - 1000; // Maximum exposure time const int T_INT_MIN = 35; // Minimum exposure time int img[108] = {0}; // Image data DigitalOut clk(PTD5); // Clock pin @@ -105,15 +108,40 @@ // Functions // ================ + +void killswitch(MODSERIAL_IRQ_INFO *q) { + MODSERIAL *serial = q->serial; + if (serial->rxGetLastChar() == 'k') { + e_stop = true; + motor = 1.0; + } + if (serial->rxGetLastChar() == '+') { + ref_v = ref_v + 0.2; + motor_ctrl.setSetPoint(ref_v); + } + if (serial->rxGetLastChar() == '-') { + ref_v = ref_v - 0.2; + motor_ctrl.setSetPoint(ref_v); + } +} + // Communications void communication(void const *args) { telemetry_serial.baud(115200); + telemetry_serial.attach(&killswitch, MODSERIAL::RxIrq); telemetry_obj.transmit_header(); while (true) { tele_time_ms = t_tele.read_ms(); telemetry_obj.do_io(); } } + +//void kill(Arguments *input, Reply *output) { +// output->putData("E_STOP\r\n"); +// motor = 1.0; +// e_stop = true; +//} + //void communication(void const *args) { // // Initialize bluetooth // bt.baud(115200); @@ -213,44 +241,65 @@ t.reset(); // Dummy read - clk = 0; - wait_us(1); - si = 1; - wait_us(1); - clk = 1; - wait_us(1); - si = 0; +// clk = 0; + PTD->PCOR = (0x01 << 5); +// j = 0; +// wait_us(1); +// si = 1; + PTD->PSOR = (0x01); +// j= 0; +// wait_us(1); +// clk = 1; + PTD->PSOR = (0x01 << 5); +// j = 0; +// wait_us(1); +// si = 0; + PTD->PCOR = (0x01); +// j = 0; for (int i = 0; i < 128; i++) { - wait_us(1); - clk = 0; - wait_us(1); - clk = 1; +// wait_us(1); + PTD->PCOR = (0x01 << 5); +// j = 0; +// wait_us(1); + PTD->PSOR = (0x01 << 5); +// j = 0; } // Expose wait_us(t_int); // Read camera - clk = 0; - wait_us(1); - si = 1; - wait_us(1); - clk = 1; - wait_us(1); - si = 0; - wait_us(1); +// clk = 0; + PTD->PCOR = (0x01 << 5); +// j = 0; +// wait_us(1); +// si = 1; + PTD->PSOR = (0x01); +// j = 0; +// wait_us(1); +// clk = 1; + PTD->PSOR = (0x01 << 5); +// j = 0; +// wait_us(1); +// si = 0; + PTD->PCOR = (0x01); +// j = 0; +// wait_us(1); for (int i = 0; i < 128; i++) { - clk = 0; +// clk = 0; + PTD->PCOR = (0x01 << 5); +// j = 0; if (i > 9 && i < 118) { img[i-10] = ao.read_u16(); tele_linescan[i-10] = img[i-10]; - } else { - wait_us(1); } - clk = 1; - wait_us(1); + //} else { +// wait_us(1); +// } + PTD->PSOR = (0x01 << 5); +// j = 0; } t_cam = t.read_us(); @@ -338,6 +387,8 @@ motor_duty = motor_ctrl.compute(); motor = 1.0 - motor_duty; tele_pwm = motor_duty; + } else { + motor = 1.0; } t_vel = t.read_us(); }