Diff: main.cpp

Revision:

45:3435bdd2d487

Parent:

44:1884ffec9a57

Child:

46:6806ea59ffed

--- a/main.cpp	Mon Mar 20 12:23:34 2017 +0000
+++ b/main.cpp	Thu Mar 23 09:36:39 2017 +0000
@@ -15,6 +15,7 @@
 #include "angular_speed.h"
 #include "main.h"
 #include "motor.h"
+#include "MMA8451Q.h"
 
 // Serial
 #if USE_COMMS
@@ -34,8 +35,15 @@
 //testing timer for laptimes
 Timer lapTimer;
 
+MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS);
+
+
+
+int loop = 0;
+float accc = 0.f;
 
 int main() {
+    
     //Set up TFC driver stuff
     TFC_Init();
     ALIGN_SERVO;
@@ -52,6 +60,7 @@
     #if !(USE_COMMS)
         buttonStartup();
     #endif
+    
               
     while(1) {
         
@@ -66,7 +75,19 @@
             servo_pid.measured_value = findCentreValue(CLOSE_CAMERA, left, right);
             
             // Check if car is at the stop line
-            handleStartStop();
+            
+            if(loop % 10 == 0) {
+                accc = checkAcc();
+            }
+            
+            if(accc < 0.18f) {
+                handleStartStop(); 
+            } else {    
+                sendString("up %f", accc);    
+            }
+            
+            
+            
             
             #if USE_COMMS
                 // Send the line scan image over serial
@@ -93,15 +114,17 @@
             
 
             
-            
+        //wait_ms(25);    
             
         }
     }
 }
 
 void buttonStartup() {
+    TFC_SetBatteryLED(led_values[b_pressed % 4]);
     while(1) {
-       
+        //handleComms();
+        
         // 2 bit is broken = max value is 13
         uint8_t dip = TFC_GetDIP_Switch();
         
@@ -113,58 +136,73 @@
         float pot_a = TFC_ReadPot(0);
         float pot_b = TFC_ReadPot(1);
         
-        if(!aDown && button_a) {
-            aDown = true;    
-        } else if(aDown && !button_a) {
+        if(button_b && !bDown) {
+            bDown = true;
+            continue;        
+        }
+        if(!button_b && bDown) {
+            b_pressed++;
+            TFC_SetBatteryLED(led_values[b_pressed % 4]);
+            bDown = false;
+            continue;
+        }
+        
+        if(button_a && !aDown) {
+            aDown = true;
+            continue;        
+        }
+        if(!button_a && aDown) {
+            
+            TFC_SetBatteryLED(0);
             aDown = false;
             
             int choice = b_pressed % 4;
             switch(choice) {
-                case 0:
+                 case 0:
                      initPID(&servo_pid, 2.2f, 0.6f, 0.f);
-                     speed = 100;
+                     speed = 40;
+                     ed_tune = 1.0f;
                     break;
                 case 1:
                      initPID(&servo_pid, 2.2f, 0.6f, 0.f);
                      speed = 120;
+                     ed_tune = 1.0f;
                     break;
                 case 2:
                      initPID(&servo_pid, 2.2f, 0.6f, 0.f);
                      speed = 130;
+                     ed_tune = 1.0f;
                     break;
                 case 3:
                      initPID(&servo_pid, 2.2f, 0.6f, 0.f);
                      speed = 140;
+                     ed_tune = 1.0f;
                     break;
-                }
-                wait(1.f);
+            }
+            
+            wait(2.f);
                 
-                ALIGN_SERVO;
-                right_motor_pid.desired_value=speed;
-                left_motor_pid.desired_value=speed;
-                TFC_HBRIDGE_ENABLE;
-                
-                    
-                servo_pid.integral = 0;
-                return;
+            ALIGN_SERVO;
+            right_motor_pid.desired_value=speed;
+            left_motor_pid.desired_value=speed;
+            TFC_HBRIDGE_ENABLE;
+            servo_pid.integral = 0;        
+            //sendString("PID: %f %f %f speed:%f ed:%f",servo_pid.Kp, servo_pid.Ki, servo_pid.Kd, speed, ed_tune);                
+            return;
         }
         
-        if(!bDown && button_b) {
-            aDown = true;    
-        } else if(bDown && !button_b) {
-            aDown = false;
-            TFC_SetBatteryLED(led_values[b_pressed % 4]);
-            b_pressed++;
-        }
+        
     }
+ 
+}
 
-}
+
 
 void initVariables() {
     // Initialise three PID controllers for the servo and each wheel.
     initPID(&servo_pid, 0.f, 0.f, 0.f);
-    initPID(&left_motor_pid, 0.01f, 0.f, 0.f);
-    initPID(&right_motor_pid, 0.01f, 0.f, 0.f);
+    initPID(&left_motor_pid, 0.007f, 0.f, 0.f);
+    initPID(&right_motor_pid, 0.007f, 0.f, 0.f);
     
     right_motor_pid.desired_value=0;
     left_motor_pid.desired_value=0;
@@ -413,7 +451,6 @@
     
     TFC_SetMotorPWM(left_motor_pid.output,right_motor_pid.output);
     
-    
     t.stop();
     t.reset();
     t.start();
@@ -440,15 +477,15 @@
         lastPixel = currentPixel;
     }
     //Was used to send an indication that the marker was seen, useful for debugging
-    //if (slower % 1000 == 0) {
+    //if (slower % 1000 == `0) {
         //sendString("Transitions seen: %d", transitionsSeen);
     //}
     //slower++;
     if(transitionsSeen >= 5) {
         //Stop the car!
         #if USE_COMMS
-            sendString("Start/stop seen");
-            lapTime();
+            //sendString("Start/stop seen");
+            //lapTime();
         #endif
         TFC_SetMotorPWM(0.f,0.f);
         TFC_HBRIDGE_DISABLE;
@@ -456,6 +493,10 @@
     }
 }
 
+float checkAcc() {
+    return abs(acc.getAccY());   
+}
+
 
 inline void initSpeedSensors() {
     t1.start();