File content as of revision 1:32610c005260:

#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 = 13.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");
        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;
            }
            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;
    wait(7);
    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);
    motor_ctrl.setBias(0.0);
    motor_ctrl.setMode(1);
    
    // Initialize bluetooth
    bt.baud(115200);
    
    // Initialize communications thread
    Thread communication_thread(communication);
    
    // Start motor controller
//    motor = 0.85;
    
    
    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);
    }
}