Revision 18:2b7db50fec4c, committed 2016-04-14

Comitter:

vsutardja

Date:

Thu Apr 14 01:44:28 2016 +0000

Parent:

17:bf6192a361ab

Commit message:

fixed pwm, pid steering

Changed in this revision

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

--- a/main.cpp	Thu Apr 14 00:45:14 2016 +0000
+++ b/main.cpp	Thu Apr 14 01:44:28 2016 +0000
@@ -11,10 +11,8 @@
 // =========
 // Telemetry
 // =========
-//DigitalOut test(PTD1);
-
-MODSERIAL telemetry_serial(PTC4, PTC3);                 // TX, RX
-//MODSERIAL telemetry_serial(USBTX, USBRX);
+//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
 
@@ -28,11 +26,9 @@
 Timer t;
 Timer t_tele;
 Ticker control_interrupt;
-int t_cam1 = 0;
-int t_cam2 = 0;
+int t_cam = 0;
 int t_steer = 0;
 int t_vel = 0;
-int t_down = 0;
 
 float interrupt_T = 0.015;
 bool ctrl_flag = false;
@@ -41,12 +37,15 @@
 // Communication
 // =============
 char comm_cmd;                                          // Command
-char comm_in[5];                                        // Input
-Serial bt(USBTX, USBRX);                                // USB connection
-//Serial bt(PTC4, PTC3);                                  // BlueSMiRF connection
+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
 // =====
@@ -61,7 +60,6 @@
 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
@@ -69,19 +67,19 @@
 // ========
 // Velocity
 // ========
-float Kmp = 36.0;                                       // Proportional factor
-float Kmi = 1;                                          // Integral factor
-float Kmd = 0;                                          // Derivative factor
+float Kmp = 8.0;                                         // Proportional factor
+float Kmi = 0;                                           // 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
+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 = 1;                                          // Steering proportion
+float Ksp = 0.9;                                         // Steering proportion
 float Ksi = 0;
 float Ksd = 0;
 PID servo_ctrl(Ksp, Ksi, Ksd, interrupt_T);
@@ -91,7 +89,7 @@
 // Camera
 // ======
 int t_int = 10000;                                      // Exposure time
-const int T_INT_MAX = interrupt_T * 1000000 - 2000;     // Maximum 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
@@ -115,18 +113,19 @@
 // Functions
 // ================
 
-void killswitch(MODSERIAL_IRQ_INFO *q) {
+
+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.2;
+        ref_v = ref_v + 0.05;
         motor_ctrl.setSetPoint(ref_v);
     }
     if (serial->rxGetLastChar() == '-') {
-        ref_v = ref_v - 0.2;
+        ref_v = ref_v - 0.05;
         motor_ctrl.setSetPoint(ref_v);
     }
 }
@@ -134,7 +133,7 @@
 // Communications
 //void communication(void const *args) {
 //    telemetry_serial.baud(115200);
-//    telemetry_serial.attach(&killswitch, MODSERIAL::RxIrq);
+//    telemetry_serial.attach(&Kmillswitch, MODSERIAL::RxIrq);
 //    telemetry_obj.transmit_header();
 //    while (true) {
 //        tele_time_ms = t_tele.read_ms();
@@ -151,13 +150,18 @@
         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 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] 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)){
@@ -168,41 +172,74 @@
                     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 (5 digits): ", Kmp);
-                    for (int i = 0; i < 5; i++) {
+                    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");
-                    Kmp = atof(comm_in);
-                    motor_ctrl.setTunings(Kmp, Kmi, Kmd);
+                    Ksp = atof(comm_in);
+                    servo_ctrl.setTunings(Ksp, Ksi, Ksd);
                     comm_cmd = 'q';
                     break;
                 case 3:
-                    bt.printf("Current: %f, New (5 digits): ", Kmi);
-                    for (int i = 0; i < 5; i++) {
+                    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");
-                    Kmi = atof(comm_in);
-                    motor_ctrl.setTunings(Kmp, Kmi, Kmd);
+                    Ksd = atof(comm_in);
+                    motor_ctrl.setTunings(Ksp, Ksi, Ksd);
                     comm_cmd = 'q';
                     break;
-                case 4:
-                    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]);
-                    }
-                    bt.printf("\r\n");
-                    Kmd = atof(comm_in);
-                    motor_ctrl.setTunings(Kmp, Kmi, Kmd);
-                    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 (5 digits): ", Ksp);
-                    for (int i = 0; i < 5; i++) {
+                    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]);
                     }
