Like Deng / Mbed 2 deprecated Lab4_Robot_Project_1

This Lab4 Robot Car Project.

Dependencies:   LSM9DS0 Motor Servo mbed

Fork of Lab4_Robot_Project_1 by ECE4180

main.cpp@1:4b76a9beeae6, 2015-10-23 (annotated)

Committer:
ldeng31
Date:
Fri Oct 23 14:01:30 2015 +0000
Revision:
1:4b76a9beeae6
Parent:
0:8239206a2f26 
Lab4

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ldeng31 0:8239206a2f26 1 // Hello World to sweep a servo through its full range
ldeng31 1:4b76a9beeae6 2
ldeng31 0:8239206a2f26 3 #include "mbed.h"
ldeng31 0:8239206a2f26 4 #include "Servo.h"
ldeng31 0:8239206a2f26 5 #include "Motor.h"
ldeng31 0:8239206a2f26 6 #include "LSM9DS0.h"
ldeng31 1:4b76a9beeae6 7
ldeng31 0:8239206a2f26 8 // SDO_XM and SDO_G are pulled up, so our addresses are:
ldeng31 0:8239206a2f26 9 #define LSM9DS0_XM_ADDR 0x1D // Would be 0x1E if SDO_XM is LOW
ldeng31 0:8239206a2f26 10 #define LSM9DS0_G_ADDR 0x6B // Would be 0x6A if SDO_G is LOW
ldeng31 1:4b76a9beeae6 11
ldeng31 0:8239206a2f26 12 // refresh time. set to 500 for checking the Data on pc.
ldeng31 0:8239206a2f26 13 #define REFRESH_TIME_MS 500
ldeng31 1:4b76a9beeae6 14
ldeng31 0:8239206a2f26 15 LSM9DS0 imu(p9, p10, LSM9DS0_G_ADDR, LSM9DS0_XM_ADDR);//IMU, read the compass value.
ldeng31 0:8239206a2f26 16 Serial pc(USBTX, USBRX);//Only use this during the testing phase for pc debug.
ldeng31 0:8239206a2f26 17 Motor MR(p25, p6, p5); // Motor A pwm, fwd, rev
ldeng31 0:8239206a2f26 18 Motor ML(p26, p7, p8);//Motor B pwm, fwd, rev
ldeng31 0:8239206a2f26 19 DigitalOut STBY(p11); // Set STBY = 1, enable both motor; Set STBY = 0; disable both motor.
ldeng31 0:8239206a2f26 20 Servo myservo(p21);//PWM control for Servo
ldeng31 0:8239206a2f26 21 AnalogIn IR(p20); //IR sensor
ldeng31 0:8239206a2f26 22 AnalogIn LeftEncoder(p19);
ldeng31 1:4b76a9beeae6 23
ldeng31 1:4b76a9beeae6 24 //Return direction from servo - left, right or front
ldeng31 1:4b76a9beeae6 25 enum DIRC{LEFT,RIGHT,FRONT};
ldeng31 1:4b76a9beeae6 26
ldeng31 1:4b76a9beeae6 27 //Control Motor to turn right.
ldeng31 0:8239206a2f26 28 void TurnRight()
ldeng31 0:8239206a2f26 29 {
ldeng31 0:8239206a2f26 30
ldeng31 0:8239206a2f26 31 ML.speed(-0.3);
ldeng31 0:8239206a2f26 32 MR.speed(0.3);
ldeng31 0:8239206a2f26 33 wait(2.1);
ldeng31 0:8239206a2f26 34
ldeng31 0:8239206a2f26 35 }
ldeng31 1:4b76a9beeae6 36
ldeng31 1:4b76a9beeae6 37 //Control Motor to turn left.
ldeng31 0:8239206a2f26 38 void TurnLeft()
ldeng31 0:8239206a2f26 39 {
ldeng31 0:8239206a2f26 40
ldeng31 0:8239206a2f26 41 ML.speed(0.3);
ldeng31 0:8239206a2f26 42 MR.speed(-0.3);
ldeng31 0:8239206a2f26 43 wait(2.1);
ldeng31 0:8239206a2f26 44
ldeng31 0:8239206a2f26 45 }
ldeng31 1:4b76a9beeae6 46
ldeng31 1:4b76a9beeae6 47 //Control Motor to go forward.
ldeng31 1:4b76a9beeae6 48 void Forward()
ldeng31 0:8239206a2f26 49 {
ldeng31 1:4b76a9beeae6 50 ML.speed(-0.55);
ldeng31 0:8239206a2f26 51 MR.speed(-0.5);
ldeng31 0:8239206a2f26 52 wait(0.01);
ldeng31 0:8239206a2f26 53 }
ldeng31 1:4b76a9beeae6 54
ldeng31 1:4b76a9beeae6 55 //Control Motor to Backup.
ldeng31 1:4b76a9beeae6 56 void Backup()
ldeng31 1:4b76a9beeae6 57 {
ldeng31 1:4b76a9beeae6 58 ML.speed(0.55);
ldeng31 1:4b76a9beeae6 59 MR.speed(0.5);
ldeng31 1:4b76a9beeae6 60 }
ldeng31 1:4b76a9beeae6 61
ldeng31 1:4b76a9beeae6 62 //Control the servo to TurnLeft.
ldeng31 1:4b76a9beeae6 63 DIRC LookLeft()
ldeng31 0:8239206a2f26 64 {
ldeng31 0:8239206a2f26 65 myservo = 0.0;
ldeng31 1:4b76a9beeae6 66 wait(1);
ldeng31 1:4b76a9beeae6 67 return LEFT;
ldeng31 0:8239206a2f26 68 }
ldeng31 1:4b76a9beeae6 69
ldeng31 1:4b76a9beeae6 70 //Control the servo to TurnRight.
ldeng31 1:4b76a9beeae6 71 DIRC LookRight()
ldeng31 0:8239206a2f26 72 {
ldeng31 0:8239206a2f26 73 myservo = 0.9;
ldeng31 1:4b76a9beeae6 74 wait(1);
ldeng31 1:4b76a9beeae6 75 return RIGHT;
ldeng31 0:8239206a2f26 76 }
ldeng31 1:4b76a9beeae6 77
ldeng31 1:4b76a9beeae6 78 //Control the servo to LookStraight.
ldeng31 1:4b76a9beeae6 79 DIRC LookStraight()
ldeng31 0:8239206a2f26 80 {
ldeng31 0:8239206a2f26 81 myservo = 0.5;
ldeng31 1:4b76a9beeae6 82 wait(1);
ldeng31 1:4b76a9beeae6 83 return FRONT;
ldeng31 0:8239206a2f26 84 }
ldeng31 1:4b76a9beeae6 85
ldeng31 0:8239206a2f26 86 int main() {
ldeng31 0:8239206a2f26 87
ldeng31 1:4b76a9beeae6 88 STBY = 1;//enable both Motor
ldeng31 1:4b76a9beeae6 89 wait(1.0);
ldeng31 1:4b76a9beeae6 90 DIRC looking_dir;
ldeng31 1:4b76a9beeae6 91
ldeng31 0:8239206a2f26 92 while(true)
ldeng31 0:8239206a2f26 93 {
ldeng31 1:4b76a9beeae6 94 Forward(); //Constantly go straight if there is no obstacle.
ldeng31 1:4b76a9beeae6 95
ldeng31 1:4b76a9beeae6 96
ldeng31 1:4b76a9beeae6 97 while(1)//Control servo swing left and right, obtain IR value.
ldeng31 0:8239206a2f26 98 {
ldeng31 1:4b76a9beeae6 99 looking_dir = LookStraight();
ldeng31 1:4b76a9beeae6 100 if(IR>0.6) break;
ldeng31 1:4b76a9beeae6 101 looking_dir = LookLeft();
ldeng31 1:4b76a9beeae6 102 if(IR>0.6) break;
ldeng31 1:4b76a9beeae6 103 looking_dir = LookRight();
ldeng31 1:4b76a9beeae6 104 if(IR>0.6) break;
ldeng31 1:4b76a9beeae6 105
ldeng31 0:8239206a2f26 106 }
ldeng31 0:8239206a2f26 107
ldeng31 1:4b76a9beeae6 108 //Base on IR value, make correct movement.
ldeng31 1:4b76a9beeae6 109 if(looking_dir == FRONT)
ldeng31 1:4b76a9beeae6 110 {
ldeng31 1:4b76a9beeae6 111 Backup();
ldeng31 1:4b76a9beeae6 112 wait(2.0);
ldeng31 1:4b76a9beeae6 113 TurnRight();
ldeng31 1:4b76a9beeae6 114 }
ldeng31 1:4b76a9beeae6 115 else if(looking_dir == LEFT)
ldeng31 1:4b76a9beeae6 116 {
ldeng31 1:4b76a9beeae6 117 Backup();
ldeng31 1:4b76a9beeae6 118 wait(2.0);
ldeng31 1:4b76a9beeae6 119 TurnRight();
ldeng31 1:4b76a9beeae6 120 }
ldeng31 1:4b76a9beeae6 121 else
ldeng31 1:4b76a9beeae6 122 {
ldeng31 1:4b76a9beeae6 123 Backup();
ldeng31 1:4b76a9beeae6 124 wait(2.0);
ldeng31 1:4b76a9beeae6 125 TurnLeft();
ldeng31 1:4b76a9beeae6 126 }
ldeng31 0:8239206a2f26 127 }
ldeng31 1:4b76a9beeae6 128 }