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