Diff: main.cpp

Revision:

89:5ea90b86fefb

Parent:

88:55cd0d863d4f

Child:

90:4e47612bc593

diff -r 55cd0d863d4f -r 5ea90b86fefb main.cpp
--- a/main.cpp	Tue Oct 25 10:16:14 2022 +0000
+++ b/main.cpp	Tue Oct 25 14:54:45 2022 +0000
@@ -1,55 +1,51 @@
 #include "mbed.h"
 #include "m3pi.h"
-#include <cstdio>
 
 m3pi m3pi;
-Timer timer;
 
-// DigitalOuts & Global Variabels
+// DigitalOuts
 DigitalOut led1(LED1);
 DigitalOut led2(LED2);
 DigitalOut led3(LED3);
 DigitalOut led4(LED4);
-float startTime;
 
 // Minimum and maximum motor speeds
-#define MAX 0.65
+#define MAX 0.80
 #define MIN 0
 
-// PID terms   Test
+// PID terms
 #define P_TERM 1
 #define I_TERM 0
 #define D_TERM 20
 
-//  Ccount before test
-#define CYCLEBEFORETEST 450
+// Ccount before test
+#define CYCLEBEFORETEST 1500
 
 // Textfile paths
 #define PITLOGPATH "/local/pitlog.txt"
 #define VOLTAGELOGPATH "/local/voltage.txt"
 
-// Prototypes
-void LED_Blink(int ledNumber); // Make ledNumber blinik
 
