File content as of revision 27:e796e9ee0495:

#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
// =========
//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<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;
Timeout expo_time;
int t_cam = 0;
int t_steer = 0;
int t_vel = 0;
int t_main = 0;
int t_coms = 0;

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

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

//void Kmill(Arguments *input, Reply *output);
//RPCFunction rpc_Kmill(&Kmill, "Kmill");

// =====
// Motor
// =====
const float motor_T = 1.0 / 100;                              // Frequency
float motor_duty = 0.0;                                 // Duty cycle
float ref_pwm = 0.0;
bool e_stop = false;                                    // Emergency stop
InterruptIn bemf_int(PTD4);
PwmOut motor(PTD4);                                     // Enable pin (PWM)
Timeout bemf_timeout;
int bemf_vel = 0;
int motor_ctrl_count = 0;

// =======
// Encoder
// =======
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 Kmp = 12.0;                                         // Proportional factor
float Kmi = 0;                                           // Integral factor
float Kmd = 0;                                           // Derivative factor
float interval = 0.01;                                  // Sampling interval
float prev_vels[10];
float ref_v = 0;                                      // Reference velocity
float master_v = 0;
float turn_minv = 1.0;
PID motor_ctrl(Kmp, Kmi, Kmd, interrupt_T);                   // Motor controller

int turn_thresh = 19;
float turn_gain = 0.06;
float turn_minpwm = 0.1;

FastAnalogIn bemf(PTB3, 0);

// =====
// Servo
// =====
float angle = 88;                                       // Angle
float Ksp = 1.5;                                         // Steering proportion
float Ksi = 9000000;
float Ksd = 1.2;
PID servo_ctrl(Ksp, Ksi, Ksd, interrupt_T);
float Ksp1 = 0.9;
float Ksi1 = 0;
float Ksd1 = 0;
PID servo_ctrl1(Ksp1, Ksi1, Ksd1, interrupt_T);
int ctrl_switch = 15;
Servo servo(PTA12);                                     // Signal pin (PWM)

// ======
// Camera
// ======
int t_int = 10000;                                      // Exposure time
bool rdyFlag = 1;                                       // Signal when camera is ready again
bool dataFlag = 0;                                       // Signal when data is ready again
DigitalOut debug(LED_BLUE);
Timeout debugger;
int fired = 0;

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 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.05;
//        motor_ctrl.setSetPoint(ref_v);
//    }
//    if (serial->rxGetLastChar() == '-') {
//        ref_v = ref_v - 0.05;
//        motor_ctrl.setSetPoint(ref_v);
//    }
//}

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

int fixed_t_int = 1100;

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 Kmp\r\n");
//        bt.printf("  [3] Change Kmi\r\n");
//        bt.printf("  [4] Change Kmd\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 Ksp1\r\n");
        bt.printf("  [5] Change turn slowing minimum velocity\r\n");
//        bt.printf("  [6] Change reference velocity\r\n");
        bt.printf("  [6] Change turn slowing gain\r\n");
//        bt.printf("  [7] EMERGENCY STOP\r\n");
        bt.printf("  [7] Change turn slowing dead zone\r\n");
//        bt.printf("  [8] Timing\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, Servo angle: %f, Track center: %d, t_int: %d,rdyFlag: %d, dataFlag: %d, fired: %d, timer: %d\r\n", max, angle, center, t_int,rdyFlag,dataFlag,fired,t_coms);
                    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 2:
//                    bt.printf("Current: %f, New (8 digits): ", Kmp);
//                    for (int i = 0; i < 8; 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 (8 digits): ", Kmi);
//                    for (int i = 0; i < 8; 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 (8 digits): ", Kmd);
//                    for (int i = 0; i < 8; 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 (8 digits): ", Ksp1);
//                    for (int i = 0; i < 8; i++) {
//                        comm_in[i] = bt.getc();
//                        bt.putc(comm_in[i]);
//                    }
//                    bt.printf("\r\n");
//                    Ksp1 = atof(comm_in);
//                    servo_ctrl1.setTunings(Ksp1, Ksi1, Ksd1);
//                    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 6:
//                    bt.printf("Current: %f, New (8 digits): ", ref_v);
//                    for (int i = 0; i < 8; 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;
//                    motor = 1.0;
//                    bt.printf("STOPPED\r\n");
//                    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:
//                    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;
                case 8:
                    master_v = master_v + 0.25;
                    motor_ctrl.setSetPoint(master_v);
//                    motor_duty = motor_duty + 0.05;
//                    ref_pwm = ref_pwm + 0.05;
//                    motor = 1.0 - motor_duty;
//                    bt.printf("%f\r\n", motor_duty);
//                    bt.printf("%f\r\n", ref_pwm);
                    bt.printf("%f\r\n", master_v);
                    comm_cmd = 'q';
                    break;
                case 9:
                    master_v = master_v - 0.25;
                    motor_ctrl.setSetPoint(master_v);
//                    motor_duty = motor_duty - 0.05;
//                    ref_pwm = ref_pwm - 0.05;
//                    motor = 1.0 - motor_duty;
//                    bt.printf("%f\r\n", motor_duty);
//                    bt.printf("%f\r\n", ref_pwm);
                    bt.printf("%f\r\n", master_v);
                    comm_cmd = 'q';
                    break;
            }
            if (bt.readable()) {
                comm_cmd = bt.getc();
            }
//        t_coms = t.read_us();
        }
       
    }
}

//void bugger(){
//    debug = !debug;
//}

