mohammad karim / Mbed 2 deprecated 4180_FINAL_PROJECTUricascode

something

Dependencies:   mbed Motor LSM9DS1_Library_cal PinDetect HC_SR04_Ultrasonic_Library

main.cpp@3:c07ea8bf242e, 2018-11-21 (annotated)

Committer:
cbrice6
Date:
Wed Nov 21 19:30:56 2018 +0000
Revision:
3:c07ea8bf242e
Parent:
1:92f6242c0196
Child:
4:6546c17ac0a6
Fixed a bunch of shit;

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 3:c07ea8bf242e 5 #include "PinDetect.h" // Pin Detect (for switch)
cbrice6 0:9e014841f2b7 6
cbrice6 0:9e014841f2b7 7
cbrice6 1:92f6242c0196 8 // Global Variables:
cbrice6 3:c07ea8bf242e 9 bool tr = true;
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 3:c07ea8bf242e 15 // Debug LED pin:
cbrice6 3:c07ea8bf242e 16 //DigitalOut led(p25);
cbrice6 3:c07ea8bf242e 17
cbrice6 3:c07ea8bf242e 18 // Motor Power Control pins:
cbrice6 3:c07ea8bf242e 19 /*
cbrice6 3:c07ea8bf242e 20 DigitalOut Ctrl1(p29);
cbrice6 3:c07ea8bf242e 21 DigitalOut Ctrl2(p30);
cbrice6 3:c07ea8bf242e 22 PinDetect sw(p20);
cbrice6 3:c07ea8bf242e 23 */
cbrice6 0:9e014841f2b7 24
cbrice6 0:9e014841f2b7 25 // Setup Motor Driver pins:
cbrice6 0:9e014841f2b7 26 Motor Lfront(p21, p6, p5); // PWM to p21, forward to p6, reverse to p5...
cbrice6 1:92f6242c0196 27 Motor Rfront(p22, p8, p7); // PWM to p22, forward to p8, reverse to p7...
cbrice6 1:92f6242c0196 28 Motor Lback(p23, p10, p9); // PWM to p23, forward to p10, reverse to p9...
cbrice6 0:9e014841f2b7 29 Motor Rback(p24, p12, p11); // PWM to p24, forward to p12, reverse to p11...
cbrice6 0:9e014841f2b7 30
cbrice6 0:9e014841f2b7 31 Timer t;
cbrice6 0:9e014841f2b7 32
cbrice6 0:9e014841f2b7 33 //-----------------------|
cbrice6 0:9e014841f2b7 34 // CLASS INITIALIZATIONS |
cbrice6 0:9e014841f2b7 35 //-----------------------|
cbrice6 0:9e014841f2b7 36 class mctrl {
cbrice6 0:9e014841f2b7 37 public:
cbrice6 1:92f6242c0196 38 // Stop all motors:
cbrice6 1:92f6242c0196 39 void stop(void) {
cbrice6 1:92f6242c0196 40 Lfront.speed(0);
cbrice6 1:92f6242c0196 41 Lback.speed(0);
cbrice6 1:92f6242c0196 42 Rfront.speed(0);
cbrice6 1:92f6242c0196 43 Rback.speed(0);
cbrice6 1:92f6242c0196 44 wait(0.02);
cbrice6 1:92f6242c0196 45 };
cbrice6 3:c07ea8bf242e 46
cbrice6 1:92f6242c0196 47 // Go forward at constant speed:
cbrice6 1:92f6242c0196 48 void fwd(void){
cbrice6 1:92f6242c0196 49 stop();
cbrice6 1:92f6242c0196 50
cbrice6 1:92f6242c0196 51 Lfront.speed(1);
cbrice6 1:92f6242c0196 52 Lback.speed(1);
cbrice6 1:92f6242c0196 53 Rfront.speed(1);
cbrice6 1:92f6242c0196 54 Rback.speed(1);
cbrice6 1:92f6242c0196 55 wait(0.02);
cbrice6 1:92f6242c0196 56 };
cbrice6 3:c07ea8bf242e 57
cbrice6 1:92f6242c0196 58 // Reverse at constant speed:
cbrice6 1:92f6242c0196 59 void rev(void){
cbrice6 1:92f6242c0196 60 stop();
cbrice6 1:92f6242c0196 61 Lfront.speed(-1);
cbrice6 1:92f6242c0196 62 Lback.speed(-1);
cbrice6 1:92f6242c0196 63 Rfront.speed(-1);
cbrice6 1:92f6242c0196 64 Rback.speed(-1);
cbrice6 1:92f6242c0196 65 wait(0.02);
cbrice6 1:92f6242c0196 66 };
cbrice6 3:c07ea8bf242e 67
cbrice6 1:92f6242c0196 68 // Turn left 90 degrees:
cbrice6 1:92f6242c0196 69 void turnLeft(void){
cbrice6 1:92f6242c0196 70 stop();
cbrice6 3:c07ea8bf242e 71 while ((t.read()-currt) < 2) {
cbrice6 1:92f6242c0196 72 Lfront.speed(-1);
cbrice6 1:92f6242c0196 73 Lback.speed(-1);
cbrice6 1:92f6242c0196 74 Rfront.speed(1);
cbrice6 1:92f6242c0196 75 Rback.speed(1);
cbrice6 1:92f6242c0196 76 }
cbrice6 1:92f6242c0196 77 stop();
cbrice6 1:92f6242c0196 78 wait(0.02);
cbrice6 1:92f6242c0196 79 };
cbrice6 3:c07ea8bf242e 80
cbrice6 1:92f6242c0196 81 // Turn right 90 degrees:
cbrice6 1:92f6242c0196 82 void turnRight(void){
cbrice6 1:92f6242c0196 83 stop();
cbrice6 3:c07ea8bf242e 84 while ((t.read()-currt) < 2) {
cbrice6 1:92f6242c0196 85 Lfront.speed(1);
cbrice6 1:92f6242c0196 86 Lback.speed(1);
cbrice6 1:92f6242c0196 87 Rfront.speed(-1);
cbrice6 1:92f6242c0196 88 Rback.speed(-1);
cbrice6 1:92f6242c0196 89 }
cbrice6 1:92f6242c0196 90 stop();
cbrice6 1:92f6242c0196 91 wait(0.02);
cbrice6 1:92f6242c0196 92 };
cbrice6 1:92f6242c0196 93 } mctrl;
cbrice6 0:9e014841f2b7 94
cbrice6 0:9e014841f2b7 95 //------------------|
cbrice6 0:9e014841f2b7 96 // HELPER FUNCTIONS |
cbrice6 0:9e014841f2b7 97 //------------------|
cbrice6 0:9e014841f2b7 98 void dist(int distance) // NOTE: by default "distance" is in mm...
cbrice6 0:9e014841f2b7 99 {
cbrice6 3:c07ea8bf242e 100 if (distance < 150) {
cbrice6 3:c07ea8bf242e 101 // PUT CODE HERE TO TRIGGER WHEN DISTANCE READS LESS THAN 15cm
cbrice6 3:c07ea8bf242e 102 // 1) Turn 90 degrees (hardcoded => 2 seconds).
cbrice6 3:c07ea8bf242e 103 // 2) Go forward 2 seconds.
cbrice6 3:c07ea8bf242e 104 // 3) Turn 90 degrees (hardcoded => 2 seconds).
cbrice6 3:c07ea8bf242e 105 // 4) Continue forward until wall.
cbrice6 3:c07ea8bf242e 106 // [Step 1]
cbrice6 1:92f6242c0196 107 float currt = t.read();
cbrice6 3:c07ea8bf242e 108 if (tr) {mctrl.turnRight(currt)} else {mctrl.turnLeft(currt)}
cbrice6 3:c07ea8bf242e 109 // [Step 2]
cbrice6 3:c07ea8bf242e 110 currt = t.read();
cbrice6 3:c07ea8bf242e 111 while ((t.read()-currt) < 2) {mctrl.fwd()}
cbrice6 3:c07ea8bf242e 112 // [Step 3]
cbrice6 3:c07ea8bf242e 113 currt = t.read();
cbrice6 3:c07ea8bf242e 114 if (tr) {mctrl.turnRight(currt)} else {mctrl.turnLeft(currt)}
cbrice6 3:c07ea8bf242e 115 // [End]
cbrice6 3:c07ea8bf242e 116 tr = !tr;
cbrice6 1:92f6242c0196 117 }
cbrice6 1:92f6242c0196 118 else {
cbrice6 1:92f6242c0196 119 mctrl.fwd();
cbrice6 0:9e014841f2b7 120 }
cbrice6 0:9e014841f2b7 121 }
cbrice6 0:9e014841f2b7 122
cbrice6 0:9e014841f2b7 123 //------------------------------|
cbrice6 0:9e014841f2b7 124 // PIN INITIALIZATIONS (cont'd) |
cbrice6 0:9e014841f2b7 125 //------------------------------|
cbrice6 0:9e014841f2b7 126 // Setup Ultrasonic Sensor pins:
cbrice6 0:9e014841f2b7 127 ultrasonic usensor(p13, p14, .07, 1, &dist); // trigger to p13, echo to p14...
cbrice6 0:9e014841f2b7 128 // update every .07 secs w/ timeout after 1 sec...
cbrice6 0:9e014841f2b7 129 // call "dist" when the distance changes...
cbrice6 0:9e014841f2b7 130
cbrice6 0:9e014841f2b7 131 //---------------|
cbrice6 0:9e014841f2b7 132 // MAIN FUNCTION |
cbrice6 0:9e014841f2b7 133 //---------------|
cbrice6 0:9e014841f2b7 134 int main() {
cbrice6 3:c07ea8bf242e 135 // Use internal pullup for the switch:
cbrice6 3:c07ea8bf242e 136 //sw.mode(PullUp);
cbrice6 3:c07ea8bf242e 137
cbrice6 3:c07ea8bf242e 138 // Delay for initial pullups to take effect:
cbrice6 3:c07ea8bf242e 139 //wait(0.01);
cbrice6 3:c07ea8bf242e 140
cbrice6 0:9e014841f2b7 141 // Initialize and calibrate the IMU:
cbrice6 3:c07ea8bf242e 142 /*
cbrice6 3:c07ea8bf242e 143 LSM9DS1 imu(p28, p27, 0xD6, 0x3C); // SDA to p28, SCL to p27...
cbrice6 3:c07ea8bf242e 144 imu.begin();
cbrice6 3:c07ea8bf242e 145 imu.calibrate(1);
cbrice6 3:c07ea8bf242e 146 imu.calibrateMag(0);
cbrice6 1:92f6242c0196 147
cbrice6 3:c07ea8bf242e 148 imu.readGyro();
cbrice6 3:c07ea8bf242e 149 start = imu.gz;
cbrice6 3:c07ea8bf242e 150 */
cbrice6 0:9e014841f2b7 151
cbrice6 0:9e014841f2b7 152 // Initialize the Ultrasonic Sensor:
cbrice6 0:9e014841f2b7 153 usensor.startUpdates();
cbrice6 0:9e014841f2b7 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 3:c07ea8bf242e 159 // Toggle motor battery banks:
cbrice6 3:c07ea8bf242e 160 /*
cbrice6 3:c07ea8bf242e 161 if (!sw == 1) {
cbrice6 3:c07ea8bf242e 162 Ctrl1 = 1;
cbrice6 3:c07ea8bf242e 163 Ctrl2 = 1;
cbrice6 3:c07ea8bf242e 164 wait(0.2);
cbrice6 3:c07ea8bf242e 165 }
cbrice6 3:c07ea8bf242e 166 else {
cbrice6 3:c07ea8bf242e 167 Ctrl1 = 0;
cbrice6 3:c07ea8bf242e 168 Ctrl2 = 0;
cbrice6 3:c07ea8bf242e 169 wait(0.2);
cbrice6 3:c07ea8bf242e 170 }
cbrice6 3:c07ea8bf242e 171 */
cbrice6 3:c07ea8bf242e 172
cbrice6 0:9e014841f2b7 173 // Read IMU gyro:
cbrice6 3:c07ea8bf242e 174 //while(!imu.gyroAvailable());
cbrice6 3:c07ea8bf242e 175 //imu.readGyro();
cbrice6 3:c07ea8bf242e 176 //heading = imu.calcGyro(imu.gz);
cbrice6 3:c07ea8bf242e 177 // X - imu.calcGyro(imu.gx)
cbrice6 3:c07ea8bf242e 178 // Y - imu.calcGyro(imu.gy)
cbrice6 3:c07ea8bf242e 179 // Z - imu.calcGyro(imu.gz)
cbrice6 3:c07ea8bf242e 180
cbrice6 3:c07ea8bf242e 181 //if (heading > 90) {led=1;} else {led=0;}
cbrice6 0:9e014841f2b7 182
cbrice6 0:9e014841f2b7 183 // Read Ultrasonic Sensor:
cbrice6 0:9e014841f2b7 184 usensor.checkDistance();
cbrice6 0:9e014841f2b7 185
cbrice6 0:9e014841f2b7 186 wait(0.1);
cbrice6 0:9e014841f2b7 187 }
cbrice6 0:9e014841f2b7 188 }
cbrice6 3:c07ea8bf242e 189