Craig Evans
/
3pi_Lab2_Task1_Example1
Simple line-follow algorithm.
Fork of 3pi_Lab1_Task2_Example1 by
main.cpp
- Committer:
- eencae
- Date:
- 2017-06-22
- Revision:
- 2:f9ec340630b0
- Parent:
- 1:497b52a2d414
File content as of revision 2:f9ec340630b0:
/* 3pi Example 2 (c) Dr Craig A. Evans, University of Leeds June 2017 */ #include "mbed.h" #include "m3pi.h" // API objects m3pi robot; AnalogIn pot_P(p15); DigitalIn button_enter(p24); // Function Prototypes void init(); void calibrate(); // Main Function int main() { init(); // move cursor to position (0,0) - top-left robot.lcd_goto_xy(0,0); robot.lcd_print("Lab 2",5); // 5 is number of characters in message (max 8) robot.lcd_goto_xy(0,1); robot.lcd_print("Task 1",6); // we will update the motors 50 times per second float dt = 1.0/50.0; calibrate(); // calibrate the sensors 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. // array to store sensor values in unsigned int values[5]= {0}; // set the initial speeds (25% forwards) float speed = 0.25; // threshold to determine when over line float threshold = 0.2; // value to increase/decrease speed by float power = 0.05; // main loop - this runs forever while(1) { // command robot to read sensors and store values in array robot.get_calibrated_values(values); // in range -1.0 to 1.0 float position = robot.calc_line_position(values); // number is negative when the line is to the left. Need to turn left if (position < threshold) { // slow down left, speed up right // ADD CODE HERE TO CONTROL MOTORS } // number is positive when the line is to the right. Need to turn right else if (position > threshold) { // slow down right, speed up left // ADD CODE HERE TO CONTROL MOTORS } // else over the line else { // drive straight ahead // ADD CODE HERE TO CONTROL MOTORS } // wait for a short time before repeating the loop wait(dt); } } // Functions void init() { robot.init(); // button is 1 when not pressed and 0 when pressed button_enter.mode(PullUp); } void calibrate() { // clear previous calibration data 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 auto-calibration routine }