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@0:8239206a2f26, 2015-10-20 (annotated)

Committer:
ldeng31
Date:
Tue Oct 20 03:01:16 2015 +0000
Revision:
0:8239206a2f26
Child:
1:4b76a9beeae6
Child:
2:26960ab3c751
This is Robot Car Project.;

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 0:8239206a2f26 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 0:8239206a2f26 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 0:8239206a2f26 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 0:8239206a2f26 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 0:8239206a2f26 23
ldeng31 0:8239206a2f26 24 void TurnRight()
ldeng31 0:8239206a2f26 25 {
ldeng31 0:8239206a2f26 26 //imu.readMag();
ldeng31 0:8239206a2f26 27 /*float current_dir = imu.calcHeading();
ldeng31 0:8239206a2f26 28 if (current_dir>=270)
ldeng31 0:8239206a2f26 29 {
ldeng31 0:8239206a2f26 30 current_dir -=360;
ldeng31 0:8239206a2f26 31 }
ldeng31 0:8239206a2f26 32
ldeng31 0:8239206a2f26 33 float target_dir = current_dir + 90;
ldeng31 0:8239206a2f26 34 */
ldeng31 0:8239206a2f26 35
ldeng31 0:8239206a2f26 36 //pc.printf("%.2f",current_dir);
ldeng31 0:8239206a2f26 37 //pc.printf(" Targe: %.2f\n",target_dir);
ldeng31 0:8239206a2f26 38 ML.speed(-0.3);
ldeng31 0:8239206a2f26 39 MR.speed(0.3);
ldeng31 0:8239206a2f26 40 //imu.readMag();
ldeng31 0:8239206a2f26 41
ldeng31 0:8239206a2f26 42 //current_dir = imu.calcHeading();
ldeng31 0:8239206a2f26 43
ldeng31 0:8239206a2f26 44 /*if (current_dir>=270)
ldeng31 0:8239206a2f26 45 {
ldeng31 0:8239206a2f26 46 current_dir -=360;
ldeng31 0:8239206a2f26 47 }*/
ldeng31 0:8239206a2f26 48 wait(2.1);
ldeng31 0:8239206a2f26 49
ldeng31 0:8239206a2f26 50 ML.speed(0.0);
ldeng31 0:8239206a2f26 51 MR.speed(0.0);
ldeng31 0:8239206a2f26 52 }
ldeng31 0:8239206a2f26 53
ldeng31 0:8239206a2f26 54 void TurnLeft()
ldeng31 0:8239206a2f26 55 {
ldeng31 0:8239206a2f26 56
ldeng31 0:8239206a2f26 57 ML.speed(0.3);
ldeng31 0:8239206a2f26 58 MR.speed(-0.3);
ldeng31 0:8239206a2f26 59
ldeng31 0:8239206a2f26 60 wait(2.1);
ldeng31 0:8239206a2f26 61
ldeng31 0:8239206a2f26 62 ML.speed(0.0);
ldeng31 0:8239206a2f26 63 MR.speed(0.0);
ldeng31 0:8239206a2f26 64 }
ldeng31 0:8239206a2f26 65
ldeng31 0:8239206a2f26 66 void Straight()
ldeng31 0:8239206a2f26 67 {
ldeng31 0:8239206a2f26 68 ML.speed(-0.5);
ldeng31 0:8239206a2f26 69 MR.speed(-0.5);
ldeng31 0:8239206a2f26 70 wait(0.01);
ldeng31 0:8239206a2f26 71 }
ldeng31 0:8239206a2f26 72
ldeng31 0:8239206a2f26 73 void LookLeft()
ldeng31 0:8239206a2f26 74 {
ldeng31 0:8239206a2f26 75 myservo = 0.0;
ldeng31 0:8239206a2f26 76
ldeng31 0:8239206a2f26 77 }
ldeng31 0:8239206a2f26 78
ldeng31 0:8239206a2f26 79 void LookRight()
ldeng31 0:8239206a2f26 80 {
ldeng31 0:8239206a2f26 81 myservo = 0.9;
ldeng31 0:8239206a2f26 82 }
ldeng31 0:8239206a2f26 83
ldeng31 0:8239206a2f26 84 void LookStraight()
ldeng31 0:8239206a2f26 85 {
ldeng31 0:8239206a2f26 86 myservo = 0.5;
ldeng31 0:8239206a2f26 87 }
ldeng31 0:8239206a2f26 88
ldeng31 0:8239206a2f26 89 int main() {
ldeng31 0:8239206a2f26 90
ldeng31 0:8239206a2f26 91 STBY = 1;
ldeng31 0:8239206a2f26 92 pc.baud(9600);
ldeng31 0:8239206a2f26 93 imu.begin();
ldeng31 0:8239206a2f26 94 LookStraight();
ldeng31 0:8239206a2f26 95 wait(2.0);
ldeng31 0:8239206a2f26 96 while(true)
ldeng31 0:8239206a2f26 97 {
ldeng31 0:8239206a2f26 98 if(IR>0.6)
ldeng31 0:8239206a2f26 99 {
ldeng31 0:8239206a2f26 100 LookLeft();
ldeng31 0:8239206a2f26 101 wait(2.0);
ldeng31 0:8239206a2f26 102 if(IR >0.6);
ldeng31 0:8239206a2f26 103 {
ldeng31 0:8239206a2f26 104 LookRight();
ldeng31 0:8239206a2f26 105 wait(2.0);
ldeng31 0:8239206a2f26 106 }
ldeng31 0:8239206a2f26 107 }
ldeng31 0:8239206a2f26 108 // Straight();
ldeng31 0:8239206a2f26 109
ldeng31 0:8239206a2f26 110 // wait(10.0);
ldeng31 0:8239206a2f26 111 // TurnRight();
ldeng31 0:8239206a2f26 112 // wait(2.0);
ldeng31 0:8239206a2f26 113 //TurnLeft();
ldeng31 0:8239206a2f26 114
ldeng31 0:8239206a2f26 115 //LookLeft();
ldeng31 0:8239206a2f26 116 //wait(2);
ldeng31 0:8239206a2f26 117 //myservo = 0.5;
ldeng31 0:8239206a2f26 118 //imu.readMag();
ldeng31 0:8239206a2f26 119 //float heading_turn = imu.calcHeading();
ldeng31 0:8239206a2f26 120 //float Turn_Right = heading + 90;
ldeng31 0:8239206a2f26 121
ldeng31 0:8239206a2f26 122 //if((heading_turn - Turn_Right)<0.1)
ldeng31 0:8239206a2f26 123 //{
ldeng31 0:8239206a2f26 124 // MA.speed(1.0f); MB.speed(1.0f);
ldeng31 0:8239206a2f26 125 // pc.printf("%.2f\n",LeftEncoder.read());
ldeng31 0:8239206a2f26 126 // wait(5);
ldeng31 0:8239206a2f26 127 else
ldeng31 0:8239206a2f26 128 LookStraight();
ldeng31 0:8239206a2f26 129 wait(1.0);
ldeng31 0:8239206a2f26 130 }
ldeng31 0:8239206a2f26 131
ldeng31 0:8239206a2f26 132
ldeng31 0:8239206a2f26 133
ldeng31 0:8239206a2f26 134 }