Revision 12:54e7d8ff3a74, committed 2016-04-08

Comitter:

vsutardja

Date:

Fri Apr 08 21:40:35 2016 +0000

Parent:

11:4348bba086a4

Child:

13:d6e167698517

Commit message:

init

Changed in this revision

--- a/main.cpp	Tue Apr 05 21:52:32 2016 +0000
+++ b/main.cpp	Fri Apr 08 21:40:35 2016 +0000
@@ -1,120 +1,123 @@
+#include "FastAnalogIn.h"
 #include "mbed.h"
-#include "rtos.h"
-#include "Servo.h"
-#include "FastAnalogIn.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
-telemetry::MbedHal telemetry_hal(telemetry_serial); // Hardware Abstraction Layer
-telemetry::Telemetry telemetry_obj(telemetry_hal);  // 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
 
-int dec = 0;
-Timer t;
 telemetry::Numeric<uint32_t> tele_time_ms(telemetry_obj, "time", "Time", "ms", 0);
-telemetry::NumericArray<uint16_t, 128> tele_linescan(telemetry_obj, "linescan", "Linescan", "ADC", 0);
-//telemetry::Numeric<float> tele_vel(telemetry_obj, "t_vel", "Velocity", "m/s", 0);
-telemetry::Numeric<uint32_t> tele_center(telemetry_obj, "t_center", "Center", "", 0);
-telemetry::Numeric<uint32_t> tele_t_int(telemetry_obj, "t_t_int", "t_int", "us", 0);
-telemetry::Numeric<uint32_t> tele_cam_dec(telemetry_obj, "t_cam_dec", "decimation", "", 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.025;
 
 // =============
 // Communication
 // =============
-Serial pc(USBTX, USBRX);                            // USB connection
-//Serial bt(PTC4, PTC3);                              // BlueSMiRF connection
-char cmd;                                           // Command
-char ch;
-char in[5];
+char comm_cmd;                                          // Command
+char comm_in[5];                                        // Input
+//Serial pc(USBTX, USBRX);                                // USB connection
+//Serial bt(PTC4, PTC3);                                  // BlueSMiRF connection
 
-void communication(void const *args);               // Communications
+void communication(void const *args);                   // Communications
 
 // =====
 // Motor
 // =====
-PwmOut motor(PTA4);                                 // Enable pin (PWM)
-int T = 25000;                                      // Frequency
-float d = 0.0;                                      // Duty cycle
+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 MVG_AVG = 4;
-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
-float vel = 0;
-float v_prev[MVG_AVG] = {0};
+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 = 6.0;                                     // Proportional factor
-float Ki = 0;                                       // Integral factor
-float Kd = 0;                                       // Derivative factor
-float interval = 0.1;                              // Sampling interval
-float ref_v = 1.0;
-PID motor_ctrl(Kp, Ki, Kd, interval);               // Motor controller
+float Kp = 6.0;                                         // Proportional factor
+float Ki = 0;                                           // Integral factor
+float Kd = 0;                                           // Derivative factor
+float interval = 0.01;                                  // Sampling interval
+float ref_v = 0.8;                                      // Reference velocity
+PID motor_ctrl(Kp, Ki, Kd, interval);                   // Motor controller
 
 // =====
 // Servo
 // =====
-Servo servo(PTA12);                                 // Enable pin (PWM)
-float a = 88;                                       // Angle
-float Ks = 0.9;
+float angle = 88;                                       // Angle
+float Ks = 0.9;                                         // Steering proportion
+Servo servo(PTA12);                                     // Signal pin (PWM)
 
 // ======
 // Camera
 // ======
-DigitalOut clk(PTD5);                               // Clock pin
-DigitalOut si(PTD0);                                // SI pin
-FastAnalogIn ao(PTC2);                              // AO pin
-Timeout camera_read;                                // Camera read timeout
-const int T_INT_MIN = 2500;
-int cam_dec = 0;
-int cam_dec_count = 0;
-int t_int = 15000;                                  // Exposure time
-int img[128];                                       // Image data
-
-void readCamera();                                  // Read data from camera
+int t_int = 10000;                                      // Exposure time
+const int T_INT_MAX = interrupt_T * 1000000 - 10000;    // 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;
-float lum_bg = 0;
-float contrast = 0;
-int argmax = 0;
-int argmin = 0;
-int temp[128];
-int center = 64;
-
-void track();                                       // Line-tracking steering
+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 tele_comm(void const *args) {
+// Communications
+void communication(void const *args) {
     telemetry_serial.baud(115200);
     telemetry_obj.transmit_header();
     while (true) {
-        tele_time_ms = t.read_ms();
+        tele_time_ms = t_tele.read_ms();
         telemetry_obj.do_io();
-        Thread::wait(100);
     }
 }
-
-// Communications
 //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");
@@ -125,100 +128,112 @@
 //        bt.printf("  [4] Change Kd\r\n");
 //        bt.printf("  [5] Change Ks\r\n");
 //        bt.printf("  [6] Change reference velocity\r\n");
-//        cmd = bt.getc();
-//        while (cmd != 'q') {
-//            switch(atoi(&cmd)){
+//        bt.printf("  [7] EMERGENCY STOP\r\n");
+//        bt.printf("  [8] Timing\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", d, curr_pulses, velocity, Kp, Ki, Kd);
+//                    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", a, center, t_int);
+//                    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 (5 digits): ", Kp);
 //                    for (int i = 0; i < 5; i++) {
-//                        in[i] = bt.getc();
-//                        bt.putc(in[i]);
+//                        comm_in[i] = bt.getc();
+//                        bt.putc(comm_in[i]);
 //                    }
 //                    bt.printf("\r\n");
-//                    Kp = atof(in);
+//                    Kp = atof(comm_in);
 //                    motor_ctrl.setTunings(Kp, Ki, Kd);
-//                    cmd = 'q';
+//                    comm_cmd = 'q';
 //                    break;
 //                case 3:
 //                    bt.printf("Current: %f, New (5 digits): ", Ki);
 //                    for (int i = 0; i < 5; i++) {
-//                        in[i] = bt.getc();
-//                        bt.putc(in[i]);
+//                        comm_in[i] = bt.getc();
+//                        bt.putc(comm_in[i]);
 //                    }
 //                    bt.printf("\r\n");
-//                    Ki = atof(in);
+//                    Ki = atof(comm_in);
 //                    motor_ctrl.setTunings(Kp, Ki, Kd);
-//                    cmd = 'q';
+//                    comm_cmd = 'q';
 //                    break;
 //                case 4:
 //                    bt.printf("Current: %f, New (5 digits): ", Kd);
 //                    for (int i = 0; i < 5; i++) {
-//                        in[i] = bt.getc();
-//                        bt.putc(in[i]);
+//                        comm_in[i] = bt.getc();
+//                        bt.putc(comm_in[i]);
 //                    }
 //                    bt.printf("\r\n");
-//                    Kd = atof(in);
+//                    Kd = atof(comm_in);
 //                    motor_ctrl.setTunings(Kp, Ki, Kd);
-//                    cmd = 'q';
+//                    comm_cmd = 'q';
 //                    break;
 //                case 5:
 //                    bt.printf("Current: %f, New (5 digits): ", Ks);
 //                    for (int i = 0; i < 5; i++) {
-//                        in[i] = bt.getc();
-//                        bt.putc(in[i]);
+//                        comm_in[i] = bt.getc();
+//                        bt.putc(comm_in[i]);
 //                    }
 //                    bt.printf("\r\n");
-//                    Ks = atof(in);
-//                    cmd = 'q';
+//                    Ks = atof(comm_in);
+//                    comm_cmd = 'q';
 //                    break;
 //                case 6:
 //                    bt.printf("Current: %f, New (5 digits): ", ref_v);
 //                    for (int i = 0; i < 5; i++) {
-//                        in[i] = bt.getc();
-//                        bt.putc(in[i]);
+//                        comm_in[i] = bt.getc();
+//                        bt.putc(comm_in[i]);
 //                    }
 //                    bt.printf("\r\n");
-//                    ref_v = atof(in);
+//                    ref_v = atof(comm_in);
 //                    motor_ctrl.setSetPoint(ref_v);
-//                    cmd = 'q';
+//                    comm_cmd = 'q';
+//                    break;
+//                case 7:
+//                    e_stop = true;
+//                    bt.printf("STOPPED\r\n");
+//                    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;
 //            }
 //            if (bt.readable()) {
-//                cmd = bt.getc();
+//                comm_cmd = bt.getc();
 //            }
 //        }
 //    }
 //}
 
-// Read data from camera
-void read_camera() {
-    //if (cam_dec_count < cam_dec) {
-//        si = 1;
-//        wait_us(1);
-//        clk = 1;
-//        wait_us(1);
-//        si = 0;
-//        
-//        for (int i = 0; i < 128; i++) {
-//            wait_us(1);
-//            clk = 0;
-//            wait_us(1);
-//            clk = 1;
-//        }
-//        clk = 0;
-//        cam_dec_count = cam_dec_count + 1;
-//        camera_read.attach_us(&read_camera, t_int);
-//        return;
-//    }
-    cam_dec_count = 0;
+void control() {
+    // Image capture
+    t.reset();
+    
+    // Dummy read
+    clk = 0;
+    wait_us(1);
+    si = 1;
+    wait_us(1);
+    clk = 1;
+    wait_us(1);
+    si = 0;
     
-    // Start data transfer
+    for (int i = 0; i < 128; i++) {
+        wait_us(1);
+        clk = 0;
+        wait_us(1);
+        clk = 1;
+    }
+    
+    // Expose
+    wait_us(t_int);
+    
+    // Read camera
+    clk = 0;
+    wait_us(1);
     si = 1;
     wait_us(1);
     clk = 1;
@@ -226,129 +241,119 @@
     si = 0;
     wait_us(1);
     
-    // Read camera data
     for (int i = 0; i < 128; i++) {
         clk = 0;
-        img[i] = ao.read_u16();
-        tele_linescan[i] = img[i];
+        if (i > 9 && i < 118) {
+            img[i-10] = ao.read_u16();
+            tele_linescan[i-10] = img[i-10];
+        } else {
+            wait_us(1);
+        }
         clk = 1;
         wait_us(1);
     }
-    clk = 0;
+    
+    t_cam = t.read_us();
     
-    // Update servo angle
-    track();
+    // Steering control
+    t.reset();
     
-    // 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;
-    lum_bg = 0;
-    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;
+    // 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;
             }
-            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;
-                }
-            }
+            right[j] = i;
+            j = j + 1;
         }
     }
     
