File content as of revision 17:bf6192a361ab:

#include "FastAnalogIn.h"
#include "mbed.h"
#include "PID.h"
#include "QEI.h"
#include "rtos.h"
#include "SerialRPCInterface.h"
#include "Servo.h"
#include "telemetry.h"


// =========
// Telemetry
// =========
//DigitalOut test(PTD1);

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

telemetry::Numeric<uint32_t> tele_time_ms(telemetry_obj, "time", "Time", "ms", 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_cam1 = 0;
int t_cam2 = 0;
int t_steer = 0;
int t_vel = 0;
int t_down = 0;

float interrupt_T = 0.015;
bool ctrl_flag = false;

// =============
// Communication
// =============
char comm_cmd;                                          // Command
char comm_in[5];                                        // Input
Serial bt(USBTX, USBRX);                                // USB connection
//Serial bt(PTC4, PTC3);                                  // BlueSMiRF connection

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

// =====
// Motor
// =====
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 ppr = 389;                                    // Pulses per revolution
const float c = 0.20106;                                // Wheel circumference
int prev_pulses = 0;                                    // Previous pulse count
int prev_pulse_counts[5] = {0};
int curr_pulses = 0;                                    // Current pulse count
float velocity = 0;                                     // Velocity
QEI qei(PTD3, PTD2, NC, ppr, QEI::X4_ENCODING);         // Quadrature encoder

// ========
// Velocity
// ========
float Kmp = 36.0;                                       // Proportional factor
float Kmi = 1;                                          // Integral factor
float Kmd = 0;                                          // Derivative factor
float interval = 0.01;                                  // Sampling interval
float ref_v = 0.5;                                      // Reference velocity
int vel_count = 1;
PID motor_ctrl(Kmp, Kmi, Kmd, interrupt_T);             // Motor controller

// =====
// Servo
// =====
float angle = 88;                                       // Angle
float Ksp = 1;                                          // Steering proportion
float Ksi = 0;
float Ksd = 0;
PID servo_ctrl(Ksp, Ksi, Ksd, interrupt_T);
Servo servo(PTA12);                                     // Signal pin (PWM)

// ======
// Camera
// ======
int t_int = 10000;                                      // Exposure time
const int T_INT_MAX = interrupt_T * 1000000 - 2000;     // 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;                                           // 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 killswitch(MODSERIAL_IRQ_INFO *q) {
    MODSERIAL *serial = q->serial;
    if (serial->rxGetLastChar() == 'k') {
        e_stop = true;
        motor = 1.0;
    }
    if (serial->rxGetLastChar() == '+') {
        ref_v = ref_v + 0.2;
        motor_ctrl.setSetPoint(ref_v);
    }
    if (serial->rxGetLastChar() == '-') {
        ref_v = ref_v - 0.2;
        motor_ctrl.setSetPoint(ref_v);
    }
}

// Communications
//void communication(void const *args) {
//    telemetry_serial.baud(115200);
//    telemetry_serial.attach(&killswitch, MODSERIAL::RxIrq);
//    telemetry_obj.transmit_header();
//    while (true) {
//        tele_time_ms = t_tele.read_ms();
//        telemetry_obj.do_io();
//    }
//}

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");
        bt.printf("  [0] Velocity\r\n");
        bt.printf("  [1] Steering\r\n");
        bt.printf("  [2] Change Kmp\r\n");
        bt.printf("  [3] Change Kmi\r\n");
        bt.printf("  [4] Change Kmd\r\n");
        bt.printf("  [5] Change Ksp\r\n");
        bt.printf("  [6] Change reference velocity\r\n");
        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, Kmp: %f, Kmi: %f, Kmd: %f\r\n", motor_duty, curr_pulses, velocity, Kmp, Kmi, Kmd);
                    break;
                case 1:
                    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): ", Kmp);
                    for (int i = 0; i < 5; i++) {
                        comm_in[i] = bt.getc();
                        bt.putc(comm_in[i]);
                    }
                    bt.printf("\r\n");
                    Kmp = atof(comm_in);
                    motor_ctrl.setTunings(Kmp, Kmi, Kmd);
                    comm_cmd = 'q';
                    break;
                case 3:
                    bt.printf("Current: %f, New (5 digits): ", Kmi);
                    for (int i = 0; i < 5; i++) {
                        comm_in[i] = bt.getc();
                        bt.putc(comm_in[i]);
                    }
                    bt.printf("\r\n");
                    Kmi = atof(comm_in);
                    motor_ctrl.setTunings(Kmp, Kmi, Kmd);
                    comm_cmd = 'q';
                    break;
                case 4:
                    bt.printf("Current: %f, New (5 digits): ", Kmd);
                    for (int i = 0; i < 5; i++) {
                        comm_in[i] = bt.getc();
                        bt.putc(comm_in[i]);
                    }
                    bt.printf("\r\n");
                    Kmd = atof(comm_in);
                    motor_ctrl.setTunings(Kmp, Kmi, Kmd);
                    comm_cmd = 'q';
                    break;
                case 5:
                    bt.printf("Current: %f, New (5 digits): ", Ksp);
                    for (int i = 0; i < 5; i++) {
                        comm_in[i] = bt.getc();
                        bt.putc(comm_in[i]);
                    }
                    bt.printf("\r\n");
                    Ksp = 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++) {
                        comm_in[i] = bt.getc();
                        bt.putc(comm_in[i]);
                    }
                    bt.printf("\r\n");
                    ref_v = atof(comm_in);
                    motor_ctrl.setSetPoint(ref_v);
                    comm_cmd = 'q';
                    break;
                case 7:
                    e_stop = true;
                    bt.printf("STOPPED\r\n");
                    comm_cmd = 'q';
                    break;
                case 8:
                    bt.printf("Read 1: %dus, Exposure: %dus, Read 2: %dus, Steering: %dus, Velocity: %dus, Down: %dus, Total: %dus\r\n", t_cam1, t_int, t_cam2, t_steer, t_vel, t_down, t_cam1+t_int+t_cam2+t_steer+t_vel+t_down);
                    break;
            }
            if (bt.readable()) {
                comm_cmd = bt.getc();
            }
        }
    }
}

