This Lab4 Robot Car Project.
Dependencies: LSM9DS0 Motor Servo mbed
Fork of Lab4_Robot_Project_1 by
main.cpp
- Committer:
- ycai47
- Date:
- 2015-10-22
- Revision:
- 2:26960ab3c751
- Parent:
- 0:8239206a2f26
File content as of revision 2:26960ab3c751:
// Ojbective: // Sweep servo fullrange from left to right (with left having the priority) to determine the next move. // Robot will not move to obstacle closer than 10 cm, turn away instead #include "mbed.h" #include "Servo.h" #include "Motor.h" #include "LSM9DS0.h" // SDO_XM and SDO_G are pulled up, so our addresses are: #define LSM9DS0_XM_ADDR 0x1D // Would be 0x1E if SDO_XM is LOW #define LSM9DS0_G_ADDR 0x6B // Would be 0x6A if SDO_G is LOW // refresh time. set to 500 for checking the Data on pc. #define REFRESH_TIME_MS 500 // IR reading of 10 cm converted to 0-1 scale #define WALL 0.7 LSM9DS0 imu(p9, p10, LSM9DS0_G_ADDR, LSM9DS0_XM_ADDR);//IMU, read the compass value. Serial pc(USBTX, USBRX);//Only use this during the testing phase for pc debug. Motor MR(p25, p6, p5); // Motor A pwm, fwd, rev Motor ML(p26, p7, p8);//Motor B pwm, fwd, rev Servo myservo(p21);//PWM control for Servo AnalogIn IR(p20); //IR sensor DigitalOut STBY(p11); // Set STBY = 1, enable both motor; Set STBY = 0; disable both motor. AnalogIn LeftEncoder(p19); AnalogIn RightEncoder(p18); // Adjust wheel speed using rotary magnetic encoder void Adjust() { //myservo = 0.5; //imu.readMag(); //float heading_turn = imu.calcHeading(); //float Turn_Right = heading + 90; //if((heading_turn - Turn_Right)<0.1) //{ // MA.speed(1.0f); MB.speed(1.0f); // pc.printf("%.2f\n",LeftEncoder.read()); // wait(5); float left = LeftEncoder.read(); float right = RightEncoder.read(); float diff = abs(left - right); pc.printf("left: %.2f, right: %.2f, diff: %.2f\n", left, right, diff); //TO DO: Utilize diff data to adjust speeds of two wheels } void TurnRight(float deg) { imu.readMag(); float current_deg = imu.calcHeading(); if (current_deg >= 270) { current_deg -= 360; } //pc.printf("Initial degree: %.2f ", current_deg); float target_deg = current_deg + deg; //pc.printf("\nTarget degree: %.2f\n", target_deg); while(current_deg < target_deg) { ML.speed(-0.5); MR.speed(0.5); wait(0.001); // update imu.readMag(); current_deg = imu.calcHeading(); if (current_deg >= 270) { current_deg -=360; } //pc.printf("Current degree: %.2f ", current_deg); } ML.speed(0.0); MR.speed(0.0); } void TurnLeft(float deg) { imu.readMag(); float current_deg = imu.calcHeading(); if (current_deg <= 90) { current_deg += 360; } pc.printf("Initial degree: %.2f ", current_deg); float target_deg = current_deg - deg; pc.printf("\nTarget degree: %.2f\n", target_deg); while(current_deg > target_deg) { ML.speed(0.5); MR.speed(-0.5); wait(0.001); // update imu.readMag(); current_deg = imu.calcHeading(); if (current_deg <= 90) { current_deg += 360; } //pc.printf("Current degree: %.2f ", current_deg); } ML.speed(0.0); MR.speed(0.0); } void Backout() { ML.speed(-0.5); MR.speed(-0.5); Adjust(); wait(5.0); } void Forward() { ML.speed(0.5); MR.speed(0.5); Adjust(); wait(5.0); } void LookLeft() { myservo = myservo - 0.1; } void LookRight() { myservo = myservo + 0.1; } void LookStraight() { myservo = 0.5; } int main() { //float deg; STBY = 1; pc.baud(9600); uint16_t status = imu.begin(); pc.printf("LSM9DS0 WHO_AM_I's returned: 0x%X\n", status); pc.printf("Should be 0x49D4\n\n"); LookStraight(); wait(2.0); //while(true) // { // TurnLeft(20.0); // } // testing /* while(true) { if(IR >= WALL) { if (myservo <= 0.5 && myservo > 0.0) { LookLeft(); wait(1.0); } // Look further left if (myservo > 0.5 && myservo < 1.0) { LookRight(); wait(1.0); } // Look further right if (myservo == 0.0) { LookStraight(); LookRight(); wait(1.0); } // When reaches leftmost bound, reset to middle and start to look right if (myservo == 1.0) { Backout(); } // When reaches rightmost bound, ie. sweeped the fullrange, backout else { pc.printf("servo error."); } wait(5.0); } else { // myservo has a range of 0.0 - 1.0 if (myservo > 0.5) { deg = (myservo - 0.5) * 360; TurnRight(deg); } else { deg = (0.5 - myservo) * 360; TurnLeft(deg); } LookStraight(); Forward(); wait(1.0); } } */ }