Diff: main.cpp

Revision:

1:32610c005260

Parent:

0:fcf070a88ba0

Child:

2:a8adff46eaca

--- a/main.cpp	Fri Mar 18 22:33:47 2016 +0000
+++ b/main.cpp	Tue Mar 29 22:00:52 2016 +0000
@@ -46,7 +46,7 @@
 // ========
 // Velocity
 // ========
-float Kp = 3.0;                                     // Proportional factor
+float Kp = 13.0;                                     // Proportional factor
 float Ki = 0;                                       // Integral factor
 float Kd = 0;                                       // Derivative factor
 float interval = 0.01;                              // Sampling interval
@@ -92,8 +92,6 @@
         bt.printf("Available diagnostics:\r\n");
         bt.printf("  [0] Velocity\r\n");
         bt.printf("  [1] Steering\r\n");
-        bt.printf("  [2] Change Kp\r\n");
-        bt.printf("  [3] Change exposure time\r\n");
         cmd = bt.getc();
         while (cmd != 'q') {
             switch(atoi(&cmd)){
@@ -103,30 +101,6 @@
                 case 1:
                     bt.printf("Servo angle: %f, Track center: %d, t_int: %d\r\n", a, center, t_int);
                     break;
-                case 2:
-//                    idx = 0;
-                    bt.printf("Current Kp = %f. Enter new Kp: ", Kp);
-//                    while ((cmd = bt.getc()) != '\n') {
-                    for (int idx = 0; idx < 2; idx++) {
-                        ch = bt.getc();
-                        in[idx] = ch;
-//                        idx = idx + 1;
-                    }
-                    bt.printf("\r\n");
-                    Kp = atof(in);
-                    break;
-                case 3:
-//                    idx = 0;
-                    bt.printf("Current t_int = %dms. Enter new t_int (ms): ", t_int / 1000);
-//                    while ((cmd = bt.getc()) != '\n') {
-                    for (int idx = 0; idx < 2; idx++) {
-                        ch = bt.getc();
-                        in[idx] = ch;
-//                        idx = idx + 1;
-                    }
-                    bt.printf("\r\n");
-                    t_int = atoi(in);
-                    break;
             }
             if (bt.readable()) {
                 cmd = bt.getc();
@@ -208,6 +182,8 @@
 int main() {
     // Initialize motor
     motor.period_us(T);
+    motor = 1.0;
+    wait(7);
     motor = 1.0 - d;
     
     // Initialize servo
@@ -216,12 +192,12 @@
     
     // Initialize & start camera
     clk = 0;
-    read_camera();
+//    read_camera();
     
     // Initialize motor controller
     motor_ctrl.setInputLimits(0.0, 10.0);
     motor_ctrl.setOutputLimits(0.1, 0.5);
-    motor_ctrl.setSetPoint(3.5);
+    motor_ctrl.setSetPoint(3);
     motor_ctrl.setBias(0.0);
     motor_ctrl.setMode(1);
     
@@ -232,6 +208,9 @@
     Thread communication_thread(communication);
     
     // Start motor controller
+//    motor = 0.85;
+    
+    
     while (true) {
         curr_pulses = qei.getPulses();
         velocity = (curr_pulses - prev_pulses)/ interval / ppr * c;
@@ -240,6 +219,5 @@
         d = motor_ctrl.compute();
         motor = 1.0 - d;
         wait(interval);
-        pc.printf("Velocity: %f\r\n", velocity);
     }
 }
\ No newline at end of file