This Lab4 Robot Car Project.
Dependencies: LSM9DS0 Motor Servo mbed
Fork of Lab4_Robot_Project_1 by
main.cpp@3:130a982ced94, 2015-11-21 (annotated)
- Committer:
- ldeng31
- Date:
- Sat Nov 21 17:34:19 2015 +0000
- Revision:
- 3:130a982ced94
- Parent:
- 1:4b76a9beeae6
final project
Who changed what in which revision?
User | Revision | Line number | New 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 | 3:130a982ced94 | 4 | //#include "Servo.h" |
ldeng31 | 0:8239206a2f26 | 5 | #include "Motor.h" |
ldeng31 | 3:130a982ced94 | 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 | 3:130a982ced94 | 9 | //#define LSM9DS0_XM_ADDR 0x1D // Would be 0x1E if SDO_XM is LOW |
ldeng31 | 3:130a982ced94 | 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 | 3:130a982ced94 | 13 | //#define REFRESH_TIME_MS 500 |
ldeng31 | 1:4b76a9beeae6 | 14 | |
ldeng31 | 3:130a982ced94 | 15 | //LSM9DS0 imu(p9, p10, LSM9DS0_G_ADDR, LSM9DS0_XM_ADDR);//IMU, read the compass value. |
ldeng31 | 3:130a982ced94 | 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 | 3:130a982ced94 | 20 | //Servo myservo(p21);//PWM control for Servo |
ldeng31 | 3:130a982ced94 | 21 | AnalogIn IR(p); //IR sensor |
ldeng31 | 0:8239206a2f26 | 22 | AnalogIn LeftEncoder(p19); |
ldeng31 | 3:130a982ced94 | 23 | AnalogIn RightEncoder(p20); |
ldeng31 | 1:4b76a9beeae6 | 24 | |
ldeng31 | 1:4b76a9beeae6 | 25 | //Return direction from servo - left, right or front |
ldeng31 | 3:130a982ced94 | 26 | //enum DIRC{LEFT,RIGHT,FRONT}; |
ldeng31 | 1:4b76a9beeae6 | 27 | |
ldeng31 | 1:4b76a9beeae6 | 28 | //Control Motor to turn right. |
ldeng31 | 0:8239206a2f26 | 29 | void TurnRight() |
ldeng31 | 0:8239206a2f26 | 30 | { |
ldeng31 | 0:8239206a2f26 | 31 | |
ldeng31 | 0:8239206a2f26 | 32 | ML.speed(-0.3); |
ldeng31 | 0:8239206a2f26 | 33 | MR.speed(0.3); |
ldeng31 | 3:130a982ced94 | 34 | wait(3.0); |
ldeng31 | 0:8239206a2f26 | 35 | |
ldeng31 | 0:8239206a2f26 | 36 | } |
ldeng31 | 1:4b76a9beeae6 | 37 | |
ldeng31 | 1:4b76a9beeae6 | 38 | //Control Motor to turn left. |
ldeng31 | 0:8239206a2f26 | 39 | void TurnLeft() |
ldeng31 | 0:8239206a2f26 | 40 | { |
ldeng31 | 0:8239206a2f26 | 41 | |
ldeng31 | 0:8239206a2f26 | 42 | ML.speed(0.3); |
ldeng31 | 0:8239206a2f26 | 43 | MR.speed(-0.3); |
ldeng31 | 3:130a982ced94 | 44 | wait(3.0); |
ldeng31 | 0:8239206a2f26 | 45 | |
ldeng31 | 0:8239206a2f26 | 46 | } |
ldeng31 | 1:4b76a9beeae6 | 47 | |
ldeng31 | 1:4b76a9beeae6 | 48 | //Control Motor to go forward. |
ldeng31 | 1:4b76a9beeae6 | 49 | void Forward() |
ldeng31 | 0:8239206a2f26 | 50 | { |
ldeng31 | 1:4b76a9beeae6 | 51 | ML.speed(-0.55); |
ldeng31 | 3:130a982ced94 | 52 | MR.speed(-0.55); |
ldeng31 | 0:8239206a2f26 | 53 | wait(0.01); |
ldeng31 | 0:8239206a2f26 | 54 | } |
ldeng31 | 1:4b76a9beeae6 | 55 | |
ldeng31 | 1:4b76a9beeae6 | 56 | //Control Motor to Backup. |
ldeng31 | 1:4b76a9beeae6 | 57 | void Backup() |
ldeng31 | 1:4b76a9beeae6 | 58 | { |
ldeng31 | 1:4b76a9beeae6 | 59 | ML.speed(0.55); |
ldeng31 | 1:4b76a9beeae6 | 60 | MR.speed(0.5); |
ldeng31 | 1:4b76a9beeae6 | 61 | } |
ldeng31 | 1:4b76a9beeae6 | 62 | |
ldeng31 | 1:4b76a9beeae6 | 63 | //Control the servo to TurnLeft. |
ldeng31 | 3:130a982ced94 | 64 | //DIRC LookLeft() |
ldeng31 | 3:130a982ced94 | 65 | //{ |
ldeng31 | 3:130a982ced94 | 66 | // myservo = 0.0; |
ldeng31 | 3:130a982ced94 | 67 | //wait(1); |
ldeng31 | 3:130a982ced94 | 68 | //return LEFT; |
ldeng31 | 3:130a982ced94 | 69 | //} |
ldeng31 | 1:4b76a9beeae6 | 70 | |
ldeng31 | 1:4b76a9beeae6 | 71 | //Control the servo to TurnRight. |
ldeng31 | 3:130a982ced94 | 72 | //DIRC LookRight() |
ldeng31 | 3:130a982ced94 | 73 | //{ |
ldeng31 | 3:130a982ced94 | 74 | // myservo = 0.9; |
ldeng31 | 3:130a982ced94 | 75 | //wait(1); |
ldeng31 | 3:130a982ced94 | 76 | //return RIGHT; |
ldeng31 | 3:130a982ced94 | 77 | //} |
ldeng31 | 1:4b76a9beeae6 | 78 | |
ldeng31 | 1:4b76a9beeae6 | 79 | //Control the servo to LookStraight. |
ldeng31 | 3:130a982ced94 | 80 | //DIRC LookStraight() |
ldeng31 | 3:130a982ced94 | 81 | //{ |
ldeng31 | 3:130a982ced94 | 82 | // myservo = 0.5; |
ldeng31 | 3:130a982ced94 | 83 | //wait(1); |
ldeng31 | 3:130a982ced94 | 84 | //return FRONT; |
ldeng31 | 3:130a982ced94 | 85 | //} |
ldeng31 | 1:4b76a9beeae6 | 86 | |
ldeng31 | 0:8239206a2f26 | 87 | int main() { |
ldeng31 | 0:8239206a2f26 | 88 | |
ldeng31 | 1:4b76a9beeae6 | 89 | STBY = 1;//enable both Motor |
ldeng31 | 1:4b76a9beeae6 | 90 | wait(1.0); |
ldeng31 | 3:130a982ced94 | 91 | // DIRC looking_dir; |
ldeng31 | 1:4b76a9beeae6 | 92 | |
ldeng31 | 0:8239206a2f26 | 93 | while(true) |
ldeng31 | 0:8239206a2f26 | 94 | { |
ldeng31 | 1:4b76a9beeae6 | 95 | Forward(); //Constantly go straight if there is no obstacle. |
ldeng31 | 1:4b76a9beeae6 | 96 | |
ldeng31 | 3:130a982ced94 | 97 | } |
ldeng31 | 0:8239206a2f26 | 98 | |
ldeng31 | 1:4b76a9beeae6 | 99 | } |