Diff: main/main.cpp

Revision:

0:008c53db1931

Child:

2:ec75506e96a3

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/main.cpp	Tue Mar 20 16:28:07 2018 +0000
@@ -0,0 +1,110 @@
+#include "main.h"
+
+// API objects
+m3pi robot;
+
+// LEDs on the Mbed board
+BusOut leds(LED4,LED3,LED2,LED1);
+
+// Buttons on the 3pi shield
+DigitalIn button_A(p18);
+DigitalIn button_B(p17);
+DigitalIn button_X(p21);
+DigitalIn button_Y(p22);
+DigitalIn button_enter(p24);
+DigitalIn button_back(p23);
+
+// Blue potentiometers on the 3pi shield
+AnalogIn pot_P(p15);
+AnalogIn pot_I(p16);
+AnalogIn pot_D(p19);
+AnalogIn pot_S(p20);
+
+
+// This bit is the main function and is the code that is run when the buggies are powered up
+int main()
+{
+    init();
+    welcome();
+    calibrate();
+
+    float dt = 1.0/50.0;
+    
+    wait_for_enter();
+    
+    // keep looping until back is pressed
+    while(button_back.read() == 1) {
+        repeat();
+        wait(dt);
+    }
+
+    robot.stop();
+    robot.lcd_clear();
+    leds = 0b0000;
+}
+
+// Functions
+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 welcome()
+{
+    robot.lcd_clear();
+    const char g_song2[]= "L16 cdegreg4";
+    robot.play_music(g_song2,sizeof(g_song2));
+    robot.display_battery_voltage(0,0);
+    robot.display_signature(0,1);
+    wait(3.0);
+
+}
+
+// this makes the buggy rotate left and right
+// should be placed over a black line so the difference
+// between white and black can be determined
+void calibrate()
+{
+    leds = 0b1111;
+    robot.reset_calibration();
+
+    wait_for_enter();
+
+
+    wait(2.0);  // leave a bit of delay so hands can be moved out of the way
+    robot.auto_calibrate();  // run calibration routine
+    leds = 0b0000;
+}
+
+void wait_for_enter()
+{
+    // display message on LCD
+    robot.lcd_clear();
+    robot.lcd_goto_xy(0,0);
+    robot.lcd_print("Place on",8);
+    robot.lcd_goto_xy(0,1);
+    robot.lcd_print("  line  ",8);
+    wait(1.0);
+    // display message on LCD
+    robot.lcd_clear();
+    robot.lcd_goto_xy(0,0);
+    robot.lcd_print(" Press  ",8);
+    robot.lcd_goto_xy(0,1);
+    robot.lcd_print(" ENTER  ",8);
+
+    while (button_enter.read() == 1) {  // wait for button press
+
+    }
+
+    robot.lcd_clear();
+
+}
\ No newline at end of file