Revision 27:e796e9ee0495, committed 2016-04-26

Comitter:

vsutardja

Date:

Tue Apr 26 06:11:46 2016 +0000

Parent:

26:722362964784

Child:

28:4b76abe148cd

Commit message:

Final race: 1.25m/s

Changed in this revision

--- a/main.cpp	Mon Apr 25 00:46:41 2016 +0000
+++ b/main.cpp	Tue Apr 26 06:11:46 2016 +0000
@@ -17,7 +17,7 @@
 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::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);
@@ -85,8 +85,8 @@
 float turn_minv = 1.0;
 PID motor_ctrl(Kmp, Kmi, Kmd, interrupt_T);                   // Motor controller
 
-int turn_thresh = 15;
-float turn_gain = 0.015;
+int turn_thresh = 19;
+float turn_gain = 0.06;
 float turn_minpwm = 0.1;
 
 FastAnalogIn bemf(PTB3, 0);
@@ -95,9 +95,9 @@
 // Servo
 // =====
 float angle = 88;                                       // Angle
-float Ksp = 0.8;                                         // Steering proportion
-float Ksi = 10000000;
-float Ksd = 0.0000001;
+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;
@@ -118,7 +118,7 @@
 
 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
+int img[128] = {0};                                     // Image data
 DigitalOut clk(PTD0);                                   // Clock pin
 DigitalOut si(PTD5);                                    // SI pin
 FastAnalogIn ao(PTC2);                                  // AO pin
@@ -141,21 +141,21 @@
 // ================
 
 
-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);
-    }
-}
+//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) {
@@ -168,6 +168,8 @@
 //    }
 //}
 
+int fixed_t_int = 1100;
+
 void communication(void const *args) {
     // Initialize bluetooth
     bt.baud(115200);
@@ -176,7 +178,8 @@
 
         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] 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");
@@ -199,10 +202,18 @@
             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("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("Servo angle: %f, Track center: %d, t_int: %d,rdyFlag: %d, dataFlag: %d, fired: %d, timer: %d\r\n", angle, center, t_int,rdyFlag,dataFlag,fired,t_coms);
+                    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);
@@ -360,15 +371,16 @@
             if (bt.readable()) {
                 comm_cmd = bt.getc();
             }
-        t_coms = t.read_us();
+//        t_coms = t.read_us();
         }
        
     }
 }
 
-void bugger(){
-    debug = !debug;
-}
+//void bugger(){
+//    debug = !debug;
+//}
+
 void cam_Read(){
     PTD->PCOR = (0x01);
     PTD->PSOR = (0x01 << 5);
@@ -377,23 +389,21 @@
     
     for (int i = 0; i < 128; i++) {
         PTD->PCOR = (0x01);
-        if (i > 9 && i < 118) {
-            img[i-10] = ao.read_u16();
-            tele_linescan[i-10] = img[i-10];
-        }
+        img[i] = ao.read_u16();
+//        tele_linescan[i] = img[i];
         PTD->PSOR = (0x01);
     }
     dataFlag = 1;
-    debugger.attach(bugger,0.2);
-    fired ++;
+//    debugger.attach(bugger,0.2);
+//    fired ++;
 }
 
 void control() {
     // Image capture
     //t.reset();
     //t.start();
-    DigitalOut debug2(LED_GREEN);
-    debug2 = 1;
+//    DigitalOut debug2(LED_GREEN);
+//    debug2 = 1;
     // Dummy read
     if (rdyFlag){
         PTD->PCOR = (0x01);
@@ -416,7 +426,7 @@
     // Detect peak edges
     if(dataFlag){
         j = 0;
-        for (int i = 0; i < 107 && j < 5; i++) {
+        for (int i = 0; i < 127 && j < 5; i++) {
             if (img[i] > max * 0.66) {
                 left[j] = i;
                 while (img[i] >  max * 0.66) {
@@ -429,7 +439,7 @@
         
         // Calculate peak centers
         for (int i = 0; i < j; i++) {
-            centers[i] = (left[i] + right[i] + 20) / 2;
+            centers[i] = (left[i] + right[i]) / 2;
         }
         
         // Assign scores
@@ -446,7 +456,7 @@
         }
         prev_center = center;
         center = centers[best_score_idx];
-        tele_center = center;
+//        tele_center = center;
         
         // Set servo angle
         //angle = 88 + (55 - center) * Ksp;
@@ -472,7 +482,7 @@
         
         // AGC
         max = -1;
-        for (int i = 0; i < 107; i++) {
+        for (int i = 0; i < 128; i++) {
             if (img[i] > max) {
                 max = img[i];
             }
@@ -480,8 +490,8 @@
         if (max > 60000) {
             t_int = t_int - 0.1 * (max - 60000);
         }
-        if (max < 50000) {
-            t_int = t_int + 0.1 * (50000 - max);
+        if (max < 40000) {
+            t_int = t_int + 0.1 * (40000 - max);
         }
         if (t_int < T_INT_MIN) {
             t_int = T_INT_MIN;
@@ -489,7 +499,8 @@
         if (t_int > T_INT_MAX) {
             t_int = T_INT_MAX;
         }
-        tele_exposure = t_int;
+        t_int = fixed_t_int;
+//        tele_exposure = t_int;
         servo_ctrl.setInterval(t_int);
         rdyFlag = 1;
         dataFlag = 0;
@@ -594,15 +605,15 @@
 // ====
 int main() {
     // t.start();
-    t_tele.start();
-    tele_center.set_limits(0, 128);
-    tele_pwm.set_limits(0.0, 1.0);
+//    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;
-    }
+//    for (int i = 0; i < 10; i++) {
+//        prev_vels[i] = -1;
+//    }
     
     // Initialize motor
     motor.period(motor_T);
@@ -619,19 +630,18 @@
     servo.calibrate(0.001, 45.0);
     servo = angle / 180.0;
     
-    servo_ctrl.setInputLimits(10, 107);
+    servo_ctrl.setInputLimits(0, 127);
     //servo_ctrl.setOutputLimits(-25, 25);
     servo_ctrl.setOutputLimits(-30, 30);
-    servo_ctrl.setSetPoint(63.5);
+    servo_ctrl.setSetPoint(64);
     servo_ctrl.setBias(0.0);
     servo_ctrl.setMode(1);
     
-    servo_ctrl1.setInputLimits(10, 107);
-    //servo_ctrl1.setOutputLimits(-25, 25);
-    servo_ctrl1.setOutputLimits(-30, 30);
-    servo_ctrl1.setSetPoint(63.5);
-    servo_ctrl1.setBias(0.0);
-    servo_ctrl1.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);