Fixed PWM
Dependencies: FastAnalogIn MODSERIAL PID QEI RPCInterface Servo mbed-rtos mbed telemetry
Fork of Sequential_Timing by
main.cpp
- Committer:
- vsutardja
- Date:
- 2016-04-12
- Revision:
- 15:db95bb7c7f93
- Parent:
- 14:a0614f48e6ef
- Child:
- 16:3ab3c4670f4f
File content as of revision 15:db95bb7c7f93:
#include "FastAnalogIn.h" #include "mbed.h" #include "PID.h" #include "QEI.h" #include "rtos.h" #include "SerialRPCInterface.h" #include "Servo.h" #include "telemetry.h" // ========= // Telemetry // ========= //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 telemetry::Numeric<uint32_t> tele_time_ms(telemetry_obj, "time", "Time", "ms", 0); telemetry::NumericArray<uint16_t, 108> tele_linescan(telemetry_obj, "linescan", "Linescan", "ADC", 0); telemetry::Numeric<uint32_t> tele_exposure(telemetry_obj, "exposure", "Exposure", "us", 0); telemetry::Numeric<float> tele_velocity(telemetry_obj, "tele_vel", "Velocity", "who knows", 0); telemetry::Numeric<float> tele_pwm(telemetry_obj, "pwm", "PWM", "", 0); telemetry::Numeric<uint8_t> tele_center(telemetry_obj, "tele_center", "Center", "px", 0); Timer t; Timer t_tele; Ticker control_interrupt; int t_cam = 0; int t_steer = 0; int t_vel = 0; float interrupt_T = 0.015; bool ctrl_flag = false; // ============= // Communication // ============= char comm_cmd; // Command char comm_in[8]; // Input //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"); // ===== // Motor // ===== const int motor_T = 25000; // Frequency float motor_duty = 0.0; // Duty cycle bool e_stop = false; // Emergency stop PwmOut motor(PTA4); // Enable pin (PWM) // ======= // Encoder // ======= const int ppr = 389; // Pulses per revolution const float c = 0.20106; // Wheel circumference int prev_pulses = 0; // Previous pulse count int curr_pulses = 0; // Current pulse count float velocity = 0; // Velocity QEI qei(PTD3, PTD2, NC, ppr, QEI::X4_ENCODING); // Quadrature encoder // ======== // Velocity // ======== float Kp = 8.0; // Proportional factor float Ki = 0; // Integral factor float Kd = 0; // Derivative factor float 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 // ===== // Servo // ===== float angle = 88; // Angle float Ks = 0.9; // Steering proportion Servo servo(PTA12); // Signal pin (PWM) // ====== // Camera // ====== int t_int = 10000; // 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 DigitalOut si(PTD0); // SI pin FastAnalogIn ao(PTC2); // AO pin // ================ // Image processing // ================ int max = -1; // Maximum luminosity int left[5] = {0}; // Left edge int right[5] = {0}; // Right edge int j = 0; // Peaks index int center = 64; // Center estimate int centers[5] = {0}; // Possible centers int prev_center = 64; // Previous center float scores[5] = {0}; // Candidate scores int best_score_idx = 0; // Most likely center index // ================ // 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.05; motor_ctrl.setSetPoint(ref_v); } if (serial->rxGetLastChar() == '-') { 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 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"); 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)){ 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; 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: 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()) { comm_cmd = bt.getc(); } } } } void control() { // Image capture // t.reset(); // Dummy read PTD->PCOR = (0x01 << 5); PTD->PSOR = (0x01); PTD->PSOR = (0x01 << 5); PTD->PCOR = (0x01); for (int i = 0; i < 128; i++) { PTD->PCOR = (0x01 << 5); PTD->PSOR = (0x01 << 5); } // Expose wait_us(t_int); // Read camera PTD->PCOR = (0x01 << 5); PTD->PSOR = (0x01); PTD->PSOR = (0x01 << 5); PTD->PCOR = (0x01); for (int i = 0; i < 128; i++) { PTD->PCOR = (0x01 << 5); if (i > 9 && i < 118) { img[i-10] = ao.read_u16(); tele_linescan[i-10] = img[i-10]; } PTD->PSOR = (0x01 << 5); } // t_cam = t.read_us(); // Steering control // t.reset(); // Detect peak edges j = 0; for (int i = 0; i < 107 && j < 5; i++) { if (img[i] > 45000) { left[j] = i; while (img[i] > 45000) { i = i + 1; } right[j] = i; j = j + 1; } } // Calculate peak centers for (int i = 0; i < j; i++) { centers[i] = (left[i] + right[i] + 10) / 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); } // 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; // Set servo angle angle = 88 + (55 - center) * Ks; if (angle > 113) { angle = 113; } if (angle < 63) { angle = 63; } servo = angle / 180; // AGC max = -1; for (int i = 0; i < 107; i++) { if (img[i] > max) { max = img[i]; } } if (max > 60000) { t_int = t_int - 0.1 * (max - 60000); } if (max < 50000) { t_int = t_int + 0.1 * (50000 - max); } if (t_int < T_INT_MIN) { t_int = T_INT_MIN; } if (t_int > T_INT_MAX) { t_int = T_INT_MAX; } tele_exposure = t_int; // 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 = 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_control_flag() { ctrl_flag = true; } // ==== // Main // ==== int main() { t.start(); t_tele.start(); 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.75); motor_ctrl.setSetPoint(ref_v); motor_ctrl.setBias(0.0); motor_ctrl.setMode(1); // Initialize servo servo.calibrate(0.001, 45.0); servo = angle / 180.0; // Initialize communications thread Thread communication_thread(communication); control_interrupt.attach(control, interrupt_T); // control_interrupt.attach(set_control_flag, interrupt_T); while (true) { // if (ctrl_flag) { // control(); // } } }