@@ -211,8 +248,8 @@
                     comm_cmd = 'q';
                     break;
                 case 6:
-                    bt.printf("Current: %f, New (5 digits): ", ref_v);
-                    for (int i = 0; i < 5; i++) {
+                    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]);
                     }
@@ -222,12 +259,25 @@
                     comm_cmd = 'q';
                     break;
                 case 7:
-                    e_stop = true;
+//                    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:
-                    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);
+                    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()) {
@@ -239,8 +289,7 @@
 
 void control() {
     // Image capture
-    t_down = t.read_us();
-    t.reset();
+    // t.reset();
     
     // Dummy read
     PTD->PCOR = (0x01 << 5);
@@ -252,7 +301,6 @@
         PTD->PCOR = (0x01 << 5);
         PTD->PSOR = (0x01 << 5);
     }
-    t_cam1 = t.read_us();
     
     // Expose
     wait_us(t_int);
@@ -263,7 +311,6 @@
     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) {
@@ -273,66 +320,57 @@
         PTD->PSOR = (0x01 << 5);
     }
     
-    t_cam2 = t.read_us();
+    // t_cam = 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] > max * 0.65) {
+    for (int i = 0; i < 107 && j < 5; i++) {
+        if (img[i] > 45000) {
             left[j] = i;
-            while (img[i] > max * 0.65) {
-                if (i < 107) {
-                    i = i + 1;
-                } else {
-                    j = j - 1;
-                    break;
-                }
+            while (img[i] > 45000) {
+                i = i + 1;
             }
-            if (i < 107) {
-                right[j] = i;
-            }
+            right[j] = i;
             j = j + 1;
         }
     }
     
+    // Calculate peak centers
+    for (int i = 0; i < j; i++) {
+        centers[i] = (left[i] + right[i] + 10) / 2;
+    }
     
-    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);
+    // 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;
         }
-        
-        // 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;
@@ -355,43 +393,49 @@
     }
     tele_exposure = t_int;
     
-    t_steer = t.read_us();
-    
-    // wait_us(8000 - t.read_us());
-    
-    
+    // t_steer = t.read_us();
     
-    // 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;
+//    // 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_ctrl_flag() {
-//    test = 1;
+void set_control_flag() {
     ctrl_flag = true;
 }
 
@@ -404,13 +448,17 @@
     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.5);
+    motor_ctrl.setOutputLimits(0.0, 0.75);
     motor_ctrl.setSetPoint(ref_v);
     motor_ctrl.setBias(0.0);
     motor_ctrl.setMode(1);
@@ -419,23 +467,21 @@
     servo.calibrate(0.001, 45.0);
     servo = angle / 180.0;
     
-    // Initialize servo controller
-    servo_ctrl.setInputLimits(10, 117);
+    servo_ctrl.setInputLimits(10, 107);
     servo_ctrl.setOutputLimits(-25, 25);
     servo_ctrl.setSetPoint(63.5);
-    servo_ctrl.setBias(0);
+    servo_ctrl.setBias(0.0);
     servo_ctrl.setMode(1);
     
     // Initialize communications thread
-//    Thread communication_thread(communication);
-    
-    // control_interrupt.attach(control, interrupt_T);
-    // Thread::wait(osWaitForever);
+    Thread communication_thread(communication);
+
+    control_interrupt.attach(control, interrupt_T);
+//    control_interrupt.attach(set_control_flag, interrupt_T);
     
-    control_interrupt.attach(set_ctrl_flag, interrupt_T);
     while (true) {
-        if (ctrl_flag) {
-            control();
-        }
+//        if (ctrl_flag) {
+//            control();
+//        }
     }
 }
\ No newline at end of file