mohammad karim / Mbed 2 deprecated 4180_FINAL_PROJECTUricascode

something

Dependencies:   mbed Motor LSM9DS1_Library_cal PinDetect HC_SR04_Ultrasonic_Library

main.cpp@1:92f6242c0196, 2018-11-19 (annotated)

Committer:
cbrice6
Date:
Mon Nov 19 18:41:30 2018 +0000
Revision:
1:92f6242c0196
Parent:
0:9e014841f2b7
Child:
3:c07ea8bf242e
2nd Commit (fixed mctrl and turning interrupt)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cbrice6 0:9e014841f2b7 1 #include "mbed.h"
cbrice6 0:9e014841f2b7 2 #include "LSM9DS1.h" // IMU
cbrice6 0:9e014841f2b7 3 #include "ultrasonic.h" // Ultrasonic Sensor
cbrice6 0:9e014841f2b7 4 #include "Motor.h" // Motor Drivers
cbrice6 0:9e014841f2b7 5
cbrice6 0:9e014841f2b7 6
cbrice6 1:92f6242c0196 7 // Global Variables:
cbrice6 1:92f6242c0196 8 float start;
cbrice6 1:92f6242c0196 9 float heading;
cbrice6 1:92f6242c0196 10 bool turnflag = false;
cbrice6 1:92f6242c0196 11
cbrice6 0:9e014841f2b7 12 //---------------------|
cbrice6 0:9e014841f2b7 13 // PIN INITIALIZATIONS |
cbrice6 0:9e014841f2b7 14 //---------------------|
cbrice6 0:9e014841f2b7 15 // Setup IMU pins:
cbrice6 0:9e014841f2b7 16 LSM9DS1 IMU(p28, p27, 0xD6, 0x3C); // SDA to p28, SCL to p27..
cbrice6 0:9e014841f2b7 17
cbrice6 0:9e014841f2b7 18 // Setup Motor Driver pins:
cbrice6 0:9e014841f2b7 19 Motor Lfront(p21, p6, p5); // PWM to p21, forward to p6, reverse to p5...
cbrice6 1:92f6242c0196 20 Motor Rfront(p22, p8, p7); // PWM to p22, forward to p8, reverse to p7...
cbrice6 1:92f6242c0196 21 Motor Lback(p23, p10, p9); // PWM to p23, forward to p10, reverse to p9...
cbrice6 0:9e014841f2b7 22 Motor Rback(p24, p12, p11); // PWM to p24, forward to p12, reverse to p11...
cbrice6 0:9e014841f2b7 23
cbrice6 0:9e014841f2b7 24 Timer t;
cbrice6 0:9e014841f2b7 25
cbrice6 0:9e014841f2b7 26 //-----------------------|
cbrice6 0:9e014841f2b7 27 // CLASS INITIALIZATIONS |
cbrice6 0:9e014841f2b7 28 //-----------------------|
cbrice6 0:9e014841f2b7 29 class mctrl {
cbrice6 0:9e014841f2b7 30 public:
cbrice6 1:92f6242c0196 31 int LeftCount;
cbrice6 1:92f6242c0196 32 int RightCount;
cbrice6 1:92f6242c0196 33
cbrice6 1:92f6242c0196 34 // Stop all motors:
cbrice6 1:92f6242c0196 35 void stop(void) {
cbrice6 1:92f6242c0196 36 Lfront.speed(0);
cbrice6 1:92f6242c0196 37 Lback.speed(0);
cbrice6 1:92f6242c0196 38 Rfront.speed(0);
cbrice6 1:92f6242c0196 39 Rback.speed(0);
cbrice6 1:92f6242c0196 40 wait(0.02);
cbrice6 1:92f6242c0196 41 };
cbrice6 1:92f6242c0196 42 // Go forward at constant speed:
cbrice6 1:92f6242c0196 43 void fwd(void){
cbrice6 1:92f6242c0196 44 stop();
cbrice6 1:92f6242c0196 45
cbrice6 1:92f6242c0196 46 Lfront.speed(1);
cbrice6 1:92f6242c0196 47 Lback.speed(1);
cbrice6 1:92f6242c0196 48 Rfront.speed(1);
cbrice6 1:92f6242c0196 49 Rback.speed(1);
cbrice6 1:92f6242c0196 50 wait(0.02);
cbrice6 1:92f6242c0196 51 };
cbrice6 1:92f6242c0196 52 // Reverse at constant speed:
cbrice6 1:92f6242c0196 53 void rev(void){
cbrice6 1:92f6242c0196 54 stop();
cbrice6 1:92f6242c0196 55
cbrice6 1:92f6242c0196 56 Lfront.speed(-1);
cbrice6 1:92f6242c0196 57 Lback.speed(-1);
cbrice6 1:92f6242c0196 58 Rfront.speed(-1);
cbrice6 1:92f6242c0196 59 Rback.speed(-1);
cbrice6 1:92f6242c0196 60 wait(0.02);
cbrice6 1:92f6242c0196 61 };
cbrice6 1:92f6242c0196 62 // Turn left 90 degrees:
cbrice6 1:92f6242c0196 63 void turnLeft(void){
cbrice6 1:92f6242c0196 64 stop();
cbrice6 1:92f6242c0196 65
cbrice6 1:92f6242c0196 66 while ((heading-start) < 90) {
cbrice6 1:92f6242c0196 67 Lfront.speed(-1);
cbrice6 1:92f6242c0196 68 Lback.speed(-1);
cbrice6 1:92f6242c0196 69 Rfront.speed(1);
cbrice6 1:92f6242c0196 70 Rback.speed(1);
cbrice6 1:92f6242c0196 71 }
cbrice6 1:92f6242c0196 72
cbrice6 1:92f6242c0196 73 LeftCount++;
cbrice6 1:92f6242c0196 74
cbrice6 1:92f6242c0196 75 stop();
cbrice6 1:92f6242c0196 76 wait(0.02);
cbrice6 1:92f6242c0196 77 };
cbrice6 1:92f6242c0196 78 // Turn right 90 degrees:
cbrice6 1:92f6242c0196 79 void turnRight(void){
cbrice6 1:92f6242c0196 80 stop();
cbrice6 1:92f6242c0196 81
cbrice6 1:92f6242c0196 82 while ((heading-start) < 90) {
cbrice6 1:92f6242c0196 83 Lfront.speed(1);
cbrice6 1:92f6242c0196 84 Lback.speed(1);
cbrice6 1:92f6242c0196 85 Rfront.speed(-1);
cbrice6 1:92f6242c0196 86 Rback.speed(-1);
cbrice6 1:92f6242c0196 87 }
cbrice6 1:92f6242c0196 88
cbrice6 1:92f6242c0196 89 RightCount++;
cbrice6 1:92f6242c0196 90
cbrice6 1:92f6242c0196 91 stop();
cbrice6 1:92f6242c0196 92 wait(0.02);
cbrice6 1:92f6242c0196 93 };
cbrice6 1:92f6242c0196 94 } mctrl;
cbrice6 0:9e014841f2b7 95
cbrice6 0:9e014841f2b7 96 //------------------|
cbrice6 0:9e014841f2b7 97 // HELPER FUNCTIONS |
cbrice6 0:9e014841f2b7 98 //------------------|
cbrice6 0:9e014841f2b7 99 void dist(int distance) // NOTE: by default "distance" is in mm...
cbrice6 0:9e014841f2b7 100 {
cbrice6 1:92f6242c0196 101 if (distance < 150 || turnflag == true) {
cbrice6 1:92f6242c0196 102 turnflag = true;
cbrice6 1:92f6242c0196 103
cbrice6 1:92f6242c0196 104 mctrl.stop();
cbrice6 0:9e014841f2b7 105 // PUT CODE HERE TO TRIGGER WHEN DISTANCE READS LESS THAN 15cm
cbrice6 0:9e014841f2b7 106 // 1) Turn right.
cbrice6 0:9e014841f2b7 107 // 2) Go forward __ seconds.
cbrice6 0:9e014841f2b7 108 // 3) Turn right.
cbrice6 0:9e014841f2b7 109 // 4) Continue forward until wall.
cbrice6 1:92f6242c0196 110 mctrl.turnRight();
cbrice6 1:92f6242c0196 111 float currt = t.read();
cbrice6 1:92f6242c0196 112 while ((t.read()-currt) < 3) {
cbrice6 1:92f6242c0196 113 mctrl.fwd();
cbrice6 1:92f6242c0196 114 }
cbrice6 1:92f6242c0196 115 mctrl.turnRight();
cbrice6 1:92f6242c0196 116 if (mctrl.RightCount == 2) {
cbrice6 1:92f6242c0196 117 turnflag = false;
cbrice6 1:92f6242c0196 118 mctrl.RightCount = 0;
cbrice6 1:92f6242c0196 119 }
cbrice6 1:92f6242c0196 120 }
cbrice6 1:92f6242c0196 121 else {
cbrice6 1:92f6242c0196 122 mctrl.fwd();
cbrice6 0:9e014841f2b7 123 }
cbrice6 0:9e014841f2b7 124 }
cbrice6 0:9e014841f2b7 125
cbrice6 0:9e014841f2b7 126 //------------------------------|
cbrice6 0:9e014841f2b7 127 // PIN INITIALIZATIONS (cont'd) |
cbrice6 0:9e014841f2b7 128 //------------------------------|
cbrice6 0:9e014841f2b7 129 // Setup Ultrasonic Sensor pins:
cbrice6 0:9e014841f2b7 130 ultrasonic usensor(p13, p14, .07, 1, &dist); // trigger to p13, echo to p14...
cbrice6 0:9e014841f2b7 131 // update every .07 secs w/ timeout after 1 sec...
cbrice6 0:9e014841f2b7 132 // call "dist" when the distance changes...
cbrice6 0:9e014841f2b7 133
cbrice6 0:9e014841f2b7 134 //---------------|
cbrice6 0:9e014841f2b7 135 // MAIN FUNCTION |
cbrice6 0:9e014841f2b7 136 //---------------|
cbrice6 0:9e014841f2b7 137 int main() {
cbrice6 0:9e014841f2b7 138 // Initialize and calibrate the IMU:
cbrice6 1:92f6242c0196 139 //IMU.begin();
cbrice6 1:92f6242c0196 140 //if (!IMU.begin()) {
cbrice6 1:92f6242c0196 141 // Do something if it doesn't detect the IMU...
cbrice6 1:92f6242c0196 142 //}
cbrice6 1:92f6242c0196 143 //IMU.calibrate();
cbrice6 1:92f6242c0196 144
cbrice6 1:92f6242c0196 145 //IMU.readGyro();
cbrice6 1:92f6242c0196 146 //start = IMU.calcGyro(IMU.gx);
cbrice6 0:9e014841f2b7 147
cbrice6 0:9e014841f2b7 148 // Initialize the Ultrasonic Sensor:
cbrice6 0:9e014841f2b7 149 usensor.startUpdates();
cbrice6 0:9e014841f2b7 150
cbrice6 1:92f6242c0196 151 // Initialize the Motor turn counts:
cbrice6 1:92f6242c0196 152 mctrl.LeftCount = 0;
cbrice6 1:92f6242c0196 153 mctrl.RightCount = 0;
cbrice6 1:92f6242c0196 154
cbrice6 0:9e014841f2b7 155 // Initialize the Timer:
cbrice6 0:9e014841f2b7 156 t.start();
cbrice6 0:9e014841f2b7 157
cbrice6 0:9e014841f2b7 158 while(1) {
cbrice6 0:9e014841f2b7 159 // Read IMU gyro:
cbrice6 1:92f6242c0196 160 //while(!IMU.gyroAvailable());
cbrice6 1:92f6242c0196 161 //IMU.readGyro();
cbrice6 1:92f6242c0196 162 //heading = IMU.calcGyro(IMU.gx);
cbrice6 0:9e014841f2b7 163 // X - IMU.calcGyro(IMU.gx)
cbrice6 1:92f6242c0196 164 // Y - IMU.calcGyro(IMU.gy)
cbrice6 0:9e014841f2b7 165 // Z - IMU.calcGyro(IMU.gz)
cbrice6 0:9e014841f2b7 166
cbrice6 0:9e014841f2b7 167 // Read Ultrasonic Sensor:
cbrice6 0:9e014841f2b7 168 usensor.checkDistance();
cbrice6 0:9e014841f2b7 169
cbrice6 0:9e014841f2b7 170 wait(0.1);
cbrice6 0:9e014841f2b7 171 }
cbrice6 0:9e014841f2b7 172 }