![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Fixed PWM
Dependencies: FastAnalogIn MODSERIAL PID QEI RPCInterface Servo mbed-rtos mbed telemetry
Fork of Sequential_Timing by
Revision 18:2b7db50fec4c, committed 2016-04-14
- Comitter:
- vsutardja
- Date:
- Thu Apr 14 01:44:28 2016 +0000
- Parent:
- 17:bf6192a361ab
- Commit message:
- fixed pwm, pid steering
Changed in this revision
PinDetect.lib | Show diff for this revision Revisions of this file |
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/PinDetect.lib Thu Apr 14 00:45:14 2016 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -http://mbed.org/users/AjK/code/PinDetect/#cb3afc45028b
--- a/main.cpp Thu Apr 14 00:45:14 2016 +0000 +++ b/main.cpp Thu Apr 14 01:44:28 2016 +0000 @@ -11,10 +11,8 @@ // ========= // Telemetry // ========= -//DigitalOut test(PTD1); - -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 @@ -28,11 +26,9 @@ Timer t; Timer t_tele; Ticker control_interrupt; -int t_cam1 = 0; -int t_cam2 = 0; +int t_cam = 0; int t_steer = 0; int t_vel = 0; -int t_down = 0; float interrupt_T = 0.015; bool ctrl_flag = false; @@ -41,12 +37,15 @@ // Communication // ============= char comm_cmd; // Command -char comm_in[5]; // Input -Serial bt(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 +//void Kmill(Arguments *input, Reply *output); +//RPCFunction rpc_Kmill(&Kmill, "Kmill"); + // ===== // Motor // ===== @@ -61,7 +60,6 @@ const int ppr = 389; // Pulses per revolution const float c = 0.20106; // Wheel circumference int prev_pulses = 0; // Previous pulse count -int prev_pulse_counts[5] = {0}; int curr_pulses = 0; // Current pulse count float velocity = 0; // Velocity QEI qei(PTD3, PTD2, NC, ppr, QEI::X4_ENCODING); // Quadrature encoder @@ -69,19 +67,19 @@ // ======== // Velocity // ======== -float Kmp = 36.0; // Proportional factor -float Kmi = 1; // Integral factor -float Kmd = 0; // Derivative factor +float Kmp = 8.0; // Proportional factor +float Kmi = 0; // Integral factor +float Kmd = 0; // Derivative factor float interval = 0.01; // Sampling interval -float ref_v = 0.5; // Reference velocity -int vel_count = 1; -PID motor_ctrl(Kmp, Kmi, Kmd, interrupt_T); // Motor controller +float prev_vels[10]; +float ref_v = 0.8; // Reference velocity +PID motor_ctrl(Kmp, Kmi, Kmd, interrupt_T); // Motor controller // ===== // Servo // ===== float angle = 88; // Angle -float Ksp = 1; // Steering proportion +float Ksp = 0.9; // Steering proportion float Ksi = 0; float Ksd = 0; PID servo_ctrl(Ksp, Ksi, Ksd, interrupt_T); @@ -91,7 +89,7 @@ // Camera // ====== int t_int = 10000; // Exposure time -const int T_INT_MAX = interrupt_T * 1000000 - 2000; // 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 @@ -115,18 +113,19 @@ // Functions // ================ -void killswitch(MODSERIAL_IRQ_INFO *q) { + +void Kmillswitch(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; + 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); } } @@ -134,7 +133,7 @@ // Communications //void communication(void const *args) { // telemetry_serial.baud(115200); -// telemetry_serial.attach(&killswitch, MODSERIAL::RxIrq); +// telemetry_serial.attach(&Kmillswitch, MODSERIAL::RxIrq); // telemetry_obj.transmit_header(); // while (true) { // tele_time_ms = t_tele.read_ms(); @@ -151,13 +150,18 @@ bt.printf("Available diagnostics:\r\n"); bt.printf(" [0] Velocity\r\n"); bt.printf(" [1] Steering\r\n"); - bt.printf(" [2] Change Kmp\r\n"); - bt.printf(" [3] Change Kmi\r\n"); - bt.printf(" [4] Change Kmd\r\n"); +// bt.printf(" [2] Change Kmp\r\n"); +// bt.printf(" [3] Change Kmi\r\n"); +// bt.printf(" [4] Change Kmd\r\n"); + bt.printf(" [2] Change Ksp\r\n"); + bt.printf(" [3] Change Ksi\r\n"); + bt.printf(" [4] Change Ksd\r\n"); bt.printf(" [5] Change Ksp\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)){ @@ -168,41 +172,74 @@ 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): ", Kmp); - for (int i = 0; i < 5; i++) { + bt.printf("Current: %f, New (8 digits): ", Ksp); + for (int i = 0; i < 8; i++) { comm_in[i] = bt.getc(); bt.putc(comm_in[i]); } bt.printf("\r\n"); - Kmp = atof(comm_in); - motor_ctrl.setTunings(Kmp, Kmi, Kmd); + Ksp = atof(comm_in); + servo_ctrl.setTunings(Ksp, Ksi, Ksd); comm_cmd = 'q'; break; case 3: - bt.printf("Current: %f, New (5 digits): ", Kmi); - for (int i = 0; i < 5; i++) { + bt.printf("Current: %f, New (8 digits): ", Ksi); + for (int i = 0; i < 8; i++) { + comm_in[i] = bt.getc(); + bt.putc(comm_in[i]); + } + bt.printf("\r\n"); + Ksi = atof(comm_in); + motor_ctrl.setTunings(Ksp, Ksi, Ksd); + comm_cmd = 'q'; + break; + case 4: + bt.printf("Current: %f, New (8 digits): ", Ksd); + for (int i = 0; i < 8; i++) { comm_in[i] = bt.getc(); bt.putc(comm_in[i]); } bt.printf("\r\n"); - Kmi = atof(comm_in); - motor_ctrl.setTunings(Kmp, Kmi, Kmd); + Ksd = atof(comm_in); + motor_ctrl.setTunings(Ksp, Ksi, Ksd); comm_cmd = 'q'; break; - case 4: - bt.printf("Current: %f, New (5 digits): ", Kmd); - for (int i = 0; i < 5; i++) { - comm_in[i] = bt.getc(); - bt.putc(comm_in[i]); - } - bt.printf("\r\n"); - Kmd = atof(comm_in); - motor_ctrl.setTunings(Kmp, Kmi, Kmd); - comm_cmd = 'q'; - break; +// case 2: +// bt.printf("Current: %f, New (8 digits): ", Kmp); +// for (int i = 0; i < 8; i++) { +// comm_in[i] = bt.getc(); +// bt.putc(comm_in[i]); +// } +// bt.printf("\r\n"); +// Kmp = atof(comm_in); +// motor_ctrl.setTunings(Kmp, Kmi, Kmd); +// comm_cmd = 'q'; +// break; +// case 3: +// bt.printf("Current: %f, New (8 digits): ", Kmi); +// for (int i = 0; i < 8; i++) { +// comm_in[i] = bt.getc(); +// bt.putc(comm_in[i]); +// } +// bt.printf("\r\n"); +// Kmi = atof(comm_in); +// motor_ctrl.setTunings(Kmp, Kmi, Kmd); +// comm_cmd = 'q'; +// break; +// case 4: +// bt.printf("Current: %f, New (8 digits): ", Kmd); +// for (int i = 0; i < 8; i++) { +// comm_in[i] = bt.getc(); +// bt.putc(comm_in[i]); +// } +// bt.printf("\r\n"); +// Kmd = atof(comm_in); +// motor_ctrl.setTunings(Kmp, Kmi, Kmd); +// comm_cmd = 'q'; +// break; case 5: - bt.printf("Current: %f, New (5 digits): ", Ksp); - for (int i = 0; i < 5; i++) { + bt.printf("Current: %f, New (8 digits): ", Ksp); + for (int i = 0; i < 8; i++) { comm_in[i] = bt.getc(); bt.putc(comm_in[i]); } @@ -211,8 +248,8 @@ comm_cmd = 'q'; break; case 6: - bt.printf("Current: %f, New (5 digits): ", ref_v); - for (int i = 0; i < 5; i++) { + 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]); } @@ -222,12 +259,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("Read 1: %dus, Exposure: %dus, Read 2: %dus, Steering: %dus, Velocity: %dus, Down: %dus, Total: %dus\r\n", t_cam1, t_int, t_cam2, t_steer, t_vel, t_down, t_cam1+t_int+t_cam2+t_steer+t_vel+t_down); + 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()) { @@ -239,8 +289,7 @@ void control() { // Image capture - t_down = t.read_us(); - t.reset(); + // t.reset(); // Dummy read PTD->PCOR = (0x01 << 5); @@ -252,7 +301,6 @@ PTD->PCOR = (0x01 << 5); PTD->PSOR = (0x01 << 5); } - t_cam1 = t.read_us(); // Expose wait_us(t_int); @@ -263,7 +311,6 @@ PTD->PSOR = (0x01 << 5); PTD->PCOR = (0x01); - t.reset(); for (int i = 0; i < 128; i++) { PTD->PCOR = (0x01 << 5); if (i > 9 && i < 118) { @@ -273,66 +320,57 @@ PTD->PSOR = (0x01 << 5); } - t_cam2 = t.read_us(); + // t_cam = t.read_us(); // Steering control - t.reset(); + // t.reset(); // Detect peak edges j = 0; - for (int i = 0; i < 108 && j < 5; i++) { - if (img[i] > max * 0.65) { + for (int i = 0; i < 107 && j < 5; i++) { + if (img[i] > 45000) { left[j] = i; - while (img[i] > max * 0.65) { - if (i < 107) { - i = i + 1; - } else { - j = j - 1; - break; - } + while (img[i] > 45000) { + i = i + 1; } - if (i < 107) { - right[j] = i; - } + right[j] = i; j = j + 1; } } + // Calculate peak centers + for (int i = 0; i < j; i++) { + centers[i] = (left[i] + right[i] + 10) / 2; + } - if (j > 0) { - // Calculate peak centers - for (int i = 0; i < j; i++) { - centers[i] = (left[i] + right[i] + 20) / 2; - } - - // Assign scores - for (int i = 0; i < j; i++) { - scores[i] = 4 / (right[i] - left[i]) + img[centers[i]] / 65536 + 16 / abs(centers[i] - prev_center); + // Assign scores + for (int i = 0; i < j; i++) { + scores[i] = 10 / (right[i] - left[i]) + img[centers[i]] / 65536 + 10 / abs(centers[i] - prev_center); + } + + // Choose most likely center + best_score_idx = 0; + for (int i = 0; i < j; i++) { + if (scores[i] > scores[best_score_idx]) { + best_score_idx = i; } - - // Choose most likely center - best_score_idx = 0; - for (int i = 0; i < j; i++) { - if (scores[i] > scores[best_score_idx]) { - best_score_idx = i; - } - } - prev_center = center; - center = centers[best_score_idx]; - tele_center = center; - - angle = 88 + (64 - center) * 0.9; - if (angle > 113) { - angle = 113; - } - if (angle < 63) { - angle = 63; - } - // servo_ctrl.setProcessValue(center); - // angle = 88 + servo_ctrl.compute(); - // servo = floor(angle / 180 * 100 + 0.5) / 100; - servo = angle / 180; } + prev_center = center; + center = centers[best_score_idx]; + tele_center = center; + + // Set servo angle + //angle = 88 + (55 - center) * Ksp; +// if (angle > 113) { +// angle = 113; +// } +// if (angle < 63) { +// angle = 63; +// } +// servo = angle / 180; + servo_ctrl.setProcessValue(center); + angle = 88 + servo_ctrl.compute(); + servo = angle / 180; // AGC max = -1; @@ -355,43 +393,49 @@ } tele_exposure = t_int; - t_steer = t.read_us(); - - // wait_us(8000 - t.read_us()); - - + // t_steer = t.read_us(); - // Velocity control - t.reset(); - if (!e_stop) { - curr_pulses = qei.getPulses(); - if (vel_count < 6) { - velocity = curr_pulses / interrupt_T / vel_count / ppr * c; - prev_pulse_counts[vel_count - 1] = curr_pulses; - vel_count = vel_count + 1; - } else { - velocity = (curr_pulses - prev_pulse_counts[0]) / interrupt_T / 5 / ppr * c; - for (int i = 0; i < 4; i++) { - prev_pulse_counts[i] = prev_pulse_counts[i+1]; - } - prev_pulse_counts[4] = curr_pulses; - } - tele_velocity = velocity; - 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(); - t.reset(); - ctrl_flag = false; -// test = 0; +// // 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; +// } +//// 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_ctrl_flag() { -// test = 1; +void set_control_flag() { ctrl_flag = true; } @@ -404,13 +448,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.0, 0.5); + motor_ctrl.setOutputLimits(0.0, 0.75); motor_ctrl.setSetPoint(ref_v); motor_ctrl.setBias(0.0); motor_ctrl.setMode(1); @@ -419,23 +467,21 @@ servo.calibrate(0.001, 45.0); servo = angle / 180.0; - // Initialize servo controller - servo_ctrl.setInputLimits(10, 117); + servo_ctrl.setInputLimits(10, 107); servo_ctrl.setOutputLimits(-25, 25); servo_ctrl.setSetPoint(63.5); - servo_ctrl.setBias(0); + servo_ctrl.setBias(0.0); servo_ctrl.setMode(1); // Initialize communications thread -// Thread communication_thread(communication); - - // control_interrupt.attach(control, interrupt_T); - // Thread::wait(osWaitForever); + Thread communication_thread(communication); + + control_interrupt.attach(control, interrupt_T); +// control_interrupt.attach(set_control_flag, interrupt_T); - control_interrupt.attach(set_ctrl_flag, interrupt_T); while (true) { - if (ctrl_flag) { - control(); - } +// if (ctrl_flag) { +// control(); +// } } } \ No newline at end of file