Revision 17:bf6192a361ab, committed 2016-04-14

Comitter:

vsutardja

Date:

Thu Apr 14 00:45:14 2016 +0000

Parent:

16:3ab3c4670f4f

Child:

18:2b7db50fec4c

Commit message:

Current iter

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PinDetect.lib	Thu Apr 14 00:45:14 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/AjK/code/PinDetect/#cb3afc45028b

--- a/main.cpp	Tue Apr 12 02:03:50 2016 +0000
+++ b/main.cpp	Thu Apr 14 00:45:14 2016 +0000
@@ -11,6 +11,8 @@
 // =========
 // Telemetry
 // =========
+//DigitalOut test(PTD1);
+
 MODSERIAL telemetry_serial(PTC4, PTC3);                 // TX, RX
 //MODSERIAL telemetry_serial(USBTX, USBRX);
 telemetry::MbedHal telemetry_hal(telemetry_serial);     // Hardware Abstraction Layer
@@ -26,9 +28,11 @@
 Timer t;
 Timer t_tele;
 Ticker control_interrupt;
-int t_cam = 0;
+int t_cam1 = 0;
+int t_cam2 = 0;
 int t_steer = 0;
 int t_vel = 0;
+int t_down = 0;
 
 float interrupt_T = 0.015;
 bool ctrl_flag = false;
@@ -37,15 +41,12 @@
 // Communication
 // =============
 char comm_cmd;                                          // Command
-char comm_in[8];                                        // Input
+char comm_in[5];                                        // 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
 // =====
@@ -60,6 +61,7 @@
 const int ppr = 389;                                    // Pulses per revolution
 const float c = 0.20106;                                // Wheel circumference
 int prev_pulses = 0;                                    // Previous pulse count
+int prev_pulse_counts[5] = {0};
 int curr_pulses = 0;                                    // Current pulse count
 float velocity = 0;                                     // Velocity
 QEI qei(PTD3, PTD2, NC, ppr, QEI::X4_ENCODING);         // Quadrature encoder
@@ -67,19 +69,19 @@
 // ========
 // Velocity
 // ========
-float Kmp = 8.0;                                         // Proportional factor
-float Kmi = 0;                                           // Integral factor
-float Kmd = 0;                                           // Derivative factor
-float motor_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
+float Kmp = 36.0;                                       // Proportional factor
+float Kmi = 1;                                          // Integral factor
+float Kmd = 0;                                          // Derivative factor
+float interval = 0.01;                                  // Sampling interval
+float ref_v = 0.5;                                      // Reference velocity
+int vel_count = 1;
+PID motor_ctrl(Kmp, Kmi, Kmd, interrupt_T);             // Motor controller
 
 // =====
 // Servo
 // =====
 float angle = 88;                                       // Angle
-float Ksp = 1.0;                                         // Steering proportion
+float Ksp = 1;                                          // Steering proportion
 float Ksi = 0;
 float Ksd = 0;
 PID servo_ctrl(Ksp, Ksi, Ksd, interrupt_T);
@@ -89,7 +91,7 @@
 // Camera
 // ======
 int t_int = 10000;                                      // Exposure time
-const int T_INT_MAX = interrupt_T * 1000000 - 1000;     // Maximum exposure time
+const int T_INT_MAX = interrupt_T * 1000000 - 2000;     // Maximum exposure time
 const int T_INT_MIN = 35;                               // Minimum exposure time
 int img[108] = {0};                                     // Image data
 DigitalOut clk(PTD5);                                   // Clock pin
@@ -113,19 +115,18 @@
 // Functions
 // ================
 
-
-void Kmillswitch(MODSERIAL_IRQ_INFO *q) {
+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;
+        ref_v = ref_v + 0.2;
         motor_ctrl.setSetPoint(ref_v);
     }
     if (serial->rxGetLastChar() == '-') {
-        ref_v = ref_v - 0.05;
+        ref_v = ref_v - 0.2;
         motor_ctrl.setSetPoint(ref_v);
     }
 }
@@ -133,7 +134,7 @@
 // Communications
 //void communication(void const *args) {
 //    telemetry_serial.baud(115200);
