Diff: main/main.cpp

Revision:

0:df5216b20861

Child:

1:79219d0a33c8

diff -r 000000000000 -r df5216b20861 main/main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/main.cpp	Tue Feb 04 19:18:10 2020 +0000
@@ -0,0 +1,148 @@
+#include "main.h"
+
+// API
+m3pi robot;
+
+// LEDs
+BusOut leds(LED4,LED3,LED2,LED1);
+
+// Buttons 
+DigitalIn button_A(p18);
+DigitalIn button_B(p17);
+DigitalIn button_X(p21);
+DigitalIn button_Y(p22);
+DigitalIn button_enter(p24);
+DigitalIn button_back(p23);
+
+// Potentiometers
+AnalogIn pot_P(p15);
+AnalogIn pot_I(p16);
+AnalogIn pot_D(p19);
+AnalogIn pot_S(p20);
+
+// Prototypes
+void init();
+void calibrate();
+void follow_line( float speed );
+void left();
+void back();
+
+// Constants
+
+const float A = 0.5;         // 20
+const float B = 1/10000;     // 10000
+const float C = 1.5;    // 2/3
+
+// This bit is the main function and is the code that is run when the buggies are powered up
+int main()
+{
+    init();
+    calibrate();
+    float speed = (pot_S*0.4)+0.1;   // have it so max is 0.5 and min is 0.1
+    unsigned int *sensor;  
+    
+    float proportional = 0.0;
+    float prev_proportional = 0.0;
+    float integral = 0.0;
+    float derivative = 0.0;
+    
+    float dt = 1/50;           // updating 50 times a second
+    
+    while (1) {
+        robot.scan();
+        sensor = robot.get_sensors(); // returns the current values of all the sensors from 0-1000
+        
+        proportional = robot.read_line();  // returns a value between -1,1     (-1 = PC0 or further , -1 to -0.5 = PC1 (-0.5 is directly below PC1) , -0.5 to 0 = PC2 , 0 to 0.5 = PC3 , 0.5 to 1 and further = PC4)
+        derivative = proportional - prev_proportional;
+        integral += proportional;
+        prev_proportional = proportional;
+        
+        // calculate motor correction
+        float motor_correction = proportional*A + integral*B + derivative*C;
+        
+        // make sure the correction is never greater than the max speed as the motor will reverse
+        if( motor_correction > speed ) {
+            motor_correction = speed;
+        }
+        if( motor_correction < -speed ) {
+            motor_correction = -speed;
+        }
+    
+        if( proportional < 0 ) {
+            robot.motors(speed+motor_correction,speed);
+        } else {
+            robot.motors(speed,speed-motor_correction);
+        }
+        
+        //follow_line(speed);
+        
+        if (sensor[0] > 500) {
+            left();
+        }else if(sensor[0] < 500 && sensor[1] < 500 && sensor[2] < 500 && sensor[3] < 500 && sensor[4] < 500) {
+            back();
+        }
+        
+        robot.display_data();
+        
+        wait(dt);
+    }
+}
+
+void init()
+{
+    robot.init();
+
+    button_A.mode(PullUp);
+    button_B.mode(PullUp);
+    button_X.mode(PullUp);
+    button_Y.mode(PullUp);
+    button_enter.mode(PullUp);
+    button_back.mode(PullUp);
+
+    leds = 0b0000;
+}
+
+void calibrate()
+{
+    leds = 0b1111;
+    robot.reset_calibration();
+
+    while (button_enter.read() == 1) {}  // keep looping waiting for Enter to be pressed
+    
+    wait(2.0);  // small delay to allow hands to move away etc.
+    
+    robot.auto_calibrate();  // run calibration routine
+    leds = 0b0000;
+}
+      
+void follow_line( float speed ) 
+{
+    float position = robot.read_line();
+
+    float correction = position * speed;
+
+    if (position>0) {
+        robot.motors(speed, speed-correction);
+    } else {
+        robot.motors(speed+correction,speed);
+    }
+}
+
+void left()
+{
+    //while (sensor[0] > 500) { robot.scan(); }
+    wait(0.2);
+    robot.spin_left(0.2);
+    wait(0.32);
+    //robot.scan();
+    //robot.follow_line();
+}
+
+void back() 
+{
+    robot.reverse(0.2);
+    wait(0.1);
+    robot.spin_right(0.2);
+    wait(0.6);
+}
+    
\ No newline at end of file