Diff: main.cpp

Revision:

7:ac88c8e35048

Parent:

6:6865930c1135

Child:

8:5640c8c5088e

Child:

10:9b04c532b57b

diff -r 6865930c1135 -r ac88c8e35048 main.cpp
--- a/main.cpp	Tue Oct 04 12:09:19 2022 +0000
+++ b/main.cpp	Wed Oct 05 11:54:30 2022 +0000
@@ -1,7 +1,6 @@
 #include "mbed.h"
 #include "m3pi.h"
 
-//Mikkel har været her
 m3pi m3pi;
 
 // Minimum and maximum motor speeds
@@ -13,8 +12,10 @@
 #define I_TERM 0
 #define D_TERM 20
 
-// Tresholds 
-#define BATVOLTRESHOLD 3.0
+
+
+// Prototypes
+int PitTest(int gotoPit); 
 
 int main() {
     
@@ -30,13 +31,11 @@
     float speed = MAX;
 
     /*Team 7 Variabels*/
-    float batVol = m3pi.battery();  //Storing battery voltage in variable
-    float potVol = m3pi.pot_voltage();  //Storing pot voltage
-    int gotopit = 0; // variabel storing weather or not the robot is heading to pit
     
-    /*Digital outs*/
-    DigitalOut led1(LED1);
+    int gotoPit = 0; // wether or not the robot is heading to pit. Initialstate false. 
+    int ccount; //used to count cycles
     
+
     
     /*Printing secret cat mission*/
     m3pi.cls();
@@ -49,27 +48,20 @@
     
     m3pi.cls();
     m3pi.locate(0,0);
-    m3pi.printf("%f.3 ",batVol);
+    m3pi.printf("%f.3 ",m3pi.battery());
     m3pi.locate(0,1);
-    m3pi.printf("%f.3 ",b);
+    m3pi.printf("%f.3 ",m3pi.pot_voltage());
     wait(200.0);
     m3pi.cls();
 
-    /*Inital state*/
-    led1 = 0; // Turn off led 1 on the embed
+
     
     while (1) {
         
-        /* Test the batteri voltage if the robot is not headed for pit */
-        if (gotopit == 0) {
-            /*Sets the battery voltage to the current voltage of the battery*/
-            batVol = battery ();
-            
-            if batteryVoltage <= BATVOLTRESHOLD (){  
-                gotopit = 1; // set goto pit condition
-                led1 = 1; // trun on Led 1
+        if (ccount %100 == 0 && gotoPit=0){
+            gotoPit=PitTest(gotoPit);
             }
-        
+    
         // Get the position of the line.
         current_pos_of_line = m3pi.line_position();        
         proportional = current_pos_of_line;
@@ -105,7 +97,33 @@
         m3pi.left_motor(left);
         m3pi.right_motor(right);
         
-        
+    cyclecount++;
+    }
+}
+
+
+int PitTest(int gotoPit){
+/* Test the batteri voltage if the robot is not headed for pit */ 
+    
+    float batVol = m3pi.battery();  //Storing battery voltage in variable
+    const float BATVOLTRESHOLD = 3.0; // Treshold i volt
+    
+    /*Digital outs*/
+    DigitalOut led1(LED1);
+    led1 = 0; // Turn off led 1 on the embed
 
+    /*Updates battery voltage*/
+    batVol = m3pi.battery(); 
+    
+    /*Test if the voltage is below the treshold if so turn on go to pit mode*/
+    if batteryVoltage <= BATVOLTRESHOLD (){  
+        gotoPit = 1; // set goto pit condition
+        led1 = 1; // trun on Led 1
+        m3pi.cls();
+        m3pi.locate(0,0);
+        m3pi.printf("Going to");
+        m3pi.locate(0,1);
+        m3pi.printf("PIT Party");
     }
+
 }
\ No newline at end of file