Diff: main.cpp

Revision:

13:d6e167698517

Parent:

12:54e7d8ff3a74

Child:

14:a0614f48e6ef

--- a/main.cpp	Fri Apr 08 21:40:35 2016 +0000
+++ b/main.cpp	Fri Apr 08 23:43:12 2016 +0000
@@ -42,6 +42,9 @@
 
 void communication(void const *args);                   // Communications
 
+//void kill(Arguments *input, Reply *output);
+//RPCFunction rpc_kill(&kill, "kill");
+
 // =====
 // Motor
 // =====
@@ -67,7 +70,7 @@
 float Ki = 0;                                           // Integral factor
 float Kd = 0;                                           // Derivative factor
 float interval = 0.01;                                  // Sampling interval
-float ref_v = 0.8;                                      // Reference velocity
+float ref_v = 0.5;                                      // Reference velocity
 PID motor_ctrl(Kp, Ki, Kd, interval);                   // Motor controller
 
 // =====
@@ -81,7 +84,7 @@
 // Camera
 // ======
 int t_int = 10000;                                      // Exposure time
-const int T_INT_MAX = interrupt_T * 1000000 - 10000;    // 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
@@ -105,15 +108,40 @@
 // Functions
 // ================
 
+
+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.2;
+        motor_ctrl.setSetPoint(ref_v);
+    }
+    if (serial->rxGetLastChar() == '-') {
+        ref_v = ref_v - 0.2;
+        motor_ctrl.setSetPoint(ref_v);
+    }
+}
+
 // Communications
 void communication(void const *args) {
     telemetry_serial.baud(115200);
+    telemetry_serial.attach(&killswitch, MODSERIAL::RxIrq);
     telemetry_obj.transmit_header();
     while (true) {
         tele_time_ms = t_tele.read_ms();
         telemetry_obj.do_io();
     }
 }
+ 
+//void kill(Arguments *input, Reply *output) {
+//    output->putData("E_STOP\r\n");
+//    motor = 1.0;
+//    e_stop = true;
+//}
+
 //void communication(void const *args) {
 //    // Initialize bluetooth
 //    bt.baud(115200);
@@ -213,44 +241,65 @@
     t.reset();
     
     // Dummy read
-    clk = 0;
-    wait_us(1);
-    si = 1;
-    wait_us(1);
-    clk = 1;
-    wait_us(1);
-    si = 0;
+//    clk = 0;
+    PTD->PCOR = (0x01 << 5);
+//    j = 0;
+//    wait_us(1);
+//    si = 1;
+    PTD->PSOR = (0x01);
+//    j= 0;
+//    wait_us(1);
+//    clk = 1;
+    PTD->PSOR = (0x01 << 5);
+//    j = 0;
+//    wait_us(1);
+//    si = 0;
+    PTD->PCOR = (0x01);
+//    j = 0;
     
     for (int i = 0; i < 128; i++) {
-        wait_us(1);
-        clk = 0;
-        wait_us(1);
-        clk = 1;
+//        wait_us(1);
+        PTD->PCOR = (0x01 << 5);
+//        j = 0;
+//        wait_us(1);
+        PTD->PSOR = (0x01 << 5);
+//        j = 0;
     }
     
     // Expose
     wait_us(t_int);
     
     // Read camera
-    clk = 0;
-    wait_us(1);
-    si = 1;
-    wait_us(1);
-    clk = 1;
-    wait_us(1);
-    si = 0;
-    wait_us(1);
+//    clk = 0;
+    PTD->PCOR = (0x01 << 5);
+//    j = 0;
+//    wait_us(1);
+//    si = 1;
+    PTD->PSOR = (0x01);
+//    j = 0;
+//    wait_us(1);
+//    clk = 1;
+    PTD->PSOR = (0x01 << 5);
+//    j = 0;
+//    wait_us(1);
+//    si = 0;
+    PTD->PCOR = (0x01);
+//    j = 0;
+//    wait_us(1);
     
     for (int i = 0; i < 128; i++) {
-        clk = 0;
+//        clk = 0;
+        PTD->PCOR = (0x01 << 5);
+//        j = 0;
         if (i > 9 && i < 118) {
             img[i-10] = ao.read_u16();
             tele_linescan[i-10] = img[i-10];
-        } else {
-            wait_us(1);
         }
-        clk = 1;
-        wait_us(1);
+        //} else {
+//            wait_us(1);
+//        }
+        PTD->PSOR = (0x01 << 5);
+//        j = 0;
     }
     
     t_cam = t.read_us();
@@ -338,6 +387,8 @@
         motor_duty = motor_ctrl.compute();
         motor = 1.0 - motor_duty;
         tele_pwm = motor_duty;
+    } else {
+        motor = 1.0;
     }
     t_vel = t.read_us();
 }