-void LCD_CountDown(int num);  //LCD Coundown function
-void LCD_InitialMessages(void); // Prints initial message to the LCD 
+// Prototypes
+int PS_BatteryTest(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 LED_Blink(int ledNumber); // Make ledNumber blinik
+void LCDCountDown(int num);  //LCD Coundown function
 
-int PS_BatteryTest(void);   // Test if to robot needs to goto pit 
+
 void PS_PitStop(void); // 
 void PS_CreateLog(void); // create a log file or resets it (WIP
-void PS_AddStopToLog(void); // Add one to the log 
-int PS_GetNumberofPS(void);  // Get number form file
-void PS_DisplayPS(void);
+void PS_AddStopToLog(void); // Add one to the log
+// void PS_DisplayNumberofPS(void);  // Display the final number on screen WIP
 
 void TE_CreateVoltageLog(void); //
 void TE_LogVoltage(int count); // test funktion that write the woltage each time the battry is checked
 
-
 int main() {
     LocalFileSystem local("local"); 
-    timer.start();
-    startTime = timer.read();
-
+    
+    
     /*Base program Variable initiation*/
     float right;
     float left;
@@ -64,28 +60,25 @@
     int ccount = 0; //used to count cycles
     
     /*Printing secret cat mission*/
-    PS_DisplayPS();
-    LCD_InitialMessages();
+    InitialMessages();
     m3pi.sensor_auto_calibrate();
     
-
-    /*Create logs used to log the number of pitstop */
-    PS_CreateLog(); //
+    
+    /*Create pitlog used to log the number of pitstop */
+    PS_CreateLog();
     TE_CreateVoltageLog();
 
     while (1) {
-
-        /* If cycle count divided by a constant does not have a rest. test if pit */
+        /* If cycle count divided by 100 does not have a rest. test if pit */
         if (ccount % CYCLEBEFORETEST == 0 && gotoPit == 0)
         {
             TE_LogVoltage(ccount);
-           // gotoPit = PS_BatteryTest();
+            gotoPit = PS_BatteryTest();
         }
         if (gotoPit == 1)
         {
             /*Add one to the nummber allready in the pitlog*/
             PS_AddStopToLog();
-
             /*Run the pitstop function*/
             PS_PitStop();
         }
@@ -127,20 +120,22 @@
         
     ccount++;
     }
+    // PS_DisplayNumberofPS();
     
 }
 
-/**
- * LCD_InitialMessages -Prints iniatial secret mission
- */
-void LCD_InitialMessages(void){
-
+void InitialMessages(void){
+    /*Prints iniatl secret mission*/
+    
+    
+    
     m3pi.cls();
     m3pi.locate(0,0);
     m3pi.printf("DESTROY");
     m3pi.locate(0,1);
     m3pi.printf("**CATS**");
     wait(5.0);
+    
     m3pi.cls();
     m3pi.locate(0,0);
     m3pi.printf("%4.4f ",m3pi.battery());
@@ -153,156 +148,111 @@
     m3pi.locate(0,1);
     m3pi.printf("TRACK!!");
     wait(4.0);
-    LCD_CountDown(3);
+    LCDCountDown(3);
     m3pi.cls();
     m3pi.locate(0,0);
     m3pi.printf("** GO **");
-
-     
-    wait (1.0);
 }
-
-/**
- * PS_DisplayPS - Display the number of pidstops from the log
- */
-
-void PS_DisplayPS(void){
-    m3pi.cls();
-    m3pi.locate(0,0);
-    m3pi.printf("PITSTOP:");
-    m3pi.locate(0,1);
-    m3pi.printf("%d", PS_GetNumberofPS() );
-
-}
-
-/**
- * LCD_CountDown - display a countdown
- * @num  The number to count down from-
- */
-void LCD_CountDown(int num){
-
-    for (int i=num; i>0; i--)
+    
+void LCDCountDown(int num){
+    for (int i=0; i<num; i++)
     {
        m3pi.cls();
        m3pi.locate(0,0); 
-       m3pi.printf("** %d **", i);
+       m3pi.printf("** %d **", i );
        wait(1.0);
     }
-}   
-
-/**
- * PS_BatteryTest - Test the batteri voltage if the robot is not headed for pi
- * 
- *return: 0 if the battery is above the threshold- Return 1 if the robot needs to goto pit
- */
+    
+}     
+    
 int PS_BatteryTest(void){
+/* Test battery voltage, if the robot is not headed for pit */ 
     
-    const float BATVOLTTRESHOLD = 0.5; // Treshold i volt
+    const float BATVOLTTRESHOLD = 0.05; // Treshold i volt
     int result = 0;
-  
-    /*Test if the voltage is below the threshold if so turn on go to pit mode*/
+    int BatteryTestCounter;
+    
+    /*Test if the voltage is below the threshold if true 10 times in row,
+     turn on go to pit mode*/
     if (m3pi.battery() <= BATVOLTTRESHOLD ){  
-        result = 1; // Set goto pit condition
-        LED_Blink(3);
-
-        m3pi.cls();
-        m3pi.locate(0,0);
-        m3pi.printf("Going to");
-        m3pi.locate(0,1);
-        m3pi.printf("**PIT**");
+        if (++BatteryTestCounter > 10){
+            result = 1; // Set goto pit condition
+            LED_Control(1, 1);
+            m3pi.cls();
+            m3pi.locate(0,0);
+            m3pi.printf("Going to");
+            m3pi.locate(0,1);
+            m3pi.printf("**PIT**");
+        }
+    }
+    else {
+        // If battery is above threshold, reset counter
+        BatteryTestCounter = 0; 
     }
     return result;
 }
 
-/**
- * LED_Blink - Make a LED blink
- *@ledNumber - The number of the targeted LED 
- */
+void LED_Control(int ledNumber, int state){
+    //LED1 on if robot is looking for pit
+    if (ledNumber == 1) {
+        led1 = state;
+    }
+    if (ledNumber == 2){
+        led2 = state;
+    }
+    if (ledNumber == 3){
+        led3 = state;
+    }
+    if (ledNumber == 4){
+        led4 = state;
+    }
+}
+
 void LED_Blink(int ledNumber)
 {
     int a = 2;
-    
-    switch (ledNumber){
-        case(1):
-            led1 = 0;
-            wait(a);
-            led1 = 1;
-            wait(a);
-            led1 = 0;
-            wait(a);
-            break;
-        
-        case(2):
-    
-            led2 = 0;
-            wait(a);
-            led2 = 1;
-            wait(a);
-            led2 = 0;
-            wait(a);
-            break;
-        
-        case(3):
-            led3 = 0;
-            wait(a);
-            led3 = 1;
-            wait(a);
-            led3 = 0;
-            wait(a);
-            break;
-
-        case(4):
-            led4 = 0;
-            wait(a);
-            led4 = 1;
-            wait(a);
-            led4 = 0;
-            wait(a);
-            break;
-        
-        
-        default:
-            break;
+    LED_Control (ledNumber , 0);
+    wait(a);
+    LED_Control (ledNumber , 1);
+    wait(a);
     }
 
-
-        
-    }
-
-/**
- * PS_PitStop - Stops the robot and starts the signal
- */
 void PS_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)
     {
         LED_Blink (1);      // signal in pit
+        
+/* missing input to stop blink. */
+        
     }
 }
 
