File content as of revision 28:4b76abe148cd:

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

#define set_clk() PTD->PSOR = (0x01);
#define clr_clk() PTD->PCOR = (0x01);
#define set_si() PTD->PSOR = (0x01 << 5);
#define clr_si() PTD->PCOR = (0x01 << 5);


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

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<uint8_t> tele_center(telemetry_obj, "tele_center", "Center", "px", 0);
telemetry::Numeric<float> tele_velocity(telemetry_obj, "tele_vel", "Velocity", "who knows", 0);

Timer t_tele;
Timeout expo_time;

float interrupt_T = 0.015;  // Something is very wrong here.

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

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

// =====
// Motor
// =====
const float motor_T = 1.0 / 100;                        // Frequency
float motor_duty = 0.0;                                 // Duty cycle
bool e_stop = false;                                    // Emergency stop
InterruptIn bemf_int(PTD4);                             // Back EMF measurement trigger
PwmOut motor(PTD4);                                     // Enable pin (PWM)
Timeout bemf_timeout;                                   // Back EMF interrupt delay
FastAnalogIn bemf(PTB3, 0);                             // Back EMF read pin
int bemf_vel = 0;                                       // Back EMF reading
float velocity = 0;                                     // Velocity
int motor_ctrl_count = 0;                               // Velocity control decimation counter
float Kmp = 12.0;                                       // Proportional factor
float Kmi = 0;                                          // Integral factor
float Kmd = 0;                                          // Derivative factor
float master_v = 0;                                     // Master velocity
float ref_v = 0;                                        // Reference velocity
PID motor_ctrl(Kmp, Kmi, Kmd, interrupt_T);             // Motor controller
int turn_thresh = 19;                                   // Minimum error to determine turn
float turn_gain = 0.06;                                 // Proportional gain for turning reference velocity
float turn_minv = 1.0;                                  // Minimum turning velocity

// =====
// Servo
// =====
float angle = 88;                                       // Angle
float Ksp = 1.5;                                        // Steering proportion
float Ksi = 9000000;                                    // Steering integral
float Ksd = 1.2;                                        // Steering derivative
PID servo_ctrl(Ksp, Ksi, Ksd, interrupt_T);             // Servo controller
Servo servo(PTA12);                                     // Signal pin (PWM)

// ======
// Camera
// ======
int fixed_t_int = 1100;                                 // Fixed exposure time
int t_int = 10000;                                      // Variable exposure time
bool rdyFlag = 1;                                       // Camera ready
bool dataFlag = 0;                                      // Data ready
const int T_INT_MAX = interrupt_T * 1000000 - 1000;     // Maximum exposure time
const int T_INT_MIN = 35;                               // Minimum exposure time
int img[128] = {0};                                     // Image data
DigitalOut clk(PTD0);                                   // Clock pin
DigitalOut si(PTD5);                                    // 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 rxCallback(MODSERIAL_IRQ_INFO *q) {
    MODSERIAL *serial = q->serial;
    if ( serial->rxGetLastChar() == '+') {
        master_v = master_v + 0.25;
        motor_ctrl.setSetPoint(master_v);
    }
    if ( serial->rxGetLastChar() == '-') {
        master_v = master_v - 0.25;
        motor_ctrl.setSetPoint(master_v);
    }
}

