Diff: main.cpp

Revision:

15:8b76add42254

Parent:

14:12fb3e326911

Child:

17:3fcc43140382

--- a/main.cpp	Thu Oct 06 12:12:39 2022 +0000
+++ b/main.cpp	Tue Oct 11 09:20:15 2022 +0000
@@ -2,6 +2,10 @@
 #include "m3pi.h"
 
 m3pi m3pi;
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
 
 // Minimum and maximum motor speeds
 #define MAX 1.0
@@ -14,8 +18,10 @@
 
 // Prototypes
 int PitTest(void);   // Test if to robot needs to goto pit 
-void InitialMessages (void); // Prints initial message to the LCD 
-void LED_Control(int ledNumber, int state) //turn ledNumber to 1=on, 0 = off
+void InitialMessages(void); // Prints initial message to the LCD 
+void LED_Control(int ledNumber, int state); //turn ledNumber to 1=on, 0 = off
+void PitStop(void); // 
+void Blink(int ledNumber); // make ledNumber blinik
 
 int main() {
     
@@ -32,20 +38,27 @@
 
     /*Team 7 Variabels*/
     int gotoPit = 0; // wether or not the robot is heading to pit. Initialstate false. 
-    int ccount; //used to count cycles
+    int ccount = 0; //used to count cycles
     
 
     
     /*Printing secret cat mission*/
-    InitialMessage();
+    InitialMessages();
  
 
     
     while (1) {
         /* If cycle count divided by 100 does not have a rest. test if pit */
-        if (ccount %100 == 0 && gotoPit=0){
-            gotoPit = PitTest(void);
-            }
+        if (ccount % 100 == 0 && gotoPit == 0)
+        {
+            gotoPit = PitTest();
+        }
+        if (gotoPit == 1)
+        {
+            PitStop ();
+            
+        }
+            
     
         // Get the position of the line.
         current_pos_of_line = m3pi.line_position();        
@@ -84,9 +97,11 @@
         
     ccount++;
     }
+    
+    
 }
 
-void InitialMessage(void){
+void InitialMessages(void){
     /*Prints iniatl secret mission*/
     
     m3pi.cls();
@@ -108,12 +123,12 @@
 /* Test the batteri voltage if the robot is not headed for pit */ 
     
     const float BATVOLTTRESHOLD = 3.0; // Treshold i volt
-    int result;
+    int result = 0;
     
     /*Test if the voltage is below the threshold if so turn on go to pit mode*/
-    if (m3pi.battery() <= BATVOLTTRESHOLD ()){  
+    if (m3pi.battery() <= BATVOLTTRESHOLD ){  
         result = 1; // Set goto pit condition
-        LED_Control(1, 1)
+        LED_Control(1, 1);
         m3pi.cls();
         m3pi.locate(0,0);
         m3pi.printf("Going to");
@@ -125,16 +140,47 @@
 
 void LED_Control(int ledNumber, int state){
     //LED1 on if robot is looking for pit
-    if ledNumber == 1{
-        DigitalOut led1(state);
+    if (ledNumber == 1)
+    {
+        led1 = state;
+    }
+    if (ledNumber == 2)
+    {
+        led2 = state;
     }
-    if ledNumber == 2{
-        DigitalOut led1(state);
+    if (ledNumber == 3)
+    {
+        led3 = state;
+    }
+    if (ledNumber == 4)
+    {
+        led4 = state;
     }
-    if ledNumber == 3{
-        DigitalOut led1(state);
+}
+
+void Blink(int ledNumber)
+{
+    int a = 2;
+    LED_Control (ledNumber , 0);
+    wait(a);
+    LED_Control (ledNumber , 1);
+    wait(a);
     }
-    if ledNumber == 4{
-        DigitalOut led1(state);
+
+void PitStop(void)
+{
+    /* Testing alternative stop function
+    m3pi.left_motor(0);
+    m3pi.right_motor(0);
+    */
+    m3pi.stop();       // stop all engine
+    
+    // increase counter with one
+    while (1)
+    {
+        Blink (1);      // signal in pit
+        
+/* missing input to stop blink. */
+        
     }
 }
\ No newline at end of file