Diff: main.cpp

Revision:

0:2b88d9638dd6

Child:

1:fbf9b4ab4bc5

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Nov 21 11:35:18 2018 +0000
@@ -0,0 +1,205 @@
+#include "mbed.h"
+#include "m3pi.h"
+
+m3pi m3pi;
+
+// Minimum and maximum motor speeds
+#define MAX 1.0
+#define MIN 0
+
+//Our variables
+#define OPTIMAL_SPEED 0.95
+
+// PID terms
+#define P_TERM 1.0
+#define D_TERM 8.0
+#define I_TERM 0.01  //skal være nul eller tæt på
+
+//Print to LCD (character, line) numbers are zero indexed
+void print_battery() {
+    
+    //measure battery voltage
+    float f = m3pi.battery();
+    char text[8];
+    m3pi.cls();
+    m3pi.locate(0,0);
+    
+    //convert to a string
+    sprintf(text, "%4.2f", f);
+    m3pi.printf(text);
+}
+
+void toggleLED(DigitalOut &led) {
+    if (led == 1) {
+        led = 0;
+    } else {
+        led = 1;
+    }
+}
+
+
+int main(void) {
+    
+    //user push button p21
+    DigitalIn button(p21);
+    button.mode(PullUp);
+
+    //set the LEDs as objects    
+    //on the m3pi
+    DigitalOut redled8(p13);
+    DigitalOut redled7(p14);
+    DigitalOut redled6(p15);
+    DigitalOut redled5(p16);
+    DigitalOut redled4(p17);
+    DigitalOut redled3(p18);
+    DigitalOut redled2(p19);
+    DigitalOut redled1(p20);
+    
+    //on the mbed
+    DigitalOut myled1 (LED1);
+    DigitalOut myled2 (LED2);
+    DigitalOut myled3 (LED3);
+    DigitalOut myled4 (LED4);
+    
+    
+    // Set all LEDs to off
+    redled1 = 0;
+    redled2 = 0;
+    redled3 = 0;
+    redled4 = 0;
+    redled5 = 0;
+    redled6 = 0;
+    redled7 = 0;
+    redled8 = 0;
+    myled1 = 0;
+    myled2 = 0;
+    myled3 = 0;
+    myled4 = 0;
+    
+    
+    //Initialize the car
+    m3pi.sensor_auto_calibrate();
+
+    // create some control variables
+    float right;
+    float left;
+    float current_pos_of_line = 0.0;
+    float previous_pos_of_line = 0.0;
+    float derivative,proportional,integral = 0;
+    float power;
+    float speed = OPTIMAL_SPEED;
+    
+    
+    // Start the loop
+    while (1) {
+    
+    // Get the position of the line.
+    current_pos_of_line = m3pi.line_position();        
+    proportional = current_pos_of_line;
+    
+    // Compute the derivative
+    derivative = current_pos_of_line - previous_pos_of_line;
+    
+    // Compute the integral
+    integral += proportional;
+    
+    // Remember the last position.
+    previous_pos_of_line = current_pos_of_line;
+    
+    // Compute the power
+    power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
+    
+    // Compute new speeds   
+    right = speed+power;
+    left  = speed-power;
+    
+    // limit checks
+    if (right < MIN)
+        right = MIN;
+    else if (right > MAX)
+        right = MAX;
+        
+    if (left < MIN)
+        left = MIN;
+    else if (left > MAX)
+        left = MAX;
+        
+   // set speed 
+    m3pi.left_motor(left);
+    m3pi.right_motor(right);
+    
+    print_battery();
+
+}
+    
+    
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        /*
+        f = m3pi.battery();
+        if (f > 4.7) {
+            myled1 = 1;
+        } else {
+            myled1 = 0;
+        }
+        
+        
+        m3pi.cls();
+        m3pi.locate(0,0);
+    
+        //convert to a string
+        sprintf(text, "%4.2f", f);
+        m3pi.printf(text);
+        wait(0.2);
+        */
+          
+      
+}
+
+
+
+/* Structure
+
+functions:
+
+Charging Strategy
+poll voltage -> return voltage
+Charging limits
+Low Batt level
+Enough charge level
+
+Control -> inherits from m3pi
+pause line follower
+resume line follower
+speed up (home stretch)
+
+
+Class: CarData
+
+
+Class: PitData
+Total time in pit
+Total stops in pit
+
+Methods:
+Update Wh
+Update pit stops
+Print pitstops to screen
+
+
+
+
+
+*/
\ No newline at end of file