// Communications
void communication(void const *args) {
    telemetry_serial.baud(115200);
    telemetry_serial.attach(&rxCallback, 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("  [0] Change exposure (us)\r\n");
//        bt.printf("  [1] Steering\r\n");
//        bt.printf("  [2] Change Ksp\r\n");
//        bt.printf("  [3] Change Ksi\r\n");
//        bt.printf("  [4] Change Ksd\r\n");
//        bt.printf("  [5] Change turn slowing minimum velocity\r\n");
//        bt.printf("  [6] Change turn slowing gain\r\n");
//        bt.printf("  [7] Change turn slowing dead zone\r\n");
//        bt.printf("  [8] master_v += 0.05\r\n");
//        bt.printf("  [9] master_v -= 0.05\r\n");
//        comm_cmd = bt.getc();
//        while (comm_cmd != 'q') {
//            t.reset();
//            t.start();
//            switch(atoi(&comm_cmd)){
//                case 0:
////                    bt.printf("bemf: %d, Duty cycle: %f, Pulse count: %d, Velocity: %f, Kmp: %f, Kmi: %f, Kmd: %f\r\n", bemf_vel, motor_duty, curr_pulses, velocity, Kmp, Kmi, Kmd);
//                    bt.printf("Current: %d, New (8 digits): ", fixed_t_int);
//                    for (int i = 0; i < 8; i++) {
//                        comm_in[i] = bt.getc();
//                        bt.putc(comm_in[i]);
//                    }
//                    bt.printf("\r\n");
//                    fixed_t_int = atoi(comm_in);
//                    comm_cmd = 'q';
//                    break;
//                case 1:
//                    bt.printf("max: %d, angle: %f, center: %d, t_int: %d, rdyFlag: %d, dataFlag: %d\r\n", max, angle, center, t_int, rdyFlag, dataFlag);
//                    break;
//                case 2:
//                    bt.printf("Current: %f, New (8 digits): ", Ksp);
//                    for (int i = 0; i < 8; i++) {
//                        comm_in[i] = bt.getc();
//                        bt.putc(comm_in[i]);
//                    }
//                    bt.printf("\r\n");
//                    Ksp = atof(comm_in);
//                    servo_ctrl.setTunings(Ksp, Ksi, Ksd);
//                    servo_ctrl.reset();
//                    comm_cmd = 'q';
//                    break;
//                case 3:
//                    bt.printf("Current: %f, New (8 digits): ", Ksi);
//                    for (int i = 0; i < 8; i++) {
//                        comm_in[i] = bt.getc();
//                        bt.putc(comm_in[i]);
//                    }
//                    bt.printf("\r\n");
//                    Ksi = atof(comm_in);
//                    servo_ctrl.setTunings(Ksp, Ksi, Ksd);
//                    servo_ctrl.reset();
//                    comm_cmd = 'q';
//                    break;
//                case 4:
//                    bt.printf("Current: %f, New (8 digits): ", Ksd);
//                    for (int i = 0; i < 8; i++) {
//                        comm_in[i] = bt.getc();
//                        bt.putc(comm_in[i]);
//                    }
//                    bt.printf("\r\n");
//                    Ksd = atof(comm_in);
//                    servo_ctrl.setTunings(Ksp, Ksi, Ksd);
//                    servo_ctrl.reset();
//                    comm_cmd = 'q';
//                    break;
//                case 5:
//                    bt.printf("Current: %f, New (8 digits): ", turn_minv);
//                    for (int i = 0; i < 8; i++) {
//                        comm_in[i] = bt.getc();
//                        bt.putc(comm_in[i]);
//                    }
//                    bt.printf("\r\n");
//                    turn_minv = atof(comm_in);
//                    comm_cmd = 'q';
//                    break;
//                case 6:
//                    bt.printf("Current: %f, New (8 digits): ", turn_gain);
//                    for (int i = 0; i < 8; i++) {
//                        comm_in[i] = bt.getc();
//                        bt.putc(comm_in[i]);
//                    }
//                    bt.printf("\r\n");
//                    turn_gain = atof(comm_in);
//                    comm_cmd = 'q';
//                    break;
//                case 7:
//                    bt.printf("Current: %d, New (8 digits): ", turn_thresh);
//                    for (int i = 0; i < 8; i++) {
//                        comm_in[i] = bt.getc();
//                        bt.putc(comm_in[i]);
//                    }
//                    bt.printf("\r\n");
//                    turn_thresh = atoi(comm_in);
//                    comm_cmd = 'q';
//                    break;
//                case 8:
//                    master_v = master_v + 0.25;
//                    motor_ctrl.setSetPoint(master_v);
//                    bt.printf("%f\r\n", master_v);
//                    comm_cmd = 'q';
//                    break;
//                case 9:
//                    master_v = master_v - 0.25;
//                    motor_ctrl.setSetPoint(master_v);
//                    bt.printf("%f\r\n", master_v);
//                    comm_cmd = 'q';
//                    break;
//            }
//            if (bt.readable()) {
//                comm_cmd = bt.getc();
//            }
//        }
//       
//    }
//}

void cam_Read(){
    // Image capture
    clr_clk();
    set_si();
    set_clk();
    clr_si();
    
    for (int i = 0; i < 128; i++) {
        clr_clk();
        img[i] = ao.read_u16();
        tele_linescan[i] = img[i];
        set_clk();
    }
    dataFlag = 1;
}

void control() {
    if (rdyFlag){
        // Dummy read
        clr_clk();
        set_si();
        set_clk();
        clr_clk();
    
        for (int i = 0; i < 128; i++) {
            clr_clk();
            set_clk();
        }
    
        // Expose
        expo_time.attach_us(&cam_Read,t_int);
        rdyFlag = 0;
    }

    // Detect peak edges
    if(dataFlag){
        j = 0;
        for (int i = 0; i < 127 && j < 5; i++) {
            if (img[i] > max * 0.66) {
                left[j] = i;
                while (img[i] >  max * 0.66) {
                    i = i + 1;
                }
                right[j] = i;
                j = j + 1;
            }
        }
        
        // Calculate peak centers
        for (int i = 0; i < j; i++) {
            centers[i] = (left[i] + right[i]) / 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);
        }
        
        // 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;
        
        servo_ctrl.setProcessValue(center);
        angle = 88 + servo_ctrl.compute();
        servo = angle / 180;
        
        
        // AGC
        max = -1;
        for (int i = 0; i < 128; i++) {
            if (img[i] > max) {
                max = img[i];
            }
        }
        if (max > 60000) {
            t_int = t_int - 0.1 * (max - 60000);
        }
        if (max < 40000) {
            t_int = t_int + 0.1 * (40000 - max);
        }
        if (t_int < T_INT_MIN) {
            t_int = T_INT_MIN;
        }
        if (t_int > T_INT_MAX) {
            t_int = T_INT_MAX;
        }
        t_int = fixed_t_int;
        servo_ctrl.setInterval(t_int);
        rdyFlag = 1;
        dataFlag = 0;
    }
    
    // Velocity control
    if (motor_ctrl_count == 1000) {
        velocity = (40000 - bemf_vel) / 6000.0;
        tele_velocity = velocity;
        motor_ctrl_count = 0;
        motor_ctrl.setProcessValue(velocity);
        motor_duty = motor_ctrl.compute();
        motor = motor_duty;
        motor_ctrl_count = 0;
    } else {
        motor_ctrl_count = motor_ctrl_count + 1;
    }
    
    // Turn handling
    ref_v = master_v - master_v * turn_gain * (abs(64 - center) - turn_thresh);
    if (ref_v < turn_minv) {
        ref_v = turn_minv;
    }
    if (ref_v > master_v) {
        ref_v = master_v;
    }
    if (abs(motor_ctrl.getSetPoint() - ref_v) > 0.05) {
        motor_ctrl.setSetPoint(ref_v);
    }
}

// Back emf measuring interrupt
void meas_bemf() {
    bemf_vel = bemf.read_u16();
}

// Back emf trigger
void bemf_irq() {
    bemf_timeout.attach(&meas_bemf, 0.002);
}

// ====
// Main
// ====
int main() {
    t_tele.start();
    
    // Initialize motor
    motor.period(motor_T);
    motor = motor_duty;
    
    // Initialize motor controller
    motor_ctrl.setInputLimits(0.0, 10.0);
    motor_ctrl.setOutputLimits(0.05, 0.75);
    motor_ctrl.setSetPoint(master_v);
    motor_ctrl.setBias(0.0);
    motor_ctrl.setMode(1);
    
    // Initialize back EMF reader
    bemf_int.fall(&bemf_irq);
    
    // Initialize servo
    servo.calibrate(0.001, 45.0);
    servo = angle / 180.0;
    
    // Initialize servo controller
    servo_ctrl.setInputLimits(0, 127);
    servo_ctrl.setOutputLimits(-30, 30);
    servo_ctrl.setSetPoint(64);
    servo_ctrl.setBias(0.0);
    servo_ctrl.setMode(1);
    
    // Initialize communications thread
    Thread communication_thread(communication);
    
    // Run
    while (true) {
        control();
    }
}