Robert Harrell
/
EBIO-Project-Robot-POLL
Code for Embedded Systems I/O class project
main.cpp@0:bcc2a42d221f, 2014-06-11 (annotated)
- Committer:
- robertwharrell
- Date:
- Wed Jun 11 01:54:21 2014 +0000
- Revision:
- 0:bcc2a42d221f
Initial version
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
robertwharrell | 0:bcc2a42d221f | 1 | #include "mbed.h" |
robertwharrell | 0:bcc2a42d221f | 2 | #include "Motor.h" |
robertwharrell | 0:bcc2a42d221f | 3 | |
robertwharrell | 0:bcc2a42d221f | 4 | //#define DEBUG 1 |
robertwharrell | 0:bcc2a42d221f | 5 | |
robertwharrell | 0:bcc2a42d221f | 6 | #define SENSOR_L p30 |
robertwharrell | 0:bcc2a42d221f | 7 | #define SENSOR_R p29 |
robertwharrell | 0:bcc2a42d221f | 8 | #define MOTOR_L_PWM p26 |
robertwharrell | 0:bcc2a42d221f | 9 | #define MOTOR_L_FWD p25 |
robertwharrell | 0:bcc2a42d221f | 10 | #define MOTOR_L_REV p24 |
robertwharrell | 0:bcc2a42d221f | 11 | #define MOTOR_R_PWM p21 |
robertwharrell | 0:bcc2a42d221f | 12 | #define MOTOR_R_FWD p23 |
robertwharrell | 0:bcc2a42d221f | 13 | #define MOTOR_R_REV p22 |
robertwharrell | 0:bcc2a42d221f | 14 | #define BASE_SPEED 0.4 |
robertwharrell | 0:bcc2a42d221f | 15 | |
robertwharrell | 0:bcc2a42d221f | 16 | DigitalOut myled(LED1); |
robertwharrell | 0:bcc2a42d221f | 17 | DigitalIn leftSensor (SENSOR_L); |
robertwharrell | 0:bcc2a42d221f | 18 | DigitalIn rightSensor(SENSOR_R); |
robertwharrell | 0:bcc2a42d221f | 19 | Motor leftMotor (MOTOR_L_PWM, MOTOR_L_FWD, MOTOR_L_REV); |
robertwharrell | 0:bcc2a42d221f | 20 | Motor rightMotor(MOTOR_R_PWM, MOTOR_R_FWD, MOTOR_R_REV); |
robertwharrell | 0:bcc2a42d221f | 21 | |
robertwharrell | 0:bcc2a42d221f | 22 | void backup(void); |
robertwharrell | 0:bcc2a42d221f | 23 | void rotate_left(void); |
robertwharrell | 0:bcc2a42d221f | 24 | void rotate_right(void); |
robertwharrell | 0:bcc2a42d221f | 25 | void turn_around(void); |
robertwharrell | 0:bcc2a42d221f | 26 | |
robertwharrell | 0:bcc2a42d221f | 27 | int main() { |
robertwharrell | 0:bcc2a42d221f | 28 | //Set motors to forward |
robertwharrell | 0:bcc2a42d221f | 29 | leftMotor.speed(BASE_SPEED); |
robertwharrell | 0:bcc2a42d221f | 30 | rightMotor.speed(BASE_SPEED); |
robertwharrell | 0:bcc2a42d221f | 31 | |
robertwharrell | 0:bcc2a42d221f | 32 | while(1) { |
robertwharrell | 0:bcc2a42d221f | 33 | #ifdef DEBUG |
robertwharrell | 0:bcc2a42d221f | 34 | printf("Sensor Check:\r\n"); |
robertwharrell | 0:bcc2a42d221f | 35 | printf("Left %d\r\n", left_test.read()); |
robertwharrell | 0:bcc2a42d221f | 36 | printf("Right %d\r\n", right_test.read()); |
robertwharrell | 0:bcc2a42d221f | 37 | #endif |
robertwharrell | 0:bcc2a42d221f | 38 | |
robertwharrell | 0:bcc2a42d221f | 39 | |
robertwharrell | 0:bcc2a42d221f | 40 | if(leftSensor & rightSensor){ |
robertwharrell | 0:bcc2a42d221f | 41 | backup(); |
robertwharrell | 0:bcc2a42d221f | 42 | turn_around(); |
robertwharrell | 0:bcc2a42d221f | 43 | }else if(leftSensor){ |
robertwharrell | 0:bcc2a42d221f | 44 | backup(); |
robertwharrell | 0:bcc2a42d221f | 45 | rotate_right(); |
robertwharrell | 0:bcc2a42d221f | 46 | }else if(rightSensor){ |
robertwharrell | 0:bcc2a42d221f | 47 | backup(); |
robertwharrell | 0:bcc2a42d221f | 48 | rotate_left(); |
robertwharrell | 0:bcc2a42d221f | 49 | } |
robertwharrell | 0:bcc2a42d221f | 50 | |
robertwharrell | 0:bcc2a42d221f | 51 | leftMotor.speed(BASE_SPEED); |
robertwharrell | 0:bcc2a42d221f | 52 | rightMotor.speed(BASE_SPEED); |
robertwharrell | 0:bcc2a42d221f | 53 | myled = 1; |
robertwharrell | 0:bcc2a42d221f | 54 | wait(0.2); |
robertwharrell | 0:bcc2a42d221f | 55 | myled = 0; |
robertwharrell | 0:bcc2a42d221f | 56 | wait(.1); |
robertwharrell | 0:bcc2a42d221f | 57 | } |
robertwharrell | 0:bcc2a42d221f | 58 | } |
robertwharrell | 0:bcc2a42d221f | 59 | |
robertwharrell | 0:bcc2a42d221f | 60 | |
robertwharrell | 0:bcc2a42d221f | 61 | void backup(void){ |
robertwharrell | 0:bcc2a42d221f | 62 | leftMotor.speed (-1*BASE_SPEED); |
robertwharrell | 0:bcc2a42d221f | 63 | rightMotor.speed(-1*BASE_SPEED); |
robertwharrell | 0:bcc2a42d221f | 64 | wait(.5); |
robertwharrell | 0:bcc2a42d221f | 65 | } |
robertwharrell | 0:bcc2a42d221f | 66 | void rotate_left(void){ |
robertwharrell | 0:bcc2a42d221f | 67 | leftMotor.speed (-1*BASE_SPEED); |
robertwharrell | 0:bcc2a42d221f | 68 | rightMotor.speed(BASE_SPEED); |
robertwharrell | 0:bcc2a42d221f | 69 | wait(1); |
robertwharrell | 0:bcc2a42d221f | 70 | } |
robertwharrell | 0:bcc2a42d221f | 71 | void rotate_right(void){ |
robertwharrell | 0:bcc2a42d221f | 72 | leftMotor.speed(BASE_SPEED); |
robertwharrell | 0:bcc2a42d221f | 73 | rightMotor.speed(-1*BASE_SPEED); |
robertwharrell | 0:bcc2a42d221f | 74 | wait(1); |
robertwharrell | 0:bcc2a42d221f | 75 | } |
robertwharrell | 0:bcc2a42d221f | 76 | void turn_around(void){ |
robertwharrell | 0:bcc2a42d221f | 77 | leftMotor.speed(BASE_SPEED); |
robertwharrell | 0:bcc2a42d221f | 78 | rightMotor.speed(-1*BASE_SPEED); |
robertwharrell | 0:bcc2a42d221f | 79 | wait(2); |
robertwharrell | 0:bcc2a42d221f | 80 | } |