Diff: main.cpp

Revision:

40:10e8e80af7da

Parent:

39:7b28ee39185d

Child:

41:d74878640739

--- a/main.cpp	Fri Jan 13 10:21:07 2017 +0000
+++ b/main.cpp	Sat Jan 21 11:59:20 2017 +0000
@@ -48,6 +48,8 @@
     //Initialise/reset PID variables
     initVariables();
     initSpeedSensors();
+    
+    //buttonStartup();
               
     while(1) {
         
@@ -95,6 +97,26 @@
     }
 }
 
+void buttonStartup() {
+    while(1) {
+        handleComms();
+        
+        // 2 bit is broken = max value is 13
+        uint8_t dip = TFC_GetDIP_Switch();
+        
+        // 1 on press, 0 otherwise
+        uint8_t button_a = TFC_ReadPushButton(0);
+        uint8_t button_b = TFC_ReadPushButton(1);
+        
+        // -1 to 1, turning clockwise INCREASES value
+        float pot_a = TFC_ReadPot(0);
+        float pot_b = TFC_ReadPot(1);
+        
+        sendString("dip=%u btA=%u btB=%u ptA=%f ptB=%f",dip,button_a, button_b, pot_a, pot_b);
+    }
+
+}
+
 void initVariables() {
     // Initialise three PID controllers for the servo and each wheel.
     initPID(&servo_pid, 0.f, 0.f, 0.f);
@@ -411,7 +433,7 @@
     
     if(frame_counter % 256 == 0) {
         float level = TFC_ReadBatteryVoltage() * 6.25;          
-        pc.putc('J');
+        pc.putc(BATTERY_LEVEL);
         byte_float_union._float = level;
         pc.putc(byte_float_union.bytes[0]);
         pc.putc(byte_float_union.bytes[1]);
@@ -423,7 +445,7 @@
 void sendString(const char *format, ...) {
     va_list arg;
 
-    pc.putc('E');
+    pc.putc(STATUS_STRING);
     va_start (arg, format); 
     pc.vprintf(format,arg);
     va_end (arg);
@@ -433,7 +455,7 @@
 inline void sendImage() {
      //Only send 1/3 of camera frames to GUI program
     if((frame_counter % 3) == 0) {
-        pc.putc('H');
+        pc.putc(SEND_LINE);
         if(sendCam == 0) {            
             for(i = 0; i < 128; i++) {
                 pc.putc((int8_t)(CLOSE_CAMERA[i] >> 4) & 0xFF);    
@@ -468,7 +490,7 @@
     }*/
     
 
-    pc.putc('B');
+    pc.putc(SEND_SPEEDS);
     byte_float_union._float = wL * WHEEL_RADIUS;//left_motor_pid.output; //
     pc.putc(byte_float_union.bytes[0]);
     pc.putc(byte_float_union.bytes[1]);
@@ -482,11 +504,12 @@
 
 }
 
-
+// Listen for incoming commands from telemetry program and change car variables
 inline void handleComms() {
     if(curr_cmd != 0) {
             switch(curr_cmd) {
-                case 'A':
+                // Change the PID values of the servo controller
+                case CHANGE_PID:
                     if(xb.cBuffer->available() >= 12) {
                     
                         byte_float_union.bytes[0] = xb.cBuffer->read();
@@ -513,7 +536,8 @@
                     }    
                 break;
                 
-                case 'F':
+                // Set the maximum speed that the motor controllers should attempt to drive the car to
+                case MAX_SPEED:
                     if(xb.cBuffer->available() >= 1) {
                         char a = xb.cBuffer->read();
                         speed = a;
@@ -524,7 +548,8 @@
                     }
                 break;
                 
-                case 'L':
+                // Change the electronic differential coefficient
+                case CHANGE_ED:
                  if(xb.cBuffer->available() >= 4) {
                         byte_float_union.bytes[0] = xb.cBuffer->read();
                         byte_float_union.bytes[1] = xb.cBuffer->read();
@@ -543,9 +568,11 @@
         }
         
         if(xb.cBuffer->available() > 0 && curr_cmd == 0) {
-            //Start car
+            
             char cmd = xb.cBuffer->read();
-            if(cmd == 'D') {
+            
+            //Start car the car motors
+            if(cmd == START) {
                 ALIGN_SERVO;
                 right_motor_pid.desired_value=speed;
                 left_motor_pid.desired_value=speed;
@@ -556,7 +583,8 @@
                 lapTimer.start();
                 lapNo =0;
                 
-            } else if (cmd == 'C') {
+            // Stop the car motors
+            } else if (cmd == STOP) {
                 TFC_SetMotorPWM(0.0,0.0);
                 right_motor_pid.desired_value=0;
                 right_motor_pid.measured_value = 0;
@@ -570,14 +598,22 @@
                 
                 TFC_HBRIDGE_DISABLE;
                 endTest();
-            } else if(cmd == 'A') {
-                curr_cmd = 'A';
-            } else if(cmd == 'F') {
-                curr_cmd = 'F';    
-            } else if(cmd == 'K') {
+                
+                // Change the PID values of the servo controller
+            } else if(cmd == CHANGE_PID) {
+                curr_cmd = CHANGE_PID;
+                
+                // Set the maximum speed of the motor controllers
+            } else if(cmd == MAX_SPEED) {
+                curr_cmd = MAX_SPEED;    
+                
+                // Switch the camera data that is sent over telemetry
+            } else if(cmd == SWITCH_CAM) {
                 sendCam = ~sendCam;    
-            } else if(cmd == 'L') {
-                curr_cmd = 'L'
+                
+                // Modify the electronic differential coefficient
+            } else if(cmd == CHANGE_ED) {
+                curr_cmd = CHANGE_ED;
             }
             
         }