void cam_Read(){
    PTD->PCOR = (0x01);
    PTD->PSOR = (0x01 << 5);
    PTD->PSOR = (0x01);
    PTD->PCOR = (0x01 << 5);
    
    for (int i = 0; i < 128; i++) {
        PTD->PCOR = (0x01);
        img[i] = ao.read_u16();
//        tele_linescan[i] = img[i];
        PTD->PSOR = (0x01);
    }
    dataFlag = 1;
//    debugger.attach(bugger,0.2);
//    fired ++;
}

void control() {
    // Image capture
    //t.reset();
    //t.start();
//    DigitalOut debug2(LED_GREEN);
//    debug2 = 1;
    // Dummy read
    if (rdyFlag){
        PTD->PCOR = (0x01);
        PTD->PSOR = (0x01 << 5);
        PTD->PSOR = (0x01);
        PTD->PCOR = (0x01 << 5);
    
        for (int i = 0; i < 128; i++) {
            PTD->PCOR = (0x01);
            PTD->PSOR = (0x01);
        }
    
        // 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;
        
        // Set servo angle
        //angle = 88 + (55 - center) * Ksp;
    //    if (angle > 113) {
    //        angle = 113;
    //    }
    //    if (angle < 63) {
    //        angle = 63;
    //    }
    //    servo = angle / 180;
    
     //   if (abs(center - 64) > ctrl_switch) {
//            servo_ctrl1.setProcessValue(center);
//            angle = 88+servo_ctrl1.compute();
//            servo_ctrl.reset();
//        } else {
            servo_ctrl.setProcessValue(center);
            angle = 88 + servo_ctrl.compute();
//            servo_ctrl1.reset();
       // }
        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;
//        tele_exposure = t_int;
        servo_ctrl.setInterval(t_int);
        rdyFlag = 1;
        dataFlag = 0;
        //t_main = t.read_us();
    }
    
    if (motor_ctrl_count == 1000) {
        velocity = (40000 - bemf_vel) / 6000.0;
//        motor_duty = motor_duty + 0.1;
//        motor = 1.0 - motor_duty;
        motor_ctrl_count = 0;
        motor_ctrl.setProcessValue(velocity);
        motor_duty = motor_ctrl.compute();
        motor = motor_duty;
        motor_ctrl_count = 0;
//    }
//    motor_ctrl_count = motor_ctrl_count + 1;
    } else {
        motor_ctrl_count = motor_ctrl_count + 1;
    }
    
    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);
    }
    
//    motor_duty =  ref_pwm - ref_pwm * turn_gain * (abs(64 - center) - turn_thresh);
//    if (motor_duty > ref_pwm) {
//        motor_duty = ref_pwm;
//    }
//    if (motor_duty < turn_minpwm) {
//        motor_duty = turn_minpwm;
//    }
//    if (abs(1.0 - motor.read() - motor_duty) > 0.01) {
//        motor = 1.0 - motor_duty;
//    }

    // t_steer = t.read_us();
    
    
    
//    // Velocity control
//    // t.reset();
//    if (!e_stop) {
//        curr_pulses = qei.getPulses();
//        //for (int i = 0; i < 9; i++) {
////            prev_vels[i] = prev_vels[i+1];
////        }
////        prev_vels[9] = (curr_pulses - prev_pulses) / t.read() / ppr * c;
////        t.reset();
////        if (prev_vels[9] < 0) {
////            prev_vels[9] = 0;
////        }
////        if (prev_vels[0] < 0) {
////            velocity = prev_vels[9];
////        } else {
////            velocity = 0;
////            for (int i = 0; i < 10; i++) {
////                velocity = velocity + prev_vels[i];
////                velocity = velocity / 10;
////            }
////        }
//        velocity = (curr_pulses - prev_pulses) / interrupt_T / ppr * c;
//        if (velocity < 0) {
//            velocity = 0;
//        }
////        velocity = (curr_pulses - prev_pulses) / t.read() / ppr * c;
//        t.reset();
//        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;
//    } else {
//        motor = 1.0;
//    }
//    // t_vel = t.read_us();
//    ctrl_flag = false;
}

void set_control_flag() {
    ctrl_flag = true;
}

void meas_bemf() {
    bemf_vel = bemf.read_u16();
}

void bemf_irq() {
    bemf_timeout.attach(&meas_bemf, 0.002);
}

// ====
// Main
// ====
int main() {
    // t.start();
//    t_tele.start();
//    tele_center.set_limits(0, 128);
//    tele_pwm.set_limits(0.0, 1.0);
    
    bemf_int.fall(&bemf_irq);
    
//    for (int i = 0; i < 10; i++) {
//        prev_vels[i] = -1;
//    }
    
    // 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 servo
    servo.calibrate(0.001, 45.0);
    servo = angle / 180.0;
    
    servo_ctrl.setInputLimits(0, 127);
    //servo_ctrl.setOutputLimits(-25, 25);
    servo_ctrl.setOutputLimits(-30, 30);
    servo_ctrl.setSetPoint(64);
    servo_ctrl.setBias(0.0);
    servo_ctrl.setMode(1);
    
//    servo_ctrl1.setInputLimits(10, 107);
//    servo_ctrl1.setOutputLimits(-30, 30);
//    servo_ctrl1.setSetPoint(64);
//    servo_ctrl1.setBias(0.0);
//    servo_ctrl1.setMode(1);
    
    // Initialize communications thread
    Thread communication_thread(communication);

//    control_interrupt.attach(control, interrupt_T);
//    control_interrupt.attach(set_control_flag, interrupt_T);
    
    while (true) {
            control();
        }
}