Fixed PWM
Dependencies: FastAnalogIn MODSERIAL PID QEI RPCInterface Servo mbed-rtos mbed telemetry
Fork of Sequential_Timing by
Diff: main.cpp
- Revision:
- 16:3ab3c4670f4f
- Parent:
- 15:db95bb7c7f93
- Child:
- 17:bf6192a361ab
--- a/main.cpp Tue Apr 12 00:13:56 2016 +0000 +++ b/main.cpp Tue Apr 12 02:03:50 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 @@ -38,13 +38,13 @@ // ============= char comm_cmd; // Command char comm_in[8]; // Input -//Serial bt(USBTX, USBRX); // USB connection -Serial bt(PTC4, PTC3); // BlueSMiRF connection +Serial bt(USBTX, USBRX); // USB connection +//Serial bt(PTC4, PTC3); // BlueSMiRF connection void communication(void const *args); // Communications -//void kill(Arguments *input, Reply *output); -//RPCFunction rpc_kill(&kill, "kill"); +//void Kmill(Arguments *input, Reply *output); +//RPCFunction rpc_Kmill(&Kmill, "Kmill"); // ===== // Motor @@ -67,19 +67,22 @@ // ======== // Velocity // ======== -float Kp = 8.0; // Proportional factor -float Ki = 0; // Integral factor -float Kd = 0; // Derivative factor -float interval = 0.01; // Sampling interval +float Kmp = 8.0; // Proportional factor +float Kmi = 0; // Integral factor +float Kmd = 0; // Derivative factor +float motor_interval = 0.01; // Sampling interval float prev_vels[10]; float ref_v = 0.8; // Reference velocity -PID motor_ctrl(Kp, Ki, Kd, interval); // Motor controller +PID motor_ctrl(Kmp, Kmi, Kmd, interrupt_T); // Motor controller // ===== // Servo // ===== float angle = 88; // Angle -float Ks = 0.9; // Steering proportion +float Ksp = 1.0; // Steering proportion +float Ksi = 0; +float Ksd = 0; +PID servo_ctrl(Ksp, Ksi, Ksd, interrupt_T); Servo servo(PTA12); // Signal pin (PWM) // ====== @@ -111,7 +114,7 @@ // ================ -void killswitch(MODSERIAL_IRQ_INFO *q) { +void Kmillswitch(MODSERIAL_IRQ_INFO *q) { MODSERIAL *serial = q->serial; if (serial->rxGetLastChar() == 'k') { e_stop = true; @@ -130,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(); @@ -147,10 +150,10 @@ 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(" [2] Change Kmp\r\n"); + bt.printf(" [3] Change Kmi\r\n"); + bt.printf(" [4] Change Kmd\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"); @@ -160,52 +163,52 @@ 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); + bt.printf("Duty cycle: %f, Pulse count: %d, Velocity: %f, Kmp: %f, Kmi: %f, Kmd: %f\r\n", motor_duty, curr_pulses, velocity, Kmp, Kmi, Kmd); 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); + 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"); - Kp = atof(comm_in); - motor_ctrl.setTunings(Kp, Ki, Kd); + Kmp = atof(comm_in); + motor_ctrl.setTunings(Kmp, Kmi, Kmd); comm_cmd = 'q'; break; case 3: - bt.printf("Current: %f, New (8 digits): ", Ki); + 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"); - Ki = atof(comm_in); - motor_ctrl.setTunings(Kp, Ki, Kd); + Kmi = atof(comm_in); + motor_ctrl.setTunings(Kmp, Kmi, Kmd); comm_cmd = 'q'; break; case 4: - bt.printf("Current: %f, New (8 digits): ", Kd); + 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"); - Kd = atof(comm_in); - motor_ctrl.setTunings(Kp, Ki, Kd); + Kmd = atof(comm_in); + motor_ctrl.setTunings(Kmp, Kmi, Kmd); comm_cmd = 'q'; break; case 5: - bt.printf("Current: %f, New (8 digits): ", Ks); + 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"); - Ks = atof(comm_in); + Ksp = atof(comm_in); comm_cmd = 'q'; break; case 6: @@ -288,10 +291,10 @@ // Detect peak edges j = 0; - for (int i = 0; i < 107 && j < 5; i++) { + for (int i = 0; i < 108 && j < 5; i++) { if (img[i] > 45000) { left[j] = i; - while (img[i] > 45000) { + while (img[i] > 45000 && i < 108) { i = i + 1; } right[j] = i; @@ -301,12 +304,12 @@ // Calculate peak centers for (int i = 0; i < j; i++) { - centers[i] = (left[i] + right[i] + 10) / 2; + centers[i] = (left[i] + right[i] + 20) / 2; } // 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); + scores[i] = 8 / (right[i] - left[i]) + img[centers[i]] / 65536 + 5 / abs(centers[i] - prev_center); } // Choose most likely center @@ -321,13 +324,17 @@ tele_center = center; // Set servo angle - angle = 88 + (55 - center) * Ks; - if (angle > 113) { - angle = 113; - } - if (angle < 63) { - angle = 63; - } + // 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 @@ -425,6 +432,13 @@ servo.calibrate(0.001, 45.0); servo = angle / 180.0; + // Initialize servo controller + servo_ctrl.setInputLimits(10, 117); + servo_ctrl.setOutputLimits(-25, 25); + servo_ctrl.setSetPoint(63.5); + servo_ctrl.setBias(0.0); + servo_ctrl.setMode(1); + // Initialize communications thread Thread communication_thread(communication);