File content as of revision 18:2b7db50fec4c:

#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, 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_cam = 0;
int t_steer = 0;
int t_vel = 0;

float interrupt_T = 0.015;
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 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 curr_pulses = 0;                                    // Current pulse count
float velocity = 0;                                     // Velocity
QEI qei(PTD3, PTD2, NC, ppr, QEI::X4_ENCODING);         // Quadrature encoder

// ========
// Velocity
// ========
float Kmp = 8.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.8;                                      // Reference velocity
PID motor_ctrl(Kmp, Kmi, Kmd, interrupt_T);                   // Motor controller

// =====
// Servo
// =====
float angle = 88;                                       // Angle
float Ksp = 0.9;                                         // 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 - 1000;     // 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 Kmillswitch(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();
//    }
//}

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("  [2] Change Ksp\r\n");
        bt.printf("  [3] Change Ksi\r\n");
        bt.printf("  [4] Change Ksd\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");
        bt.printf("  [8] duty += 0.05\r\n");
        bt.printf("  [9] duty -= 0.05\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 (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);
                    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);
                    motor_ctrl.setTunings(Ksp, Ksi, Ksd);
                    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);
                    motor_ctrl.setTunings(Ksp, Ksi, Ksd);
                    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): ", 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);
                    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 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:
                    motor_duty = motor_duty + 0.05;
                    motor = 1.0 - motor_duty;
                    bt.printf("%f\r\n", motor_duty);
                    comm_cmd = 'q';
                    break;
                case 9:
                    motor_duty = motor_duty - 0.05;
                    motor = 1.0 - motor_duty;
                    bt.printf("%f\r\n", motor_duty);
                    comm_cmd = 'q';
                    break;
            }
            if (bt.readable()) {
                comm_cmd = bt.getc();
            }
        }
    }
}

void control() {
    // Image capture
    // 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);
    }
    
    // Expose
    wait_us(t_int);
    
    // Read camera
    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);
        if (i > 9 && i < 118) {
            img[i-10] = ao.read_u16();
            tele_linescan[i-10] = img[i-10];
        }
        PTD->PSOR = (0x01 << 5);
    }
    
    // t_cam = t.read_us();
    
    // Steering control
    // t.reset();
    
    // Detect peak edges
    j = 0;
    for (int i = 0; i < 107 && j < 5; i++) {
        if (img[i] > 45000) {
            left[j] = i;
            while (img[i] > 45000) {
                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] + 10) / 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;
    servo_ctrl.setProcessValue(center);
    angle = 88 + servo_ctrl.compute();
    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();
    
//    // 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;
}

// ====
// Main
// ====
int main() {
    t.start();
    t_tele.start();
    tele_center.set_limits(0, 128);
    tele_pwm.set_limits(0.0, 1.0);
    
    for (int i = 0; i < 10; i++) {
        prev_vels[i] = -1;
    }
    
    // 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.75);
    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;
    
    servo_ctrl.setInputLimits(10, 107);
    servo_ctrl.setOutputLimits(-25, 25);
    servo_ctrl.setSetPoint(63.5);
    servo_ctrl.setBias(0.0);
    servo_ctrl.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) {
//        if (ctrl_flag) {
//            control();
//        }
    }
}