void control() {
    // Image capture
    t_down = t.read_us();
    t.reset();
    
    // Dummy read
    PTD->PCOR = (0x01 << 5);
    PTD->PSOR = (0x01);
    PTD->PSOR = (0x01 << 5);
    PTD->PCOR = (0x01);
    
    for (int i = 0; i < 128; i++) {
        PTD->PCOR = (0x01 << 5);
        PTD->PSOR = (0x01 << 5);
    }
    t_cam1 = t.read_us();
    
    // Expose
    wait_us(t_int);
    
    // Read camera
    PTD->PCOR = (0x01 << 5);
    PTD->PSOR = (0x01);
    PTD->PSOR = (0x01 << 5);
    PTD->PCOR = (0x01);
    
    t.reset();
    for (int i = 0; i < 128; i++) {
        PTD->PCOR = (0x01 << 5);
        if (i > 9 && i < 118) {
            img[i-10] = ao.read_u16();
            tele_linescan[i-10] = img[i-10];
        }
        PTD->PSOR = (0x01 << 5);
    }
    
    t_cam2 = t.read_us();
    
    // Steering control
    t.reset();
    
    // Detect peak edges
    j = 0;
    for (int i = 0; i < 108 && j < 5; i++) {
        if (img[i] > max * 0.65) {
            left[j] = i;
            while (img[i] > max * 0.65) {
                if (i < 107) {
                    i = i + 1;
                } else {
                    j = j - 1;
                    break;
                }
            }
            if (i < 107) {
                right[j] = i;
            }
            j = j + 1;
        }
    }
    
    
    if (j > 0) {
        // Calculate peak centers
        for (int i = 0; i < j; i++) {
            centers[i] = (left[i] + right[i] + 20) / 2;
        }
        
        // Assign scores
        for (int i = 0; i < j; i++) {
            scores[i] = 4 / (right[i] - left[i]) + img[centers[i]] / 65536 + 16 / abs(centers[i] - prev_center);
        }
        
        // 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;
        
        angle = 88 + (64 - center) * 0.9;
        if (angle > 113) {
            angle = 113;
        }
        if (angle < 63) {
            angle = 63;
        }
        // servo_ctrl.setProcessValue(center);
        // angle = 88 + servo_ctrl.compute();
        // servo = floor(angle / 180 * 100 + 0.5) / 100;
        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 < T_INT_MIN) {
        t_int = T_INT_MIN;
    }
    if (t_int > T_INT_MAX) {
        t_int = T_INT_MAX;
    }
    tele_exposure = t_int;
    
    t_steer = t.read_us();
    
    // wait_us(8000 - t.read_us());
    
    
    
    // Velocity control
    t.reset();
    if (!e_stop) {
        curr_pulses = qei.getPulses();
        if (vel_count < 6) {
            velocity = curr_pulses / interrupt_T / vel_count / ppr * c;
            prev_pulse_counts[vel_count - 1] = curr_pulses;
            vel_count = vel_count + 1;
        } else {
            velocity = (curr_pulses - prev_pulse_counts[0]) / interrupt_T / 5 / ppr * c;
            for (int i = 0; i < 4; i++) {
                prev_pulse_counts[i] = prev_pulse_counts[i+1];
            }
            prev_pulse_counts[4] = curr_pulses;
        }
        tele_velocity = velocity;
        motor_ctrl.setProcessValue(velocity);
        motor_duty = motor_ctrl.compute();
        motor = 1.0 - motor_duty;
        tele_pwm = motor_duty;
    } else {
        motor = 1.0;
    }
    t_vel = t.read_us();
    t.reset();
    ctrl_flag = false;
//    test = 0;
}

void set_ctrl_flag() {
//    test = 1;
    ctrl_flag = true;
}

// ====
// Main
// ====
int main() {
    t.start();
    t_tele.start();
    tele_center.set_limits(0, 128);
    tele_pwm.set_limits(0.0, 1.0);
    
    // Initialize motor
    motor.period_us(motor_T);
    motor = 1.0 - motor_duty;
    
    // Initialize motor controller
    motor_ctrl.setInputLimits(0.0, 10.0);
    motor_ctrl.setOutputLimits(0.0, 0.5);
    motor_ctrl.setSetPoint(ref_v);
    motor_ctrl.setBias(0.0);
    motor_ctrl.setMode(1);
    
    // Initialize servo
    servo.calibrate(0.001, 45.0);
    servo = angle / 180.0;
    
    // Initialize servo controller
    servo_ctrl.setInputLimits(10, 117);
    servo_ctrl.setOutputLimits(-25, 25);
    servo_ctrl.setSetPoint(63.5);
    servo_ctrl.setBias(0);
    servo_ctrl.setMode(1);
    
    // Initialize communications thread
//    Thread communication_thread(communication);
    
    // control_interrupt.attach(control, interrupt_T);
    // Thread::wait(osWaitForever);
    
    control_interrupt.attach(set_ctrl_flag, interrupt_T);
    while (true) {
        if (ctrl_flag) {
            control();
        }
    }
}