-    for (int i = 0; i < 10; i++) {
-        lum_bg = lum_bg + img[55 - 4 - i] / 20.0 + img[55 + 4 + i] / 20.0;
+    // 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);
     }
     
-    contrast = (max - lum_bg) / lum_bg;
+    // 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;
     
-//    if (contrast < 1.5) {
-    // Underexposed
-    //if (max < 60000) {
-//        t_int = t_int + 0.15 * (60000 - max);
-//    }
-//    // Overexposed
-//    if (lum_bg > 25000) {
-//        t_int = t_int - 0.15 * (lum_bg - 25000);
-//    }
-//    }
-
+    // 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 < 1000) {
-        t_int = 1000;
+    if (t_int < T_INT_MIN) {
+        t_int = T_INT_MIN;
+    }
+    if (t_int > T_INT_MAX) {
+        t_int = T_INT_MAX;
     }
-
-    tele_t_int = t_int;
+    tele_exposure = t_int;
     
-    //if (t_int < T_INT_MIN) {
-//        cam_dec = T_INT_MIN / t_int;
-//    }
+    t_steer = t.read_us();
     
-    tele_cam_dec = cam_dec;
+    // wait_us(8000 - t.read_us());
     
-    if (max > 43253 && argmax < argmin) {
-        center = (argmax + argmin + 2 + 11) / 2;
-        tele_center = center;
-        a = 88 + (55 - center) * Ks;
-        if (a > 113) {
-            a = 113;
-        }
-        if (a < 63) {
-            a = 63;
-        }
-        servo = a / 180;
+    // Velocity control
+    t.reset();
+    if (!e_stop) {
+        curr_pulses = qei.getPulses();
+        velocity = (curr_pulses - prev_pulses) / interrupt_T / ppr * c;
+        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;
     }
-    
-//    camera_read.attach_us(&read_camera, 1000);
-    camera_read.attach_us(&read_camera, t_int);
+    t_vel = t.read_us();
 }
 
 // ====
 // Main
 // ====
 int main() {
-//    osThreadSetPriority(osThreadGetId(), osPriorityRealTime);
     t.start();
+    t_tele.start();
+    tele_center.set_limits(0, 128);
+    tele_pwm.set_limits(0.0, 1.0);
     
     // 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();
+    motor.period_us(motor_T);
+    motor = 1.0 - motor_duty;
     
     // Initialize motor controller
     motor_ctrl.setInputLimits(0.0, 10.0);
@@ -357,42 +362,14 @@
     motor_ctrl.setBias(0.0);
     motor_ctrl.setMode(1);
     
-    // Initialize bluetooth
-//    bt.baud(115200);
-
-//    osThreadSetPriority(osThreadGetId(), osPriorityAboveNormal);
+    // Initialize servo
+    servo.calibrate(0.001, 45.0);
+    servo = angle / 180.0;
     
     // Initialize communications thread
-//    Thread communication_thread(communication);
-    Thread tele_comm_thread(tele_comm);
-//    tele_comm_thread.set_priority(osPriorityBelowNormal);
+    Thread communication_thread(communication);
     
-    // Start motor controller
-    while (true) {
-//        telemetry_serial.printf("%d\r\n", t.read_ms());
-//        if (dec == 100) {
-            //tele_time_ms = t.read_ms();
-//            telemetry_obj.do_io();
-//            dec = 0;
-//        }
-//        dec = dec + 1;
-        curr_pulses = qei.getPulses();
-        //for (int i = 0; i < MVG_AVG - 1; i++) {
-//            v_prev[i] = abs(1.5 - 0.5 * (1.5 - v_prev[i+1]));
-//        }
-//        v_prev[MVG_AVG-1] = velocity;
-        velocity = (curr_pulses - prev_pulses)/ interval / ppr * c / 2.5;
-//        tele_vel = velocity;
-        //vel = velocity;
-//        for (int i = 0; i < MVG_AVG; i++) {
-//            velocity = velocity + v_prev[i];
-//        }
-//        velocity = velocity / (MVG_AVG + 1.0);
-        prev_pulses = curr_pulses;
-        motor_ctrl.setProcessValue(velocity);
-        d = motor_ctrl.compute();
-        motor = 1.0 - d;
-        Thread::wait(interval*1000);
-//        pc.printf("Exposure: %d, Center: %d, Angle: %f\r\n", t_int, center, a);
-    }
+    control_interrupt.attach(control, interrupt_T);
+    
+    while (true);
 }
\ No newline at end of file