Diff: main.cpp

Revision:

15:db95bb7c7f93

Parent:

14:a0614f48e6ef

Child:

16:3ab3c4670f4f

--- a/main.cpp	Mon Apr 11 23:56:23 2016 +0000
+++ b/main.cpp	Tue Apr 12 00:13:56 2016 +0000
@@ -117,7 +117,7 @@
         e_stop = true;
         motor = 1.0;
     }
-    if (serial->rxGetLastChar() == '=') {
+    if (serial->rxGetLastChar() == '+') {
         ref_v = ref_v + 0.05;
         motor_ctrl.setSetPoint(ref_v);
     }
@@ -153,7 +153,9 @@
         bt.printf("  [5] Change Ks\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)){
@@ -218,12 +220,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("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);
+                    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()) {
@@ -338,44 +353,44 @@
     
     // 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 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;
-//            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 = (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() {
@@ -413,12 +428,12 @@
     // Initialize communications thread
     Thread communication_thread(communication);
 
-//    control_interrupt.attach(control, interrupt_T);
-    control_interrupt.attach(set_control_flag, interrupt_T);
+    control_interrupt.attach(control, interrupt_T);
+//    control_interrupt.attach(set_control_flag, interrupt_T);
     
     while (true) {
-        if (ctrl_flag) {
-            control();
-        }
+//        if (ctrl_flag) {
+//            control();
+//        }
     }
 }
\ No newline at end of file