Robert Harrell / Mbed 2 deprecated EBIO-Project-Robot-POLL

Code for Embedded Systems I/O class project

Dependencies:   Motor mbed

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?

UserRevisionLine numberNew 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 }