Revision 28:4b76abe148cd, committed 2016-04-27

Comitter:

vsutardja

Date:

Wed Apr 27 22:24:11 2016 +0000

Parent:

27:e796e9ee0495

Commit message:

trying telemetry again

Changed in this revision

--- a/main.cpp	Tue Apr 26 06:11:46 2016 +0000
+++ b/main.cpp	Wed Apr 27 22:24:11 2016 +0000
@@ -7,6 +7,11 @@
 #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
@@ -18,23 +23,13 @@
 
 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<uint8_t> tele_center(telemetry_obj, "tele_center", "Center", "px", 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
@@ -46,76 +41,46 @@
 
 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
+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);
+InterruptIn bemf_int(PTD4);                             // Back EMF measurement trigger
 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
+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
-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);
+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;
-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;
+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 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;
-
+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
@@ -140,289 +105,188 @@
 // 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);
-//    }
-//}
+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(&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);
-    
+    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();
+    }
+}
 
-        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);
+//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");
-//                    Kmp = atof(comm_in);
-//                    motor_ctrl.setTunings(Kmp, Kmi, Kmd);
+//                    fixed_t_int = atoi(comm_in);
 //                    comm_cmd = 'q';
 //                    break;
-//                case 3:
-//                    bt.printf("Current: %f, New (8 digits): ", Kmi);
+//                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");
-//                    Kmi = atof(comm_in);
-//                    motor_ctrl.setTunings(Kmp, Kmi, Kmd);
+//                    Ksp = 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): ", Kmd);
+//                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");
-//                    Kmd = atof(comm_in);
-//                    motor_ctrl.setTunings(Kmp, Kmi, Kmd);
+//                    Ksi = 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): ", Ksp1);
+//                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");
-//                    Ksp1 = atof(comm_in);
-//                    servo_ctrl1.setTunings(Ksp1, Ksi1, Ksd1);
+//                    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 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): ", ref_v);
+//                    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");
-//                    ref_v = atof(comm_in);
-//                    motor_ctrl.setSetPoint(ref_v);
+//                    turn_thresh = atoi(comm_in);
 //                    comm_cmd = 'q';
 //                    break;
-//                case 7:
-//                    e_stop = true;
-//                    motor = 1.0;
-//                    bt.printf("STOPPED\r\n");
+//                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 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);
+//                case 9:
+//                    master_v = master_v - 0.25;
+//                    motor_ctrl.setSetPoint(master_v);
+//                    bt.printf("%f\r\n", master_v);
+//                    comm_cmd = 'q';
 //                    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;
+//            }
+//            if (bt.readable()) {
+//                comm_cmd = bt.getc();
+//            }
+//        }
+//       
+//    }
 //}
 
 void cam_Read(){
-    PTD->PCOR = (0x01);
-    PTD->PSOR = (0x01 << 5);
-    PTD->PSOR = (0x01);
-    PTD->PCOR = (0x01 << 5);
+    // Image capture
+    clr_clk();
+    set_si();
+    set_clk();
+    clr_si();
     
     for (int i = 0; i < 128; i++) {
-        PTD->PCOR = (0x01);
+        clr_clk();
         img[i] = ao.read_u16();
-//        tele_linescan[i] = img[i];
-        PTD->PSOR = (0x01);
+        tele_linescan[i] = img[i];
+        set_clk();
     }
     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);
+        // Dummy read
+        clr_clk();
+        set_si();
+        set_clk();
+        clr_clk();
     
         for (int i = 0; i < 128; i++) {
-            PTD->PCOR = (0x01);
-            PTD->PSOR = (0x01);
+            clr_clk();
+            set_clk();
         }
     
         // Expose
         expo_time.attach_us(&cam_Read,t_int);
         rdyFlag = 0;
-
     }
 
-    
     // Detect peak edges
     if(dataFlag){
         j = 0;
@@ -456,27 +320,10 @@
         }
         prev_center = center;
         center = centers[best_score_idx];
-//        tele_center = center;
+        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_ctrl.setProcessValue(center);
+        angle = 88 + servo_ctrl.compute();
         servo = angle / 180;
         
         
@@ -500,28 +347,25 @@
             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();
     }
     
+    // Velocity control
     if (motor_ctrl_count == 1000) {
         velocity = (40000 - bemf_vel) / 6000.0;
-//        motor_duty = motor_duty + 0.1;
-//        motor = 1.0 - motor_duty;
+        tele_velocity = velocity;
         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;
     }
     
+    // Turn handling
     ref_v = master_v - master_v * turn_gain * (abs(64 - center) - turn_thresh);
     if (ref_v < turn_minv) {
         ref_v = turn_minv;
@@ -532,70 +376,14 @@
     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;
-}
-
+// 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);
 }
@@ -604,16 +392,7 @@
 // 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;
-//    }
+    t_tele.start();
     
     // Initialize motor
     motor.period(motor_T);
@@ -626,30 +405,25 @@
     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(-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);
     
+    // Run
     while (true) {
-            control();
-        }
+        control();
+    }
 }
\ No newline at end of file