something
Dependencies: mbed Motor LSM9DS1_Library_cal PinDetect HC_SR04_Ultrasonic_Library
main.cpp
- Committer:
- uwang3
- Date:
- 2018-12-08
- Revision:
- 11:f95294698901
- Parent:
- 10:2a1c8ce9d76c
- Child:
- 12:7598d38e7d44
File content as of revision 11:f95294698901:
#include "mbed.h" #include "LSM9DS1.h" // IMU #include "ultrasonic.h" // Ultrasonic Sensor #include "Motor.h" // Motor Drivers #include "PinDetect.h" // Pin Detect (for switch) // Global Variables: bool tr = true; bool turnflag = false; int leftdistancestarting= 0; int leftdistanceending= 0; //---------------------| // PIN INITIALIZATIONS | //---------------------| // Debug LED pin: //DigitalOut led(p25); DigitalIn sw(p20); Serial pc(USBTX,USBRX); // Setup Motor Driver pins: Motor Lfront(p21, p6, p5); // PWM to p21, forward to p5, reverse to p6... Motor Rfront(p22, p8, p7); // PWM to p22, forward to p7, reverse to p8... Motor Lback(p23, p10, p9); // PWM to p23, forward to p10, reverse to p9... Motor Rback(p24, p12, p11); // PWM to p24, forward to p12, reverse to p11... LSM9DS1 imu(p28, p27, 0xD6, 0x3C); Timer t; //-----------------------| // CLASS INITIALIZATIONS | //-----------------------| class mctrl { public: // Stop all motors: void stop(void) { Lfront.speed(0); Lback.speed(0); Rfront.speed(0); Rback.speed(0); wait(0.55); }; // Go forward at constant speed: void fwd(void){ //stop(); Lfront.speed(0.85); Lback.speed(0.85); Rfront.speed(0.85); Rback.speed(0.85); wait(0.02); }; // Reverse at constant speed: void rev(void){ stop(); Lfront.speed(-1); Lback.speed(-1); Rfront.speed(-1); Rback.speed(-1); wait(0.02); }; // Turn left 90 degrees: void turnLeft(){ stop(); float degree = 0.0, angularV = 0.0; float currt = t.read(); while(degree < 90) { while(imu.gyroAvailable()); imu.readGyro(); angularV = imu.gz; Lfront.speed(-1); Lback.speed(-1); Rfront.speed(1); Rback.speed(1); if(angularV > 50.0 || angularV <-50.0) { degree += (abs(angularV))/100.00; } wait(.45); } stop(); wait(0.02); }; // Turn right 90 degrees: void turnRight(){ stop(); float degree = 0.0, angularV = 0.0; float currt = t.read(); while(degree < 90) { while(imu.gyroAvailable()); imu.readGyro(); angularV = imu.gz; Lfront.speed(1); Lback.speed(1); Rfront.speed(-1); Rback.speed(-1); if(angularV > 50.0 || angularV <-50.0) { degree += (abs(angularV))/100.00; } wait(.45); } stop(); wait(0.02); }; // Turn L or R, "mag" of turn prop. to dist. from wall void turnLeftScaled(double scale) { stop(); Lfront.speed(-1 * scale); Lback.speed(-1 * scale); Rfront.speed(1 * scale); Rback.speed(1 * scale); //stop(); wait(0.02); }; void turnRightScaled(double scale) { stop(); Lfront.speed(1 * scale); Lback.speed(1 * scale); Rfront.speed(-1 * scale); Rback.speed(-1 * scale); //stop(); wait(0.02); }; } mctrl; //------------------| // HELPER FUNCTIONS | //------------------| // PUT CODE HERE TO TRIGGER WHEN DISTANCE READS LESS THAN 15cm void dist(int distance) // NOTE: by default "distance" is in mm... { if (distance < 150) { mctrl.stop(); // Turn 90 degrees float currt = t.read(); if (tr) { mctrl.turnRight(); } else { mctrl.turnLeft(); } // Go forward X (1?) number of seconds. // change dstance???? globals?? currt = t.read(); while ((t.read() - currt) < 1.25 && distance > 150) { mctrl.fwd(); } // Turn 90 degrees currt = t.read(); if (tr) { mctrl.turnRight(); } else { mctrl.turnLeft(); } tr = !tr; } else { // Continue forward until wall. mctrl.fwd(); } } void dist2(int distance) // left sensor interrupt { leftdistanceending = distance; //pc.printf(" Distance %d mm\r\n", distance); } void dist3(int distance) // right sensor interrupt { /*if (distance < 150) { pc.printf("Right Sensor trigg"); } else { pc.printf("Right Sensor echo"); }*/ //rightdistance = distance; } //------------------------------| // PIN INITIALIZATIONS (cont'd) | //------------------------------| // Setup Ultrasonic Sensor pins: ultrasonic usensor(p15, p16, .07, 1, &dist); // trigger to p13, echo to p14... // update every .07 secs w/ timeout after 1 sec... // call "dist" when the distance changes... ultrasonic usensor2(p15, p26, .07, 1, &dist2); // left sensor trigger ultrasonic usensor3(p15, p25, .07, 1, &dist3); //right sensor trigger //---------------| // MAIN FUNCTION | //---------------| int main() { // Use internal pullup for the switch: sw.mode(PullUp); // Initialize the Ultrasonic Sensor: usensor.startUpdates(); usensor2.startUpdates(); usensor3.startUpdates(); wait(0.5); // obtain and print starting distances int forwardDist = usensor.getCurrentDistance(); int leftdistancestarting = usensor2.getCurrentDistance(); int rightDist = usensor3.getCurrentDistance(); wait(0.5); pc.printf("Current Forward Starting Distance %d mm\r\n", forwardDist); pc.printf("Current Left Starting Distance %d mm\r\n", leftdistancestarting); pc.printf("Current Right Starting Distance %d mm\r\n", rightDist); // Initialize the Timer: t.start(); //while(1) { while (!sw) { usensor.checkDistance(); //determine which sensor is closest to wall int currLeftDist = usensor2.getCurrentDistance(); int currRightDist = usensor3.getCurrentDistance(); // may also have to scale the difference (40, etc.) based on - // iteration of the 90deg turn sequence (loss of accuracy) // or decrease numbers of turns //if left sensor closest to wall if (currLeftDist > 100 && currRightDist > 100) { //mctrl.rev(); mctrl.fwd(); } else { if (currLeftDist < currRightDist) { //if robot has drifted toward the wall by a lot, scale R turn prop. if ((leftdistancestarting - currLeftDist) > 40) { mctrl.turnRightScaled(1); mctrl.fwd(); //if drifted toward wall a little, scale accordingly } else if ((leftdistancestarting - currLeftDist) > 5) { mctrl.turnRightScaled(0.75); mctrl.fwd(); //if drift away a lot, scale L turn accordingly } else if ((currLeftDist - leftdistancestarting) > 40) { mctrl.turnLeftScaled(1); mctrl.fwd(); } else if ((currLeftDist - leftdistancestarting) > 5) { mctrl.turnLeftScaled(0.75); mctrl.fwd(); } //else { // mctrl.fwd(); // } } else if (currLeftDist > currRightDist) { //if robot has drifted toward the wall by a lot, scale L turn by a lot if ((rightDist - currRightDist) > 40) { mctrl.turnLeftScaled(1); mctrl.fwd(); //if drifted toward wall a little, scale accordingly } else if ((rightDist - currRightDist) > 5) { mctrl.turnLeftScaled(0.75); mctrl.fwd(); //if drift away a lot, scale R turn accordingly } else if ((currRightDist - rightDist) > 40) { mctrl.turnRightScaled(1); mctrl.fwd(); } else if ((currRightDist - rightDist) > 5) { mctrl.turnRightScaled(0.75); mctrl.fwd(); } //else { // mctrl.fwd(); // } } // may be weird when updating after turning? maybe wait/etc. leftdistancestarting = currLeftDist; rightDist = currRightDist; } //usensor2.checkDistance(); //usensor3.checkDistance(); //pc.printf("Current Ending Distance %d mm\r\n", leftdistanceending); } mctrl.stop(); //} }