main.cpp

00001 #include "FastAnalogIn.h"
00002 #include "mbed.h"
00003 #include "PID.h"
00004 #include "QEI.h"
00005 #include "rtos.h"
00006 #include "SerialRPCInterface.h"
00007 #include "Servo.h"
00008 #include "telemetry.h"
00009 
00010 
00011 // =========
00012 // Telemetry
00013 // =========
00014 //MODSERIAL telemetry_serial(PTC4, PTC3);                 // TX, RX
00015 MODSERIAL telemetry_serial(USBTX, USBRX);
00016 telemetry::MbedHal telemetry_hal(telemetry_serial);     // Hardware Abstraction Layer
00017 telemetry::Telemetry telemetry_obj(telemetry_hal);      // Telemetry
00018 
00019 telemetry::Numeric<uint32_t> tele_time_ms(telemetry_obj, "time", "Time", "ms", 0);
00020 telemetry::NumericArray<uint16_t, 108> tele_linescan(telemetry_obj, "linescan", "Linescan", "ADC", 0);
00021 telemetry::Numeric<uint32_t> tele_exposure(telemetry_obj, "exposure", "Exposure", "us", 0);
00022 telemetry::Numeric<float> tele_velocity(telemetry_obj, "tele_vel", "Velocity", "who knows", 0);
00023 telemetry::Numeric<float> tele_pwm(telemetry_obj, "pwm", "PWM", "", 0);
00024 telemetry::Numeric<uint8_t> tele_center(telemetry_obj, "tele_center", "Center", "px", 0);
00025 
00026 Timer t;
00027 Timer t_tele;
00028 Ticker control_interrupt;
00029 int t_cam = 0;
00030 int t_steer = 0;
00031 int t_vel = 0;
00032 
00033 float interrupt_T = 0.015;
00034 bool ctrl_flag = false;
00035 
00036 // =============
00037 // Communication
00038 // =============
00039 char comm_cmd;                                          // Command
00040 char comm_in[8];                                        // Input
00041 //Serial bt(USBTX, USBRX);                                // USB connection
00042 Serial bt(PTC4, PTC3);                                  // BlueSMiRF connection
00043 
00044 void communication(void const *args);                   // Communications
00045 
00046 //void Kmill(Arguments *input, Reply *output);
00047 //RPCFunction rpc_Kmill(&Kmill, "Kmill");
00048 
00049 // =====
00050 // Motor
00051 // =====
00052 const int motor_T = 25000;                              // Frequency
00053 float motor_duty = 0.0;                                 // Duty cycle
00054 bool e_stop = false;                                    // Emergency stop
00055 PwmOut motor(PTA4);                                     // Enable pin (PWM)
00056 
00057 // =======
00058 // Encoder
00059 // =======
00060 const int ppr = 389;                                    // Pulses per revolution
00061 const float c = 0.20106;                                // Wheel circumference
00062 int prev_pulses = 0;                                    // Previous pulse count
00063 int curr_pulses = 0;                                    // Current pulse count
00064 float velocity = 0;                                     // Velocity
00065 QEI qei(PTD3, PTD2, NC, ppr, QEI::X4_ENCODING);         // Quadrature encoder
00066 
00067 // ========
00068 // Velocity
00069 // ========
00070 float Kmp = 8.0;                                         // Proportional factor
00071 float Kmi = 0;                                           // Integral factor
00072 float Kmd = 0;                                           // Derivative factor
00073 float interval = 0.01;                                  // Sampling interval
00074 float prev_vels[10];
00075 float ref_v = 0.8;                                      // Reference velocity
00076 PID motor_ctrl(Kmp, Kmi, Kmd, interrupt_T);                   // Motor controller
00077 
00078 // =====
00079 // Servo
00080 // =====
00081 float angle = 88;                                       // Angle
00082 float Ksp = 0.9;                                         // Steering proportion
00083 float Ksi = 0;
00084 float Ksd = 0;
00085 PID servo_ctrl(Ksp, Ksi, Ksd, interrupt_T);
00086 Servo servo(PTA12);                                     // Signal pin (PWM)
00087 
00088 // ======
00089 // Camera
00090 // ======
00091 int t_int = 10000;                                      // Exposure time
00092 const int T_INT_MAX = interrupt_T * 1000000 - 1000;     // Maximum exposure time
00093 const int T_INT_MIN = 35;                               // Minimum exposure time
00094 int img[108] = {0};                                     // Image data
00095 DigitalOut clk(PTD5);                                   // Clock pin
00096 DigitalOut si(PTD0);                                    // SI pin
00097 FastAnalogIn ao(PTC2);                                  // AO pin
00098 
00099 // ================
00100 // Image processing
00101 // ================
00102 int max = -1;                                           // Maximum luminosity
00103 int left[5] = {0};                                      // Left edge
00104 int right[5] = {0};                                     // Right edge
00105 int j = 0;                                              // Peaks index
00106 int center = 64;                                        // Center estimate
00107 int centers[5] = {0};                                   // Possible centers
00108 int prev_center = 64;                                   // Previous center
00109 float scores[5] = {0};                                  // Candidate scores
00110 int best_score_idx = 0;                                 // Most likely center index
00111 
00112 // ================
00113 // Functions
00114 // ================
00115 
00116 
00117 void Kmillswitch(MODSERIAL_IRQ_INFO *q) {
00118     MODSERIAL *serial = q->serial;
00119     if (serial->rxGetLastChar() == 'k') {
00120         e_stop = true;
00121         motor = 1.0;
00122     }
00123     if (serial->rxGetLastChar() == '+') {
00124         ref_v = ref_v + 0.05;
00125         motor_ctrl.setSetPoint(ref_v);
00126     }
00127     if (serial->rxGetLastChar() == '-') {
00128         ref_v = ref_v - 0.05;
00129         motor_ctrl.setSetPoint(ref_v);
00130     }
00131 }
00132 
00133 // Communications
00134 //void communication(void const *args) {
00135 //    telemetry_serial.baud(115200);
00136 //    telemetry_serial.attach(&Kmillswitch, MODSERIAL::RxIrq);
00137 //    telemetry_obj.transmit_header();
00138 //    while (true) {
00139 //        tele_time_ms = t_tele.read_ms();
00140 //        telemetry_obj.do_io();
00141 //    }
00142 //}
00143 
00144 void communication(void const *args) {
00145     // Initialize bluetooth
00146     bt.baud(115200);
00147     
00148     while (true) {
00149         bt.printf("\r\nPress q to return to this prompt.\r\n");
00150         bt.printf("Available diagnostics:\r\n");
00151         bt.printf("  [0] Velocity\r\n");
00152         bt.printf("  [1] Steering\r\n");
00153 //        bt.printf("  [2] Change Kmp\r\n");
00154 //        bt.printf("  [3] Change Kmi\r\n");
00155 //        bt.printf("  [4] Change Kmd\r\n");
00156         bt.printf("  [2] Change Ksp\r\n");
00157         bt.printf("  [3] Change Ksi\r\n");
00158         bt.printf("  [4] Change Ksd\r\n");
00159         bt.printf("  [5] Change Ksp\r\n");
00160         bt.printf("  [6] Change reference velocity\r\n");
00161         bt.printf("  [7] EMERGENCY STOP\r\n");
00162 //        bt.printf("  [8] Timing\r\n");
00163         bt.printf("  [8] duty += 0.05\r\n");
00164         bt.printf("  [9] duty -= 0.05\r\n");
00165         comm_cmd = bt.getc();
00166         while (comm_cmd != 'q') {
00167             switch(atoi(&comm_cmd)){
00168                 case 0:
00169                     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);
00170                     break;
00171                 case 1:
00172                     bt.printf("Servo angle: %f, Track center: %d, t_int: %d\r\n", angle, center, t_int);
00173                     break;
00174                 case 2:
00175                     bt.printf("Current: %f, New (8 digits): ", Ksp);
00176                     for (int i = 0; i < 8; i++) {
00177                         comm_in[i] = bt.getc();
00178                         bt.putc(comm_in[i]);
00179                     }
00180                     bt.printf("\r\n");
00181                     Ksp = atof(comm_in);
00182                     servo_ctrl.setTunings(Ksp, Ksi, Ksd);
00183                     comm_cmd = 'q';
00184                     break;
00185                 case 3:
00186                     bt.printf("Current: %f, New (8 digits): ", Ksi);
00187                     for (int i = 0; i < 8; i++) {
00188                         comm_in[i] = bt.getc();
00189                         bt.putc(comm_in[i]);
00190                     }
00191                     bt.printf("\r\n");
00192                     Ksi = atof(comm_in);
00193                     motor_ctrl.setTunings(Ksp, Ksi, Ksd);
00194                     comm_cmd = 'q';
00195                     break;
00196                 case 4:
00197                     bt.printf("Current: %f, New (8 digits): ", Ksd);
00198                     for (int i = 0; i < 8; i++) {
00199                         comm_in[i] = bt.getc();
00200                         bt.putc(comm_in[i]);
00201                     }
00202                     bt.printf("\r\n");
00203                     Ksd = atof(comm_in);
00204                     motor_ctrl.setTunings(Ksp, Ksi, Ksd);
00205                     comm_cmd = 'q';
00206                     break;
00207 //                case 2:
00208 //                    bt.printf("Current: %f, New (8 digits): ", Kmp);
00209 //                    for (int i = 0; i < 8; i++) {
00210 //                        comm_in[i] = bt.getc();
00211 //                        bt.putc(comm_in[i]);
00212 //                    }
00213 //                    bt.printf("\r\n");
00214 //                    Kmp = atof(comm_in);
00215 //                    motor_ctrl.setTunings(Kmp, Kmi, Kmd);
00216 //                    comm_cmd = 'q';
00217 //                    break;
00218 //                case 3:
00219 //                    bt.printf("Current: %f, New (8 digits): ", Kmi);
00220 //                    for (int i = 0; i < 8; i++) {
00221 //                        comm_in[i] = bt.getc();
00222 //                        bt.putc(comm_in[i]);
00223 //                    }
00224 //                    bt.printf("\r\n");
00225 //                    Kmi = atof(comm_in);
00226 //                    motor_ctrl.setTunings(Kmp, Kmi, Kmd);
00227 //                    comm_cmd = 'q';
00228 //                    break;
00229 //                case 4:
00230 //                    bt.printf("Current: %f, New (8 digits): ", Kmd);
00231 //                    for (int i = 0; i < 8; i++) {
00232 //                        comm_in[i] = bt.getc();
00233 //                        bt.putc(comm_in[i]);
00234 //                    }
00235 //                    bt.printf("\r\n");
00236 //                    Kmd = atof(comm_in);
00237 //                    motor_ctrl.setTunings(Kmp, Kmi, Kmd);
00238 //                    comm_cmd = 'q';
00239 //                    break;
00240                 case 5:
00241                     bt.printf("Current: %f, New (8 digits): ", Ksp);
00242                     for (int i = 0; i < 8; i++) {
00243                         comm_in[i] = bt.getc();
00244                         bt.putc(comm_in[i]);
00245                     }
00246                     bt.printf("\r\n");
00247                     Ksp = atof(comm_in);
00248                     comm_cmd = 'q';
00249                     break;
00250                 case 6:
00251                     bt.printf("Current: %f, New (8 digits): ", ref_v);
00252                     for (int i = 0; i < 8; i++) {
00253                         comm_in[i] = bt.getc();
00254                         bt.putc(comm_in[i]);
00255                     }
00256                     bt.printf("\r\n");
00257                     ref_v = atof(comm_in);
00258                     motor_ctrl.setSetPoint(ref_v);
00259                     comm_cmd = 'q';
00260                     break;
00261                 case 7:
00262 //                    e_stop = true;
00263                     motor = 1.0;
00264                     bt.printf("STOPPED\r\n");
00265                     comm_cmd = 'q';
00266                     break;
00267 //                case 8:
00268 //                    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);
00269 //                    break;
00270                 case 8:
00271                     motor_duty = motor_duty + 0.05;
00272                     motor = 1.0 - motor_duty;
00273                     bt.printf("%f\r\n", motor_duty);
00274                     comm_cmd = 'q';
00275                     break;
00276                 case 9:
00277                     motor_duty = motor_duty - 0.05;
00278                     motor = 1.0 - motor_duty;
00279                     bt.printf("%f\r\n", motor_duty);
00280                     comm_cmd = 'q';
00281                     break;
00282             }
00283             if (bt.readable()) {
00284                 comm_cmd = bt.getc();
00285             }
00286         }
00287     }
00288 }
00289 
00290 void control() {
00291     // Image capture
00292     // t.reset();
00293     
00294     // Dummy read
00295     PTD->PCOR = (0x01 << 5);
00296     PTD->PSOR = (0x01);
00297     PTD->PSOR = (0x01 << 5);
00298     PTD->PCOR = (0x01);
00299     
00300     for (int i = 0; i < 128; i++) {
00301         PTD->PCOR = (0x01 << 5);
00302         PTD->PSOR = (0x01 << 5);
00303     }
00304     
00305     // Expose
00306     wait_us(t_int);
00307     
00308     // Read camera
00309     PTD->PCOR = (0x01 << 5);
00310     PTD->PSOR = (0x01);
00311     PTD->PSOR = (0x01 << 5);
00312     PTD->PCOR = (0x01);
00313     
00314     for (int i = 0; i < 128; i++) {
00315         PTD->PCOR = (0x01 << 5);
00316         if (i > 9 && i < 118) {
00317             img[i-10] = ao.read_u16();
00318             tele_linescan[i-10] = img[i-10];
00319         }
00320         PTD->PSOR = (0x01 << 5);
00321     }
00322     
00323     // t_cam = t.read_us();
00324     
00325     // Steering control
00326     // t.reset();
00327     
00328     // Detect peak edges
00329     j = 0;
00330     for (int i = 0; i < 107 && j < 5; i++) {
00331         if (img[i] > 45000) {
00332             left[j] = i;
00333             while (img[i] > 45000) {
00334                 i = i + 1;
00335             }
00336             right[j] = i;
00337             j = j + 1;
00338         }
00339     }
00340     
00341     // Calculate peak centers
00342     for (int i = 0; i < j; i++) {
00343         centers[i] = (left[i] + right[i] + 10) / 2;
00344     }
00345     
00346     // Assign scores
00347     for (int i = 0; i < j; i++) {
00348         scores[i] = 10 / (right[i] - left[i]) + img[centers[i]] / 65536 + 10 / abs(centers[i] - prev_center);
00349     }
00350     
00351     // Choose most likely center
00352     best_score_idx = 0;
00353     for (int i = 0; i < j; i++) {
00354         if (scores[i] > scores[best_score_idx]) {
00355             best_score_idx = i;
00356         }
00357     }
00358     prev_center = center;
00359     center = centers[best_score_idx];
00360     tele_center = center;
00361     
00362     // Set servo angle
00363     //angle = 88 + (55 - center) * Ksp;
00364 //    if (angle > 113) {
00365 //        angle = 113;
00366 //    }
00367 //    if (angle < 63) {
00368 //        angle = 63;
00369 //    }
00370 //    servo = angle / 180;
00371     servo_ctrl.setProcessValue(center);
00372     angle = 88 + servo_ctrl.compute();
00373     servo = angle / 180;
00374     
00375     // AGC
00376     max = -1;
00377     for (int i = 0; i < 107; i++) {
00378         if (img[i] > max) {
00379             max = img[i];
00380         }
00381     }
00382     if (max > 60000) {
00383         t_int = t_int - 0.1 * (max - 60000);
00384     }
00385     if (max < 50000) {
00386         t_int = t_int + 0.1 * (50000 - max);
00387     }
00388     if (t_int < T_INT_MIN) {
00389         t_int = T_INT_MIN;
00390     }
00391     if (t_int > T_INT_MAX) {
00392         t_int = T_INT_MAX;
00393     }
00394     tele_exposure = t_int;
00395     
00396     // t_steer = t.read_us();
00397     
00398 //    // Velocity control
00399 //    // t.reset();
00400 //    if (!e_stop) {
00401 //        curr_pulses = qei.getPulses();
00402 //        //for (int i = 0; i < 9; i++) {
00403 ////            prev_vels[i] = prev_vels[i+1];
00404 ////        }
00405 ////        prev_vels[9] = (curr_pulses - prev_pulses) / t.read() / ppr * c;
00406 ////        t.reset();
00407 ////        if (prev_vels[9] < 0) {
00408 ////            prev_vels[9] = 0;
00409 ////        }
00410 ////        if (prev_vels[0] < 0) {
00411 ////            velocity = prev_vels[9];
00412 ////        } else {
00413 ////            velocity = 0;
00414 ////            for (int i = 0; i < 10; i++) {
00415 ////                velocity = velocity + prev_vels[i];
00416 ////                velocity = velocity / 10;
00417 ////            }
00418 ////        }
00419 //        velocity = (curr_pulses - prev_pulses) / interrupt_T / ppr * c;
00420 //        if (velocity < 0) {
00421 //            velocity = 0;
00422 //        }
00423 ////        velocity = (curr_pulses - prev_pulses) / t.read() / ppr * c;
00424 //        t.reset();
00425 //        tele_velocity = velocity;
00426 //        prev_pulses = curr_pulses;
00427 //        motor_ctrl.setProcessValue(velocity);
00428 //        motor_duty = motor_ctrl.compute();
00429 //        motor = 1.0 - motor_duty;
00430 //        tele_pwm = motor_duty;
00431 //    } else {
00432 //        motor = 1.0;
00433 //    }
00434 //    // t_vel = t.read_us();
00435 //    ctrl_flag = false;
00436 }
00437 
00438 void set_control_flag() {
00439     ctrl_flag = true;
00440 }
00441 
00442 // ====
00443 // Main
00444 // ====
00445 int main() {
00446     t.start();
00447     t_tele.start();
00448     tele_center.set_limits(0, 128);
00449     tele_pwm.set_limits(0.0, 1.0);
00450     
00451     for (int i = 0; i < 10; i++) {
00452         prev_vels[i] = -1;
00453     }
00454     
00455     // Initialize motor
00456     motor.period_us(motor_T);
00457     motor = 1.0 - motor_duty;
00458     
00459     // Initialize motor controller
00460     motor_ctrl.setInputLimits(0.0, 10.0);
00461     motor_ctrl.setOutputLimits(0.0, 0.75);
00462     motor_ctrl.setSetPoint(ref_v);
00463     motor_ctrl.setBias(0.0);
00464     motor_ctrl.setMode(1);
00465     
00466     // Initialize servo
00467     servo.calibrate(0.001, 45.0);
00468     servo = angle / 180.0;
00469     
00470     servo_ctrl.setInputLimits(10, 107);
00471     servo_ctrl.setOutputLimits(-25, 25);
00472     servo_ctrl.setSetPoint(63.5);
00473     servo_ctrl.setBias(0.0);
00474     servo_ctrl.setMode(1);
00475     
00476     // Initialize communications thread
00477     Thread communication_thread(communication);
00478 
00479     control_interrupt.attach(control, interrupt_T);
00480 //    control_interrupt.attach(set_control_flag, interrupt_T);
00481     
00482     while (true) {
00483 //        if (ctrl_flag) {
00484 //            control();
00485 //        }
00486     }
00487 }