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:
- 14:a0614f48e6ef
- Parent:
- 13:d6e167698517
- Child:
- 15:db95bb7c7f93
--- a/main.cpp Fri Apr 08 23:43:12 2016 +0000 +++ b/main.cpp Mon Apr 11 23:56:23 2016 +0000 @@ -11,8 +11,8 @@ // ========= // Telemetry // ========= -MODSERIAL telemetry_serial(PTC4, PTC3); // TX, RX -//MODSERIAL telemetry_serial(USBTX, USBRX); +//MODSERIAL telemetry_serial(PTC4, PTC3); // TX, RX +MODSERIAL telemetry_serial(USBTX, USBRX); telemetry::MbedHal telemetry_hal(telemetry_serial); // Hardware Abstraction Layer telemetry::Telemetry telemetry_obj(telemetry_hal); // Telemetry @@ -30,15 +30,16 @@ int t_steer = 0; int t_vel = 0; -float interrupt_T = 0.025; +float interrupt_T = 0.015; +bool ctrl_flag = false; // ============= // Communication // ============= char comm_cmd; // Command -char comm_in[5]; // Input -//Serial pc(USBTX, USBRX); // USB connection -//Serial bt(PTC4, PTC3); // BlueSMiRF connection +char comm_in[8]; // Input +//Serial bt(USBTX, USBRX); // USB connection +Serial bt(PTC4, PTC3); // BlueSMiRF connection void communication(void const *args); // Communications @@ -66,11 +67,12 @@ // ======== // Velocity // ======== -float Kp = 6.0; // Proportional factor +float Kp = 8.0; // Proportional factor float Ki = 0; // Integral factor float Kd = 0; // Derivative factor float interval = 0.01; // Sampling interval -float ref_v = 0.5; // Reference velocity +float prev_vels[10]; +float ref_v = 0.8; // Reference velocity PID motor_ctrl(Kp, Ki, Kd, interval); // Motor controller // ===== @@ -115,197 +117,159 @@ e_stop = true; motor = 1.0; } - if (serial->rxGetLastChar() == '+') { - ref_v = ref_v + 0.2; + if (serial->rxGetLastChar() == '=') { + ref_v = ref_v + 0.05; motor_ctrl.setSetPoint(ref_v); } if (serial->rxGetLastChar() == '-') { - ref_v = ref_v - 0.2; + ref_v = ref_v - 0.05; 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); -// +// telemetry_serial.baud(115200); +// telemetry_serial.attach(&killswitch, MODSERIAL::RxIrq); +// telemetry_obj.transmit_header(); // while (true) { -// bt.printf("\r\nPress q to return to this prompt.\r\n"); -// bt.printf("Available diagnostics:\r\n"); -// bt.printf(" [0] Velocity\r\n"); -// bt.printf(" [1] Steering\r\n"); -// bt.printf(" [2] Change Kp\r\n"); -// bt.printf(" [3] Change Ki\r\n"); -// bt.printf(" [4] Change Kd\r\n"); -// 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"); -// comm_cmd = bt.getc(); -// while (comm_cmd != 'q') { -// switch(atoi(&comm_cmd)){ -// case 0: -// bt.printf("Duty cycle: %f, Pulse count: %d, Velocity: %f, Kp: %f, Ki: %f, Kd: %f\r\n", motor_duty, curr_pulses, velocity, Kp, Ki, Kd); -// break; -// case 1: -// bt.printf("Servo angle: %f, Track center: %d, t_int: %d\r\n", angle, center, t_int); -// break; -// case 2: -// bt.printf("Current: %f, New (5 digits): ", Kp); -// for (int i = 0; i < 5; i++) { -// comm_in[i] = bt.getc(); -// bt.putc(comm_in[i]); -// } -// bt.printf("\r\n"); -// Kp = atof(comm_in); -// motor_ctrl.setTunings(Kp, Ki, Kd); -// comm_cmd = 'q'; -// break; -// case 3: -// bt.printf("Current: %f, New (5 digits): ", Ki); -// for (int i = 0; i < 5; i++) { -// comm_in[i] = bt.getc(); -// bt.putc(comm_in[i]); -// } -// bt.printf("\r\n"); -// Ki = atof(comm_in); -// motor_ctrl.setTunings(Kp, Ki, Kd); -// comm_cmd = 'q'; -// break; -// case 4: -// bt.printf("Current: %f, New (5 digits): ", Kd); -// for (int i = 0; i < 5; i++) { -// comm_in[i] = bt.getc(); -// bt.putc(comm_in[i]); -// } -// bt.printf("\r\n"); -// Kd = atof(comm_in); -// motor_ctrl.setTunings(Kp, Ki, Kd); -// comm_cmd = 'q'; -// break; -// case 5: -// bt.printf("Current: %f, New (5 digits): ", Ks); -// for (int i = 0; i < 5; i++) { -// comm_in[i] = bt.getc(); -// bt.putc(comm_in[i]); -// } -// bt.printf("\r\n"); -// Ks = atof(comm_in); -// comm_cmd = 'q'; -// break; -// case 6: -// bt.printf("Current: %f, New (5 digits): ", ref_v); -// for (int i = 0; i < 5; i++) { -// comm_in[i] = bt.getc(); -// bt.putc(comm_in[i]); -// } -// bt.printf("\r\n"); -// ref_v = atof(comm_in); -// motor_ctrl.setSetPoint(ref_v); -// comm_cmd = 'q'; -// break; -// case 7: -// e_stop = true; -// bt.printf("STOPPED\r\n"); -// 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; -// } -// if (bt.readable()) { -// comm_cmd = bt.getc(); -// } -// } +// tele_time_ms = t_tele.read_ms(); +// telemetry_obj.do_io(); // } //} +void communication(void const *args) { + // Initialize bluetooth + bt.baud(115200); + + while (true) { + bt.printf("\r\nPress q to return to this prompt.\r\n"); + bt.printf("Available diagnostics:\r\n"); + bt.printf(" [0] Velocity\r\n"); + bt.printf(" [1] Steering\r\n"); + bt.printf(" [2] Change Kp\r\n"); + bt.printf(" [3] Change Ki\r\n"); + bt.printf(" [4] Change Kd\r\n"); + 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"); + comm_cmd = bt.getc(); + while (comm_cmd != 'q') { + switch(atoi(&comm_cmd)){ + case 0: + bt.printf("Duty cycle: %f, Pulse count: %d, Velocity: %f, Kp: %f, Ki: %f, Kd: %f\r\n", motor_duty, curr_pulses, velocity, Kp, Ki, Kd); + break; + case 1: + bt.printf("Servo angle: %f, Track center: %d, t_int: %d\r\n", angle, center, t_int); + break; + case 2: + bt.printf("Current: %f, New (8 digits): ", Kp); + for (int i = 0; i < 8; i++) { + comm_in[i] = bt.getc(); + bt.putc(comm_in[i]); + } + bt.printf("\r\n"); + Kp = atof(comm_in); + motor_ctrl.setTunings(Kp, Ki, Kd); + comm_cmd = 'q'; + break; + case 3: + bt.printf("Current: %f, New (8 digits): ", Ki); + for (int i = 0; i < 8; i++) { + comm_in[i] = bt.getc(); + bt.putc(comm_in[i]); + } + bt.printf("\r\n"); + Ki = atof(comm_in); + motor_ctrl.setTunings(Kp, Ki, Kd); + comm_cmd = 'q'; + break; + case 4: + bt.printf("Current: %f, New (8 digits): ", Kd); + for (int i = 0; i < 8; i++) { + comm_in[i] = bt.getc(); + bt.putc(comm_in[i]); + } + bt.printf("\r\n"); + Kd = atof(comm_in); + motor_ctrl.setTunings(Kp, Ki, Kd); + comm_cmd = 'q'; + break; + case 5: + bt.printf("Current: %f, New (8 digits): ", Ks); + for (int i = 0; i < 8; i++) { + comm_in[i] = bt.getc(); + bt.putc(comm_in[i]); + } + bt.printf("\r\n"); + Ks = atof(comm_in); + comm_cmd = 'q'; + break; + case 6: + bt.printf("Current: %f, New (8 digits): ", ref_v); + for (int i = 0; i < 8; i++) { + comm_in[i] = bt.getc(); + bt.putc(comm_in[i]); + } + bt.printf("\r\n"); + ref_v = atof(comm_in); + motor_ctrl.setSetPoint(ref_v); + comm_cmd = 'q'; + break; + case 7: + e_stop = true; + 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; + } + if (bt.readable()) { + comm_cmd = bt.getc(); + } + } + } +} + void control() { // Image capture - t.reset(); + // t.reset(); // Dummy read -// 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); PTD->PCOR = (0x01 << 5); -// j = 0; -// wait_us(1); PTD->PSOR = (0x01 << 5); -// j = 0; } // Expose wait_us(t_int); // Read camera -// 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; 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); -// } PTD->PSOR = (0x01 << 5); -// j = 0; } - t_cam = t.read_us(); + // t_cam = t.read_us(); // Steering control - t.reset(); + // t.reset(); // Detect peak edges j = 0; @@ -372,15 +336,35 @@ } tele_exposure = t_int; - t_steer = t.read_us(); - - // wait_us(8000 - t.read_us()); + // t_steer = t.read_us(); // Velocity control - t.reset(); + // 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; + } +// velocity = (curr_pulses - prev_pulses) / t.read() / ppr * c; + t.reset(); tele_velocity = velocity; prev_pulses = curr_pulses; motor_ctrl.setProcessValue(velocity); @@ -390,7 +374,12 @@ } else { motor = 1.0; } - t_vel = t.read_us(); + // t_vel = t.read_us(); + ctrl_flag = false; +} + +void set_control_flag() { + ctrl_flag = true; } // ==== @@ -402,13 +391,17 @@ tele_center.set_limits(0, 128); tele_pwm.set_limits(0.0, 1.0); + for (int i = 0; i < 10; i++) { + prev_vels[i] = -1; + } + // Initialize motor motor.period_us(motor_T); motor = 1.0 - motor_duty; // Initialize motor controller motor_ctrl.setInputLimits(0.0, 10.0); - motor_ctrl.setOutputLimits(0.01, 0.5); + motor_ctrl.setOutputLimits(0.0, 0.75); motor_ctrl.setSetPoint(ref_v); motor_ctrl.setBias(0.0); motor_ctrl.setMode(1); @@ -419,8 +412,13 @@ // Initialize communications thread Thread communication_thread(communication); - - control_interrupt.attach(control, interrupt_T); + +// control_interrupt.attach(control, interrupt_T); + control_interrupt.attach(set_control_flag, interrupt_T); - while (true); + while (true) { + if (ctrl_flag) { + control(); + } + } } \ No newline at end of file