-/**
- * PS_CreateLog - Create a pitstop log i none excist
- * 
- * Return: The number of ppitstops as noted in the log
- */
 void PS_CreateLog(void){
+/* Create a pitlog file and test if it can open*/
     FILE *fptr;
-    
-    if ((fptr = fopen(PITLOGPATH,"r")) == NULL){
-        fptr = fopen(PITLOGPATH,"w");
-        fprintf(fptr,"%d", 0);
-        fclose(fptr);
-    }
-    
+    fptr = fopen(PITLOGPATH,"w");
+
+    if(fptr == NULL)
+         {
+            printf("Error creating log file ");
+            exit(1);
+         }
+    fprintf(fptr,"%d", 0);
+    fclose(fptr);
 }
 
-/**
- * PS_AddStopToLog - Add one to the number in the pitstop log 
- */
 void PS_AddStopToLog(void){
- 
+    /*Opens the pit log and read the number.
+     * Then adds one to that number at write it into the pitlog */
+
     FILE *fptr;
     int x, y;
     if ((fptr = fopen(PITLOGPATH,"r")) == NULL){
@@ -326,12 +276,9 @@
     fclose(fptr);
 }
 
-/** PS_GetNumberofPS - Return the  number i the pitstop recorded in the logfile
-*
-*return: An interger from the the pitlog
-*/
-int PS_GetNumberofPS(void){
-    // 
+/*
+void PS_DisplayNumberofPS(void){
+    // Display the number i the pitstop recorded in the logfile
     FILE *fptr;
     int x;
     if ((fptr = fopen(PITLOGPATH,"r")) == NULL){
@@ -340,13 +287,11 @@
         exit(1);
        }
      fscanf(fptr,"%d", &x);
-     //printf("Final number of pits stops %d", x);
+     printf("Final number of pits stops %d", x);
      fclose(fptr);
-     return x;
 }
+*/
 
-/** TE_CreateVoltageLog - create a voltagelog
-*/
 void TE_CreateVoltageLog(void){
 /* Create a voltagelog file and test if it can open*/
     FILE *fptr;
@@ -361,15 +306,11 @@
     fclose(fptr);
 }
 
-/** TE_LogVoltage - Add an entry to the voltagelog 
-*@count: The number the counter has reached in the main loop 
-*/
 void TE_LogVoltage(int count){
-
+/* Create a pitlog file and test if it can open*/
     FILE *fptr; /* voltagelog adres */
     fptr = fopen(VOLTAGELOGPATH,"a");
   
-    fprintf(fptr,"%8lf ; %8d ; %4.4f; %4.4f \n"  , (timer.read()-startTime), count, m3pi.battery(),m3pi.pot_voltage()  );
+    fprintf(fptr," %8d      %4.4f      %4.4f \n"  ,count, m3pi.battery(),m3pi.pot_voltage()  );
     fclose(fptr);
-
-}
+}
\ No newline at end of file