![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Everything put together
Dependencies: FastAnalogIn MODSERIAL PID QEI RPCInterface Servo mbed-rtos mbed telemetry
Diff: main.cpp
- Revision:
- 0:fcf070a88ba0
- Child:
- 1:32610c005260
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Mar 18 22:33:47 2016 +0000 @@ -0,0 +1,245 @@ +#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); + } +} \ No newline at end of file