Craig Evans
/
3pi_Move
Example to move the robot.
Fork of 3pi_Line_Follow by
main/main.cpp@0:008c53db1931, 2018-03-20 (annotated)
- Committer:
- eencae
- Date:
- Tue Mar 20 16:28:07 2018 +0000
- Revision:
- 0:008c53db1931
- Child:
- 2:ec75506e96a3
Simple line follow code for the m3pi robot.; ; Written in an Arudino-like way - there is a repeat() function that is the only thing that needs to be modified.; ; Useful for coding beginners.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
eencae | 0:008c53db1931 | 1 | #include "main.h" |
eencae | 0:008c53db1931 | 2 | |
eencae | 0:008c53db1931 | 3 | // API objects |
eencae | 0:008c53db1931 | 4 | m3pi robot; |
eencae | 0:008c53db1931 | 5 | |
eencae | 0:008c53db1931 | 6 | // LEDs on the Mbed board |
eencae | 0:008c53db1931 | 7 | BusOut leds(LED4,LED3,LED2,LED1); |
eencae | 0:008c53db1931 | 8 | |
eencae | 0:008c53db1931 | 9 | // Buttons on the 3pi shield |
eencae | 0:008c53db1931 | 10 | DigitalIn button_A(p18); |
eencae | 0:008c53db1931 | 11 | DigitalIn button_B(p17); |
eencae | 0:008c53db1931 | 12 | DigitalIn button_X(p21); |
eencae | 0:008c53db1931 | 13 | DigitalIn button_Y(p22); |
eencae | 0:008c53db1931 | 14 | DigitalIn button_enter(p24); |
eencae | 0:008c53db1931 | 15 | DigitalIn button_back(p23); |
eencae | 0:008c53db1931 | 16 | |
eencae | 0:008c53db1931 | 17 | // Blue potentiometers on the 3pi shield |
eencae | 0:008c53db1931 | 18 | AnalogIn pot_P(p15); |
eencae | 0:008c53db1931 | 19 | AnalogIn pot_I(p16); |
eencae | 0:008c53db1931 | 20 | AnalogIn pot_D(p19); |
eencae | 0:008c53db1931 | 21 | AnalogIn pot_S(p20); |
eencae | 0:008c53db1931 | 22 | |
eencae | 0:008c53db1931 | 23 | |
eencae | 0:008c53db1931 | 24 | // This bit is the main function and is the code that is run when the buggies are powered up |
eencae | 0:008c53db1931 | 25 | int main() |
eencae | 0:008c53db1931 | 26 | { |
eencae | 0:008c53db1931 | 27 | init(); |
eencae | 0:008c53db1931 | 28 | welcome(); |
eencae | 0:008c53db1931 | 29 | calibrate(); |
eencae | 0:008c53db1931 | 30 | |
eencae | 0:008c53db1931 | 31 | float dt = 1.0/50.0; |
eencae | 0:008c53db1931 | 32 | |
eencae | 0:008c53db1931 | 33 | wait_for_enter(); |
eencae | 0:008c53db1931 | 34 | |
eencae | 0:008c53db1931 | 35 | // keep looping until back is pressed |
eencae | 0:008c53db1931 | 36 | while(button_back.read() == 1) { |
eencae | 0:008c53db1931 | 37 | repeat(); |
eencae | 0:008c53db1931 | 38 | wait(dt); |
eencae | 0:008c53db1931 | 39 | } |
eencae | 0:008c53db1931 | 40 | |
eencae | 0:008c53db1931 | 41 | robot.stop(); |
eencae | 0:008c53db1931 | 42 | robot.lcd_clear(); |
eencae | 0:008c53db1931 | 43 | leds = 0b0000; |
eencae | 0:008c53db1931 | 44 | } |
eencae | 0:008c53db1931 | 45 | |
eencae | 0:008c53db1931 | 46 | // Functions |
eencae | 0:008c53db1931 | 47 | void init() |
eencae | 0:008c53db1931 | 48 | { |
eencae | 0:008c53db1931 | 49 | robot.init(); |
eencae | 0:008c53db1931 | 50 | |
eencae | 0:008c53db1931 | 51 | button_A.mode(PullUp); |
eencae | 0:008c53db1931 | 52 | button_B.mode(PullUp); |
eencae | 0:008c53db1931 | 53 | button_X.mode(PullUp); |
eencae | 0:008c53db1931 | 54 | button_Y.mode(PullUp); |
eencae | 0:008c53db1931 | 55 | button_enter.mode(PullUp); |
eencae | 0:008c53db1931 | 56 | button_back.mode(PullUp); |
eencae | 0:008c53db1931 | 57 | |
eencae | 0:008c53db1931 | 58 | leds = 0b0000; |
eencae | 0:008c53db1931 | 59 | } |
eencae | 0:008c53db1931 | 60 | |
eencae | 0:008c53db1931 | 61 | void welcome() |
eencae | 0:008c53db1931 | 62 | { |
eencae | 0:008c53db1931 | 63 | robot.lcd_clear(); |
eencae | 0:008c53db1931 | 64 | const char g_song2[]= "L16 cdegreg4"; |
eencae | 0:008c53db1931 | 65 | robot.play_music(g_song2,sizeof(g_song2)); |
eencae | 0:008c53db1931 | 66 | robot.display_battery_voltage(0,0); |
eencae | 0:008c53db1931 | 67 | robot.display_signature(0,1); |
eencae | 0:008c53db1931 | 68 | wait(3.0); |
eencae | 0:008c53db1931 | 69 | |
eencae | 0:008c53db1931 | 70 | } |
eencae | 0:008c53db1931 | 71 | |
eencae | 0:008c53db1931 | 72 | // this makes the buggy rotate left and right |
eencae | 0:008c53db1931 | 73 | // should be placed over a black line so the difference |
eencae | 0:008c53db1931 | 74 | // between white and black can be determined |
eencae | 0:008c53db1931 | 75 | void calibrate() |
eencae | 0:008c53db1931 | 76 | { |
eencae | 0:008c53db1931 | 77 | leds = 0b1111; |
eencae | 0:008c53db1931 | 78 | robot.reset_calibration(); |
eencae | 0:008c53db1931 | 79 | |
eencae | 0:008c53db1931 | 80 | wait_for_enter(); |
eencae | 0:008c53db1931 | 81 | |
eencae | 0:008c53db1931 | 82 | |
eencae | 0:008c53db1931 | 83 | wait(2.0); // leave a bit of delay so hands can be moved out of the way |
eencae | 0:008c53db1931 | 84 | robot.auto_calibrate(); // run calibration routine |
eencae | 0:008c53db1931 | 85 | leds = 0b0000; |
eencae | 0:008c53db1931 | 86 | } |
eencae | 0:008c53db1931 | 87 | |
eencae | 0:008c53db1931 | 88 | void wait_for_enter() |
eencae | 0:008c53db1931 | 89 | { |
eencae | 0:008c53db1931 | 90 | // display message on LCD |
eencae | 0:008c53db1931 | 91 | robot.lcd_clear(); |
eencae | 0:008c53db1931 | 92 | robot.lcd_goto_xy(0,0); |
eencae | 0:008c53db1931 | 93 | robot.lcd_print("Place on",8); |
eencae | 0:008c53db1931 | 94 | robot.lcd_goto_xy(0,1); |
eencae | 0:008c53db1931 | 95 | robot.lcd_print(" line ",8); |
eencae | 0:008c53db1931 | 96 | wait(1.0); |
eencae | 0:008c53db1931 | 97 | // display message on LCD |
eencae | 0:008c53db1931 | 98 | robot.lcd_clear(); |
eencae | 0:008c53db1931 | 99 | robot.lcd_goto_xy(0,0); |
eencae | 0:008c53db1931 | 100 | robot.lcd_print(" Press ",8); |
eencae | 0:008c53db1931 | 101 | robot.lcd_goto_xy(0,1); |
eencae | 0:008c53db1931 | 102 | robot.lcd_print(" ENTER ",8); |
eencae | 0:008c53db1931 | 103 | |
eencae | 0:008c53db1931 | 104 | while (button_enter.read() == 1) { // wait for button press |
eencae | 0:008c53db1931 | 105 | |
eencae | 0:008c53db1931 | 106 | } |
eencae | 0:008c53db1931 | 107 | |
eencae | 0:008c53db1931 | 108 | robot.lcd_clear(); |
eencae | 0:008c53db1931 | 109 | |
eencae | 0:008c53db1931 | 110 | } |