Diff: main.cpp

Revision:

20:76f94dec91d1

Parent:

19:e0a4f9e88383

Child:

21:c3e256b18b96

--- a/main.cpp	Tue Oct 11 10:56:07 2022 +0000
+++ b/main.cpp	Tue Oct 11 11:18:45 2022 +0000
@@ -16,13 +16,26 @@
 #define I_TERM 0
 #define D_TERM 20
 
+
+// Textfile paths
+#define PITLOGPATH "pitlog.txt"
+#define VOLTAGELOGPATH "voltage.txt"
+
+
 // 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 PitStop(void); // 
 void Blink(int ledNumber); // make ledNumber blinik
 
+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
+// void PS_DisplayNumberofPS(void);  // Display the final number on screen WIP
+
+void TE_CreateVoltageLog(void); //
+void TE_VoltageLog(int count); // test funktion that write the woltage each time the battry is checked
+
 int main() {
     
     m3pi.sensor_auto_calibrate();
@@ -40,26 +53,31 @@
     int gotoPit = 0; // wether or not the robot is heading to pit. Initialstate false. 
     int ccount = 0; //used to count cycles
     
-
+    /*Create pitlog used to log the number of pitstop */
+    PS_CreateLog();
     
     /*Printing secret cat mission*/
     InitialMessages();
- 
+    
 
     
     while (1) {
         /* If cycle count divided by 100 does not have a rest. test if pit */
         if (ccount % 100 == 0 && gotoPit == 0)
         {
+            TE_VoltageLog(ccount);
             gotoPit = PitTest();
+        
         }
         if (gotoPit == 1)
         {
-            PitStop ();
+            /*Add one to the nummber allready in the pitlog*/
+            PS_AddStopToLog();
+            /*Run the pitstop function*/
+            PS_PitStop ();
             
         }
             
-    
         // Get the position of the line.
         current_pos_of_line = m3pi.line_position();        
         proportional = current_pos_of_line;
@@ -97,7 +115,7 @@
         
     ccount++;
     }
-    
+    // PS_DisplayNumberofPS();
     
 }
 
@@ -109,20 +127,20 @@
     m3pi.printf("eliminate");
     m3pi.locate(0,1);
     m3pi.printf("all cats");
-    wait(10.0);
+    wait(5.0);
     
     m3pi.cls();
     m3pi.locate(0,0);
-    m3pi.printf("%f.3 ",m3pi.battery());
+    m3pi.printf("%4.4f ",m3pi.battery());
     m3pi.locate(0,1);
-    m3pi.printf("%f.3 ",m3pi.pot_voltage());
-    wait(200.0);
+    m3pi.printf("%4.4f ",m3pi.pot_voltage());
+    wait(10.0);
     m3pi.cls();
 }
 int PitTest(void){
 /* Test the batteri voltage if the robot is not headed for pit */ 
     
-    const float BATVOLTTRESHOLD = 3.0; // Treshold i volt
+    const float BATVOLTTRESHOLD = 4.64; // Treshold i volt
     int result = 0;
     
     /*Test if the voltage is below the threshold if so turn on go to pit mode*/
@@ -140,20 +158,16 @@
 
 void LED_Control(int ledNumber, int state){
     //LED1 on if robot is looking for pit
-    if (ledNumber == 1)
-    {
+    if (ledNumber == 1) {
         led1 = state;
     }
-    if (ledNumber == 2)
-    {
+    if (ledNumber == 2){
         led2 = state;
     }
-    if (ledNumber == 3)
-    {
+    if (ledNumber == 3){
         led3 = state;
     }
-    if (ledNumber == 4)
-    {
+    if (ledNumber == 4){
         led4 = state;
     }
 }
@@ -167,7 +181,7 @@
     wait(a);
     }
 
-void PitStop(void)
+void PS_PitStop(void)
 {
     /* Testing alternative stop function
     m3pi.left_motor(0);
@@ -183,4 +197,86 @@
 /* missing input to stop blink. */
         
     }
+}
+
+void PS_CreateLog(void){
+/* Create a pitlog file and test if it can open*/
+    FILE *fptr;
+    fptr = fopen(PITLOGPATH,"w");
+
+    if(fptr == NULL)
+         {
+            printf("Error creating log file ");
+            exit(1);
+         }
+    fprintf(fptr,"%d", 0);
+    fclose(fptr);
+}
+
+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){
+        printf("Error! opening file");
+        // Program exits if the file pointer returns NULL.
+        exit(1);
+       }
+
+    fscanf(fptr,"%d", &x);
+    fclose(fptr);
+
+    y = x+1;
+    fptr = fopen(PITLOGPATH,"w");
+
+    if(fptr == NULL)
+        {
+             printf("Error creating log file ");
+             exit(1);
+        }
+    fprintf(fptr,"%d", y);
+    fclose(fptr);
+}
+
+/*
+void PS_DisplayNumberofPS(void){
+    //Display the number i the pitstop recorded in the logfile
+/*
+    FILE *fptr;
+    int x;
+    if ((fptr = fopen(PITLOGPATH,"r")) == NULL){
+        printf("Error! opening file");
+        // Program exits if the file pointer returns NULL.
+        exit(1);
+       }
+     fscanf(fptr,"%d", &x);
+     printf("Final number of pits stops %d", x);
+     fclose(fptr);
+}
+*/
+
+void TE_CreateVoltageLog(void){
+/* Create a voltagelog file and test if it can open*/
+    FILE *fptr;
+    fptr = fopen(VOLTAGELOGPATH,"w");
+
+    if(fptr == NULL)
+         {
+            printf("Error creating log file ");
+            exit(1);
+         }
+    
+    fclose(fptr);
+}
+
+
+void TE_VoltageLog(int count){
+/* Create a pitlog file and test if it can open*/
+    FILE *fptr; /* voltagelog adres */
+    fptr = fopen(VOLTAGELOGPATH,"a");
+  
+    fprintf(fptr," %8d      %4.4f      %4.4f "  ,count, m3pi.battery(),m3pi.pot_voltage()  );
+    fclose(fptr);
 }
\ No newline at end of file