![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Everything put together
Dependencies: FastAnalogIn MODSERIAL PID QEI RPCInterface Servo mbed-rtos mbed telemetry
main.cpp
- Committer:
- vsutardja
- Date:
- 2016-03-18
- Revision:
- 0:fcf070a88ba0
- Child:
- 1:32610c005260
File content as of revision 0:fcf070a88ba0:
#include "mbed.h" #include "rtos.h" #include "Servo.h" #include "FastAnalogIn.h" #include "PID.h" #include "QEI.h" #include "telemetry.h" // ========= // Telemetry // ========= //MODSERIAL telemetry_serial(PTC4, PTC3); // TX, RX //telemetry::MbedHal telemetry_hal(telemetry_serial); // Hardware Abstraction Layer //telemetry::Telemetry telemetry_obj(telemetry_hal); // Telemetry // ============= // Communication // ============= Serial pc(USBTX, USBRX); // USB connection Serial bt(PTC4, PTC3); // BlueSMiRF connection //int idx = 0; char cmd; // Command char ch; char in[2]; void communication(void const *args); // Communications // ===== // Motor // ===== PwmOut motor(PTA4); // Enable pin (PWM) int T = 25000; // Frequency float d = 0.0; // Duty cycle // ======= // Encoder // ======= int ppr = 389; // Pulses per revolution QEI qei(PTD3, PTD2, NC, ppr, QEI::X4_ENCODING); // Quadrature encoder float c = 0.20106; // Wheel circumference int prev_pulses = 0; // Previous pulse count int curr_pulses = 0; // Current pulse count float velocity = 0; // Velocity // ======== // Velocity // ======== float Kp = 3.0; // Proportional factor float Ki = 0; // Integral factor float Kd = 0; // Derivative factor float interval = 0.01; // Sampling interval PID motor_ctrl(Kp, Ki, Kd, interval); // Motor controller // ===== // Servo // ===== Servo servo(PTA12); // Enable pin (PWM) float a = 88; // Angle // ====== // Camera // ====== DigitalOut clk(PTD5); // Clock pin DigitalOut si(PTD0); // SI pin FastAnalogIn ao(PTC2); // AO pin Timeout camera_read; // Camera read timeout int t_int = 15000; // Exposure time int img[128]; // Image data void readCamera(); // Read data from camera // ================ // Image processing // ================ int max = -1; int argmax = 0; int argmin = 0; int temp[128]; int center; void track(); // Line-tracking steering // ================ // Functions // ================ // Communications void communication(void const *args) { 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 exposure time\r\n"); cmd = bt.getc(); while (cmd != 'q') { switch(atoi(&cmd)){ case 0: bt.printf("Duty cycle: %f, Pulse count: %d, Velocity: %f, Kp: %f\r\n", d, curr_pulses, velocity, Kp); break; case 1: bt.printf("Servo angle: %f, Track center: %d, t_int: %d\r\n", a, center, t_int); break; case 2: // idx = 0; bt.printf("Current Kp = %f. Enter new Kp: ", Kp); // while ((cmd = bt.getc()) != '\n') { for (int idx = 0; idx < 2; idx++) { ch = bt.getc(); in[idx] = ch; // idx = idx + 1; } bt.printf("\r\n"); Kp = atof(in); break; case 3: // idx = 0; bt.printf("Current t_int = %dms. Enter new t_int (ms): ", t_int / 1000); // while ((cmd = bt.getc()) != '\n') { for (int idx = 0; idx < 2; idx++) { ch = bt.getc(); in[idx] = ch; // idx = idx + 1; } bt.printf("\r\n"); t_int = atoi(in); break; } if (bt.readable()) { cmd = bt.getc(); } } } } // Read data from camera void read_camera() { // Start data transfer si = 1; wait_us(1); clk = 1; wait_us(1); si = 0; wait_us(1); // Read camera data for (int i = 0; i < 128; i++) { clk = 0; img[i] = ao.read_u16(); clk = 1; wait_us(1); } clk = 0; // Update servo angle track(); // Set next frame exposure time camera_read.attach_us(&read_camera, t_int); } // Find line center from image // Take two-point moving average to smooth the data // Find indices where max and min of smoothed data are attained // Calculate and return midpoint of argmax and argmin void track() { max = -1; argmax = 0; argmin = 0; for (int i = 0; i < 107; i++) { if (img[i+11] > max) { max = img[i+11]; } if (i == 126) { temp[i-1] = (img[i+11] + img[i+1+11]) / 2 - temp[i-1]; if (temp[i-1] > temp[argmax]) { argmax = i - 1; } if (temp[i-1] < temp[argmin]) { argmin = i - 1; } } else { temp[i] = (img[i+11] + img[i+1+11]) / 2; if (i > 0) { temp[i-1] = temp[i] - temp[i-1]; if (temp[i-1] > temp[argmax]) { argmax = i - 1; } if (temp[i-1] < temp[argmin]) { argmin = i - 1; } } } } if (max > 43253) { center = (argmax + argmin + 2 + 11) / 2; a = 88 + (64 - center) * 0.625; servo = a / 180; } } // ==== // Main // ==== int main() { // Initialize motor motor.period_us(T); motor = 1.0 - d; // Initialize servo servo.calibrate(0.001, 45.0); servo = a / 180.0; // Initialize & start camera clk = 0; read_camera(); // Initialize motor controller motor_ctrl.setInputLimits(0.0, 10.0); motor_ctrl.setOutputLimits(0.1, 0.5); motor_ctrl.setSetPoint(3.5); motor_ctrl.setBias(0.0); motor_ctrl.setMode(1); // Initialize bluetooth bt.baud(115200); // Initialize communications thread Thread communication_thread(communication); // Start motor controller while (true) { curr_pulses = qei.getPulses(); velocity = (curr_pulses - prev_pulses)/ interval / ppr * c; prev_pulses = curr_pulses; motor_ctrl.setProcessValue(velocity); d = motor_ctrl.compute(); motor = 1.0 - d; wait(interval); pc.printf("Velocity: %f\r\n", velocity); } }