Z Zh / Mbed 2 deprecated 3pi_Lab2_Task1

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed m3pi

main.cpp@1:497b52a2d414, 2017-06-22 (annotated)

Committer:
eencae
Date:
Thu Jun 22 14:52:57 2017 +0000
Revision:
1:497b52a2d414
Parent:
0:127d52afa7a9
Child:
2:f9ec340630b0
Initial commit.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
eencae 0:127d52afa7a9 1 /* 3pi Example 2
eencae 0:127d52afa7a9 2
eencae 0:127d52afa7a9 3 (c) Dr Craig A. Evans, University of Leeds
eencae 0:127d52afa7a9 4
eencae 0:127d52afa7a9 5 June 2017
eencae 0:127d52afa7a9 6
eencae 0:127d52afa7a9 7 */
eencae 0:127d52afa7a9 8
eencae 0:127d52afa7a9 9 #include "mbed.h"
eencae 0:127d52afa7a9 10 #include "m3pi.h"
eencae 0:127d52afa7a9 11
eencae 0:127d52afa7a9 12 // API objects
eencae 1:497b52a2d414 13 m3pi robot;
eencae 0:127d52afa7a9 14 AnalogIn pot_P(p15);
eencae 1:497b52a2d414 15 DigitalIn button_enter(p24);
eencae 0:127d52afa7a9 16
eencae 0:127d52afa7a9 17 // Function Prototypes
eencae 0:127d52afa7a9 18 void init();
eencae 1:497b52a2d414 19 void calibrate();
eencae 0:127d52afa7a9 20
eencae 0:127d52afa7a9 21 // Main Function
eencae 0:127d52afa7a9 22 int main()
eencae 0:127d52afa7a9 23 {
eencae 0:127d52afa7a9 24 init();
eencae 1:497b52a2d414 25
eencae 0:127d52afa7a9 26 // move cursor to position (0,0) - top-left
eencae 1:497b52a2d414 27 robot.lcd_goto_xy(0,0);
eencae 1:497b52a2d414 28 robot.lcd_print("Lab 2",5); // 5 is number of characters in message (max 8)
eencae 1:497b52a2d414 29 robot.lcd_goto_xy(0,1);
eencae 1:497b52a2d414 30 robot.lcd_print("Task 1",6);
eencae 1:497b52a2d414 31
eencae 0:127d52afa7a9 32 // we will update the motors 50 times per second
eencae 0:127d52afa7a9 33 float dt = 1.0/50.0;
eencae 0:127d52afa7a9 34
eencae 1:497b52a2d414 35 calibrate(); // calibrate the sensors
eencae 1:497b52a2d414 36
eencae 1:497b52a2d414 37 while (button_enter.read() == 1) {
eencae 1:497b52a2d414 38 // keep looping waiting for Enter to be pressed
eencae 1:497b52a2d414 39 }
eencae 1:497b52a2d414 40 wait(2.0); // small delay to allow hands to move away etc.
eencae 1:497b52a2d414 41
eencae 1:497b52a2d414 42 // array to store sensor values in
eencae 1:497b52a2d414 43 unsigned int values[5]= {0};
eencae 1:497b52a2d414 44
eencae 1:497b52a2d414 45 // set the initial speeds (25% forwards)
eencae 1:497b52a2d414 46 float speed = 0.25;
eencae 1:497b52a2d414 47
eencae 1:497b52a2d414 48 // threshold to determine when over line
eencae 1:497b52a2d414 49 float threshold = 0.2;
eencae 1:497b52a2d414 50
eencae 1:497b52a2d414 51 // value to increase/decrease speed by
eencae 1:497b52a2d414 52 float power = 0.05;
eencae 1:497b52a2d414 53
eencae 0:127d52afa7a9 54 // main loop - this runs forever
eencae 0:127d52afa7a9 55 while(1) {
eencae 0:127d52afa7a9 56
eencae 1:497b52a2d414 57 // command robot to read sensors and store values in array
eencae 1:497b52a2d414 58 robot.get_calibrated_values(values);
eencae 1:497b52a2d414 59
eencae 1:497b52a2d414 60 // in range -1.0 to 1.0
eencae 1:497b52a2d414 61 float position = robot.calc_line_position(values);
eencae 1:497b52a2d414 62
eencae 1:497b52a2d414 63 // number is negative when the line is to the left. Need to turn left
eencae 1:497b52a2d414 64 if (position < threshold) {
eencae 1:497b52a2d414 65 // slow down left, speed up right
eencae 1:497b52a2d414 66 robot.motors(speed-power,speed+power);
eencae 1:497b52a2d414 67 }
eencae 1:497b52a2d414 68 // number is positive when the line is to the right. Need to turn right
eencae 1:497b52a2d414 69 else if (position > threshold) {
eencae 1:497b52a2d414 70 // slow down right, speed up left
eencae 1:497b52a2d414 71 robot.motors(speed+power,speed-power);
eencae 1:497b52a2d414 72 }
eencae 1:497b52a2d414 73 // else over the line
eencae 1:497b52a2d414 74 else {
eencae 1:497b52a2d414 75 // drive straight ahead
eencae 1:497b52a2d414 76 robot.motors(speed,speed);
eencae 1:497b52a2d414 77 }
eencae 1:497b52a2d414 78
eencae 0:127d52afa7a9 79 // wait for a short time before repeating the loop
eencae 0:127d52afa7a9 80 wait(dt);
eencae 0:127d52afa7a9 81
eencae 0:127d52afa7a9 82 }
eencae 0:127d52afa7a9 83 }
eencae 0:127d52afa7a9 84
eencae 0:127d52afa7a9 85 // Functions
eencae 0:127d52afa7a9 86 void init()
eencae 0:127d52afa7a9 87 {
eencae 1:497b52a2d414 88 robot.init();
eencae 1:497b52a2d414 89
eencae 1:497b52a2d414 90 // button is 1 when not pressed and 0 when pressed
eencae 1:497b52a2d414 91 button_enter.mode(PullUp);
eencae 1:497b52a2d414 92
eencae 1:497b52a2d414 93 }
eencae 1:497b52a2d414 94
eencae 1:497b52a2d414 95 void calibrate()
eencae 1:497b52a2d414 96 {
eencae 1:497b52a2d414 97 // clear previous calibration data
eencae 1:497b52a2d414 98 robot.reset_calibration();
eencae 1:497b52a2d414 99
eencae 1:497b52a2d414 100 while (button_enter.read() == 1) {
eencae 1:497b52a2d414 101 // keep looping waiting for Enter to be pressed
eencae 1:497b52a2d414 102 }
eencae 1:497b52a2d414 103 wait(2.0); // small delay to allow hands to move away etc.
eencae 1:497b52a2d414 104
eencae 1:497b52a2d414 105 robot.auto_calibrate(); // run auto-calibration routine
eencae 1:497b52a2d414 106
eencae 0:127d52afa7a9 107 }