File content as of revision 9:10fcf3d0ec2d:

#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

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);


// =============
// Communication
// =============
Serial pc(USBTX, USBRX);                            // USB connection
//Serial bt(PTC4, PTC3);                              // BlueSMiRF connection
char cmd;                                           // Command
char ch;
char in[5];

void communication(void const *args);               // Communications

// =====
// Motor
// =====
PwmOut motor(PTA4);                                 // Enable pin (PWM)
int T = 25000;                                      // Frequency
float d = 0.0;                                      // Duty cycle

// =======
// 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};

// ========
// 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

// =====
// Servo
// =====
Servo servo(PTA12);                                 // Enable pin (PWM)
float a = 88;                                       // Angle
float Ks = 0.7;

// ======
// 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;
float lum_bg = 0;
float contrast = 0;
int argmax = 0;
int argmin = 0;
int temp[128];
int center = 64;

void track();                                       // Line-tracking steering

// ================
// Functions
// ================

void tele_comm(void const *args) {
    telemetry_serial.baud(115200);
    telemetry_obj.transmit_header();
    while (true) {
        tele_time_ms = t.read_ms();
        telemetry_obj.do_io();
        Thread::wait(100);
    }
}

// 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 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");
//        cmd = bt.getc();
//        while (cmd != 'q') {
//            switch(atoi(&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);
//                    break;
//                case 1:
//                    bt.printf("Servo angle: %f, Track center: %d, t_int: %d\r\n", a, 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]);
//                    }
//                    bt.printf("\r\n");
//                    Kp = atof(in);
//                    motor_ctrl.setTunings(Kp, Ki, Kd);
//                    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]);
//                    }
//                    bt.printf("\r\n");
//                    Ki = atof(in);
//                    motor_ctrl.setTunings(Kp, Ki, Kd);
//                    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]);
//                    }
//                    bt.printf("\r\n");
//                    Kd = atof(in);
//                    motor_ctrl.setTunings(Kp, Ki, Kd);
//                    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]);
//                    }
//                    bt.printf("\r\n");
//                    Ks = atof(in);
//                    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]);
//                    }
//                    bt.printf("\r\n");
//                    ref_v = atof(in);
//                    motor_ctrl.setSetPoint(ref_v);
//                    cmd = 'q';
//                    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();
        tele_linescan[i] = img[i];
        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;
    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;
            }
            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;
                }
            }
        }
    }
    
    for (int i = 0; i < 10; i++) {
        lum_bg = lum_bg + img[64 - 4 - i] / 20.0 + img[64 + 4 + i] / 20.0;
    }
    
    contrast = (max - lum_bg) / lum_bg;
    
//    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);
        }
//    }
    
    if (max > 43253) {
        center = (argmax + argmin + 2 + 11) / 2;
        tele_center = center;
        a = 88 + (64 - center) * Ks;
        servo = a / 180;
    }
    
    camera_read.attach_us(&read_camera, t_int);
}

// ====
// Main
// ====
int main() {
    osThreadSetPriority(osThreadGetId(), osPriorityRealTime);
    t.start();
    
    // 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.01, 0.5);
    motor_ctrl.setSetPoint(ref_v);
    motor_ctrl.setBias(0.0);
    motor_ctrl.setMode(1);
    
    // Initialize bluetooth
//    bt.baud(115200);

//    osThreadSetPriority(osThreadGetId(), osPriorityAboveNormal);
    
    // Initialize communications thread
//    Thread communication_thread(communication);
    Thread tele_comm_thread(tele_comm);
//    tele_comm_thread.set_priority(osPriorityBelowNormal);
    
    // 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);
    }
}