Robert Harrell
/
EBIO-Project-Robot-POLL
Code for Embedded Systems I/O class project
main.cpp
- Committer:
- robertwharrell
- Date:
- 2014-06-11
- Revision:
- 0:bcc2a42d221f
File content as of revision 0:bcc2a42d221f:
#include "mbed.h" #include "Motor.h" //#define DEBUG 1 #define SENSOR_L p30 #define SENSOR_R p29 #define MOTOR_L_PWM p26 #define MOTOR_L_FWD p25 #define MOTOR_L_REV p24 #define MOTOR_R_PWM p21 #define MOTOR_R_FWD p23 #define MOTOR_R_REV p22 #define BASE_SPEED 0.4 DigitalOut myled(LED1); DigitalIn leftSensor (SENSOR_L); DigitalIn rightSensor(SENSOR_R); Motor leftMotor (MOTOR_L_PWM, MOTOR_L_FWD, MOTOR_L_REV); Motor rightMotor(MOTOR_R_PWM, MOTOR_R_FWD, MOTOR_R_REV); void backup(void); void rotate_left(void); void rotate_right(void); void turn_around(void); int main() { //Set motors to forward leftMotor.speed(BASE_SPEED); rightMotor.speed(BASE_SPEED); while(1) { #ifdef DEBUG printf("Sensor Check:\r\n"); printf("Left %d\r\n", left_test.read()); printf("Right %d\r\n", right_test.read()); #endif if(leftSensor & rightSensor){ backup(); turn_around(); }else if(leftSensor){ backup(); rotate_right(); }else if(rightSensor){ backup(); rotate_left(); } leftMotor.speed(BASE_SPEED); rightMotor.speed(BASE_SPEED); myled = 1; wait(0.2); myled = 0; wait(.1); } } void backup(void){ leftMotor.speed (-1*BASE_SPEED); rightMotor.speed(-1*BASE_SPEED); wait(.5); } void rotate_left(void){ leftMotor.speed (-1*BASE_SPEED); rightMotor.speed(BASE_SPEED); wait(1); } void rotate_right(void){ leftMotor.speed(BASE_SPEED); rightMotor.speed(-1*BASE_SPEED); wait(1); } void turn_around(void){ leftMotor.speed(BASE_SPEED); rightMotor.speed(-1*BASE_SPEED); wait(2); }