Diff: main.cpp

Revision:

43:649473c5a12b

Parent:

41:d74878640739

Child:

44:1884ffec9a57

--- a/main.cpp	Wed Jan 25 15:46:50 2017 +0000
+++ b/main.cpp	Mon Mar 20 11:55:20 2017 +0000
@@ -49,7 +49,9 @@
     initVariables();
     initSpeedSensors();
     
-    //buttonStartup();
+    #if !(USE_COMMS)
+        buttonStartup();
+    #endif
               
     while(1) {
         
@@ -99,8 +101,7 @@
 
 void buttonStartup() {
     while(1) {
-        handleComms();
-        
+       
         // 2 bit is broken = max value is 13
         uint8_t dip = TFC_GetDIP_Switch();
         
@@ -112,7 +113,49 @@
         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);
+        if(!aDown && button_a) {
+            aDown = true;    
+        } else if(aDown && !button_a) {
+            aDown = false;
+            
+            int choice = b_pressed % 4;
+            switch(choice) {
+                case 0:
+                     initPID(&servo_pid, 2.2f, 0.6f, 0.f);
+                     speed = 100;
+                    break;
+                case 1:
+                     initPID(&servo_pid, 2.2f, 0.6f, 0.f);
+                     speed = 120;
+                    break;
+                case 2:
+                     initPID(&servo_pid, 2.2f, 0.6f, 0.f);
+                     speed = 130;
+                    break;
+                case 3:
+                     initPID(&servo_pid, 2.2f, 0.6f, 0.f);
+                     speed = 140;
+                    break;
+                }
+                wait(1.f);
+                
+                ALIGN_SERVO;
+                right_motor_pid.desired_value=speed;
+                left_motor_pid.desired_value=speed;
+                TFC_HBRIDGE_ENABLE;
+                
+                    
+                servo_pid.integral = 0;
+                return;
+        }
+        
+        if(!bDown && button_b) {
+            aDown = true;    
+        } else if(bDown && !button_b) {
+            aDown = false;
+            TFC_SetBatteryLED(led_values[b_pressed % 4]);
+            b_pressed++;
+        }
     }
 
 }
@@ -202,7 +245,9 @@
     
     //If both edges are not visible, we are likely in a crossroads
     if(!rightSeen && !leftSeen) {
-        sendString("lost edges");
+        #if USE_COMMS
+            sendString("lost edges");
+        #endif
         ALIGN_SERVO; //Straighten wheels so we go straight through the crossroads
         servo_pid.integral = 0;
         l=0;r=128;
@@ -401,10 +446,13 @@
     //slower++;
     if(transitionsSeen >= 5) {
         //Stop the car!
-        sendString("Start/stop seen");
+        #if USE_COMMS
+            sendString("Start/stop seen");
+            lapTime();
+        #endif
         TFC_SetMotorPWM(0.f,0.f);
         TFC_HBRIDGE_DISABLE;
-        lapTime();
+        
     }
 }
 
@@ -434,11 +482,7 @@
     if(frame_counter % 256 == 0) {
         float level = TFC_ReadBatteryVoltage() * 6.25;          
         pc.putc(BATTERY_LEVEL);
-        byte_float_union._float = level;
-        pc.putc(byte_float_union.bytes[0]);
-        pc.putc(byte_float_union.bytes[1]);
-        pc.putc(byte_float_union.bytes[2]);
-        pc.putc(byte_float_union.bytes[3]);
+        writeFloat(level);
     }
 }
 
@@ -472,36 +516,17 @@
 }
 
 inline void sendSpeeds() {
-  
-    /*float en = getLineEntropy();
-    
-    if(onTrack) {
-        if(en <= 14000) {
-            onTrack = false;
-            sendString("offfffffffffffff");    
-            TFC_SetMotorPWM(0.0,0.0);
-            TFC_HBRIDGE_DISABLE;
-        }
-    } else {
-        if(en > 14000) {
-            onTrack = true;
-            sendString("ON TRACK");
-        }    
-    }*/
-    
+    pc.putc(SEND_SPEEDS);
+    writeFloat(wL * WHEEL_RADIUS);
+    writeFloat(wR * WHEEL_RADIUS);
+}
 
-    pc.putc(SEND_SPEEDS);
-    byte_float_union._float = wL * WHEEL_RADIUS;//left_motor_pid.output; //
+void writeFloat(float f) {
+    byte_float_union._float = f;
     pc.putc(byte_float_union.bytes[0]);
     pc.putc(byte_float_union.bytes[1]);
     pc.putc(byte_float_union.bytes[2]);
     pc.putc(byte_float_union.bytes[3]);
-    byte_float_union._float = wR * WHEEL_RADIUS; // right_motor_pid.output; //
-    pc.putc(byte_float_union.bytes[0]);
-    pc.putc(byte_float_union.bytes[1]);
-    pc.putc(byte_float_union.bytes[2]);
-    pc.putc(byte_float_union.bytes[3]);    
-
 }
 
 float readFloat() {