-//    telemetry_serial.attach(&Kmillswitch, MODSERIAL::RxIrq);
+//    telemetry_serial.attach(&killswitch, MODSERIAL::RxIrq);
 //    telemetry_obj.transmit_header();
 //    while (true) {
 //        tele_time_ms = t_tele.read_ms();
@@ -156,9 +157,7 @@
         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");
+        bt.printf("  [8] Timing\r\n");
         comm_cmd = bt.getc();
         while (comm_cmd != 'q') {
             switch(atoi(&comm_cmd)){
@@ -169,8 +168,8 @@
                     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): ", Kmp);
-                    for (int i = 0; i < 8; i++) {
+                    bt.printf("Current: %f, New (5 digits): ", Kmp);
+                    for (int i = 0; i < 5; i++) {
                         comm_in[i] = bt.getc();
                         bt.putc(comm_in[i]);
                     }
@@ -180,8 +179,8 @@
                     comm_cmd = 'q';
                     break;
                 case 3:
-                    bt.printf("Current: %f, New (8 digits): ", Kmi);
-                    for (int i = 0; i < 8; i++) {
+                    bt.printf("Current: %f, New (5 digits): ", Kmi);
+                    for (int i = 0; i < 5; i++) {
                         comm_in[i] = bt.getc();
                         bt.putc(comm_in[i]);
                     }
@@ -191,8 +190,8 @@
                     comm_cmd = 'q';
                     break;
                 case 4:
-                    bt.printf("Current: %f, New (8 digits): ", Kmd);
-                    for (int i = 0; i < 8; i++) {
+                    bt.printf("Current: %f, New (5 digits): ", Kmd);
+                    for (int i = 0; i < 5; i++) {
                         comm_in[i] = bt.getc();
                         bt.putc(comm_in[i]);
                     }
@@ -202,8 +201,8 @@
                     comm_cmd = 'q';
                     break;
                 case 5:
-                    bt.printf("Current: %f, New (8 digits): ", Ksp);
-                    for (int i = 0; i < 8; i++) {
+                    bt.printf("Current: %f, New (5 digits): ", Ksp);
+                    for (int i = 0; i < 5; i++) {
                         comm_in[i] = bt.getc();
                         bt.putc(comm_in[i]);
                     }
@@ -212,8 +211,8 @@
                     comm_cmd = 'q';
                     break;
                 case 6:
-                    bt.printf("Current: %f, New (8 digits): ", ref_v);
-                    for (int i = 0; i < 8; i++) {
+                    bt.printf("Current: %f, New (5 digits): ", ref_v);
+                    for (int i = 0; i < 5; i++) {
                         comm_in[i] = bt.getc();
                         bt.putc(comm_in[i]);
                     }
@@ -223,25 +222,12 @@
                     comm_cmd = 'q';
                     break;
                 case 7:
-//                    e_stop = true;
-                    motor = 1.0;
+                    e_stop = true;
                     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';
+                    bt.printf("Read 1: %dus, Exposure: %dus, Read 2: %dus, Steering: %dus, Velocity: %dus, Down: %dus, Total: %dus\r\n", t_cam1, t_int, t_cam2, t_steer, t_vel, t_down, t_cam1+t_int+t_cam2+t_steer+t_vel+t_down);
                     break;
             }
             if (bt.readable()) {
@@ -253,7 +239,8 @@
 
 void control() {
     // Image capture
-    // t.reset();
+    t_down = t.read_us();
+    t.reset();
     
     // Dummy read
     PTD->PCOR = (0x01 << 5);
@@ -265,6 +252,7 @@
         PTD->PCOR = (0x01 << 5);
         PTD->PSOR = (0x01 << 5);
     }
+    t_cam1 = t.read_us();
     
     // Expose
     wait_us(t_int);
@@ -275,6 +263,7 @@
     PTD->PSOR = (0x01 << 5);
     PTD->PCOR = (0x01);
     
+    t.reset();
     for (int i = 0; i < 128; i++) {
         PTD->PCOR = (0x01 << 5);
         if (i > 9 && i < 118) {
@@ -284,58 +273,66 @@
         PTD->PSOR = (0x01 << 5);
     }
     
-    // t_cam = t.read_us();
+    t_cam2 = t.read_us();
     
     // Steering control
-    // t.reset();
+    t.reset();
     
     // Detect peak edges
     j = 0;
     for (int i = 0; i < 108 && j < 5; i++) {
-        if (img[i] > 45000) {
+        if (img[i] > max * 0.65) {
             left[j] = i;
-            while (img[i] > 45000 && i < 108) {
-                i = i + 1;
+            while (img[i] > max * 0.65) {
+                if (i < 107) {
+                    i = i + 1;
+                } else {
+                    j = j - 1;
+                    break;
+                }
             }
-            right[j] = i;
+            if (i < 107) {
+                right[j] = i;
+            }
             j = j + 1;
         }
     }
     
-    // Calculate peak centers
-    for (int i = 0; i < j; i++) {
-        centers[i] = (left[i] + right[i] + 20) / 2;
-    }
     
-    // Assign scores
-    for (int i = 0; i < j; i++) {
-        scores[i] = 8 / (right[i] - left[i]) + img[centers[i]] / 65536 + 5 / 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;
+    if (j > 0) {
+        // Calculate peak centers
+        for (int i = 0; i < j; i++) {
+            centers[i] = (left[i] + right[i] + 20) / 2;
+        }
+        
+        // Assign scores
+        for (int i = 0; i < j; i++) {
+            scores[i] = 4 / (right[i] - left[i]) + img[centers[i]] / 65536 + 16 / 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;
+        
+        angle = 88 + (64 - center) * 0.9;
+        if (angle > 113) {
+            angle = 113;
+        }
+        if (angle < 63) {
+            angle = 63;
+        }
+        // servo_ctrl.setProcessValue(center);
+        // angle = 88 + servo_ctrl.compute();
+        // servo = floor(angle / 180 * 100 + 0.5) / 100;
+        servo = angle / 180;
     }
-    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;
@@ -358,49 +355,43 @@
     }
     tele_exposure = t_int;
     
-    // t_steer = t.read_us();
+    t_steer = t.read_us();
+    
+    // wait_us(8000 - 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;
+    // Velocity control
+    t.reset();
+    if (!e_stop) {
+        curr_pulses = qei.getPulses();
+        if (vel_count < 6) {
+            velocity = curr_pulses / interrupt_T / vel_count / ppr * c;
+            prev_pulse_counts[vel_count - 1] = curr_pulses;
+            vel_count = vel_count + 1;
+        } else {
+            velocity = (curr_pulses - prev_pulse_counts[0]) / interrupt_T / 5 / ppr * c;
+            for (int i = 0; i < 4; i++) {
+                prev_pulse_counts[i] = prev_pulse_counts[i+1];
+            }
+            prev_pulse_counts[4] = curr_pulses;
+        }
+        tele_velocity = velocity;
+        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();
+    t.reset();
+    ctrl_flag = false;
+//    test = 0;
 }
 
-void set_control_flag() {
+void set_ctrl_flag() {
+//    test = 1;
     ctrl_flag = true;
 }
 
@@ -413,17 +404,13 @@
     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.setOutputLimits(0.0, 0.5);
     motor_ctrl.setSetPoint(ref_v);
     motor_ctrl.setBias(0.0);
     motor_ctrl.setMode(1);
@@ -436,18 +423,19 @@
     servo_ctrl.setInputLimits(10, 117);
     servo_ctrl.setOutputLimits(-25, 25);
     servo_ctrl.setSetPoint(63.5);
-    servo_ctrl.setBias(0.0);
+    servo_ctrl.setBias(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);
+//    Thread communication_thread(communication);
+    
+    // control_interrupt.attach(control, interrupt_T);
+    // Thread::wait(osWaitForever);
     
+    control_interrupt.attach(set_ctrl_flag, interrupt_T);
     while (true) {
-//        if (ctrl_flag) {
-//            control();
-//        }
+        if (ctrl_flag) {
+            control();
+        }
     }
 }
\ No newline at end of file