something
Dependencies: mbed Motor LSM9DS1_Library_cal PinDetect HC_SR04_Ultrasonic_Library
main.cpp@0:9e014841f2b7, 2018-11-19 (annotated)
- Committer:
- cbrice6
- Date:
- Mon Nov 19 01:08:21 2018 +0000
- Revision:
- 0:9e014841f2b7
- Child:
- 1:92f6242c0196
First commit (template)
Who changed what in which revision?
User | Revision | Line number | New 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 | 0:9e014841f2b7 | 7 | //---------------------| |
cbrice6 | 0:9e014841f2b7 | 8 | // PIN INITIALIZATIONS | |
cbrice6 | 0:9e014841f2b7 | 9 | //---------------------| |
cbrice6 | 0:9e014841f2b7 | 10 | // Setup IMU pins: |
cbrice6 | 0:9e014841f2b7 | 11 | LSM9DS1 IMU(p28, p27, 0xD6, 0x3C); // SDA to p28, SCL to p27.. |
cbrice6 | 0:9e014841f2b7 | 12 | |
cbrice6 | 0:9e014841f2b7 | 13 | // Setup Motor Driver pins: |
cbrice6 | 0:9e014841f2b7 | 14 | Motor Lfront(p21, p6, p5); // PWM to p21, forward to p6, reverse to p5... |
cbrice6 | 0:9e014841f2b7 | 15 | Motor Lback(p22, p8, p7); // PWM to p22, forward to p8, reverse to p7... |
cbrice6 | 0:9e014841f2b7 | 16 | Motor Rfront(p23, p10, p9); // PWM to p23, forward to p10, reverse to p9... |
cbrice6 | 0:9e014841f2b7 | 17 | Motor Rback(p24, p12, p11); // PWM to p24, forward to p12, reverse to p11... |
cbrice6 | 0:9e014841f2b7 | 18 | |
cbrice6 | 0:9e014841f2b7 | 19 | Timer t; |
cbrice6 | 0:9e014841f2b7 | 20 | |
cbrice6 | 0:9e014841f2b7 | 21 | //-----------------------| |
cbrice6 | 0:9e014841f2b7 | 22 | // CLASS INITIALIZATIONS | |
cbrice6 | 0:9e014841f2b7 | 23 | //-----------------------| |
cbrice6 | 0:9e014841f2b7 | 24 | class mctrl { |
cbrice6 | 0:9e014841f2b7 | 25 | public: |
cbrice6 | 0:9e014841f2b7 | 26 | void stop(void); |
cbrice6 | 0:9e014841f2b7 | 27 | void fwd(float s); |
cbrice6 | 0:9e014841f2b7 | 28 | void rev(float s); |
cbrice6 | 0:9e014841f2b7 | 29 | void turnLeft(float s); |
cbrice6 | 0:9e014841f2b7 | 30 | void turnRight(float s); |
cbrice6 | 0:9e014841f2b7 | 31 | private: |
cbrice6 | 0:9e014841f2b7 | 32 | float tcurr; |
cbrice6 | 0:9e014841f2b7 | 33 | float turntime |
cbrice6 | 0:9e014841f2b7 | 34 | }; |
cbrice6 | 0:9e014841f2b7 | 35 | |
cbrice6 | 0:9e014841f2b7 | 36 | //------------------| |
cbrice6 | 0:9e014841f2b7 | 37 | // HELPER FUNCTIONS | |
cbrice6 | 0:9e014841f2b7 | 38 | //------------------| |
cbrice6 | 0:9e014841f2b7 | 39 | void mctrl::stop(void) |
cbrice6 | 0:9e014841f2b7 | 40 | { |
cbrice6 | 0:9e014841f2b7 | 41 | Lfront.speed(0); |
cbrice6 | 0:9e014841f2b7 | 42 | Lback.speed(0); |
cbrice6 | 0:9e014841f2b7 | 43 | Rfront.speed(0); |
cbrice6 | 0:9e014841f2b7 | 44 | Rback.speed(0); |
cbrice6 | 0:9e014841f2b7 | 45 | wait(0.02); |
cbrice6 | 0:9e014841f2b7 | 46 | } |
cbrice6 | 0:9e014841f2b7 | 47 | |
cbrice6 | 0:9e014841f2b7 | 48 | void mctrl::fwd(float s) |
cbrice6 | 0:9e014841f2b7 | 49 | { |
cbrice6 | 0:9e014841f2b7 | 50 | mctrl.stop(); |
cbrice6 | 0:9e014841f2b7 | 51 | |
cbrice6 | 0:9e014841f2b7 | 52 | Lfront.speed(s); |
cbrice6 | 0:9e014841f2b7 | 53 | Lback.speed(s); |
cbrice6 | 0:9e014841f2b7 | 54 | Rfront.speed(s); |
cbrice6 | 0:9e014841f2b7 | 55 | Rback.speed(s); |
cbrice6 | 0:9e014841f2b7 | 56 | wait(0.02); |
cbrice6 | 0:9e014841f2b7 | 57 | } |
cbrice6 | 0:9e014841f2b7 | 58 | |
cbrice6 | 0:9e014841f2b7 | 59 | void mctrl::rev(float s) |
cbrice6 | 0:9e014841f2b7 | 60 | { |
cbrice6 | 0:9e014841f2b7 | 61 | mctrl.stop(); |
cbrice6 | 0:9e014841f2b7 | 62 | |
cbrice6 | 0:9e014841f2b7 | 63 | Lfront.speed(-s); |
cbrice6 | 0:9e014841f2b7 | 64 | Lback.speed(-s); |
cbrice6 | 0:9e014841f2b7 | 65 | Rfront.speed(-s); |
cbrice6 | 0:9e014841f2b7 | 66 | Rback.speed(-s); |
cbrice6 | 0:9e014841f2b7 | 67 | wait(0.02); |
cbrice6 | 0:9e014841f2b7 | 68 | } |
cbrice6 | 0:9e014841f2b7 | 69 | |
cbrice6 | 0:9e014841f2b7 | 70 | void mctrl::turnLeft(float s) |
cbrice6 | 0:9e014841f2b7 | 71 | { |
cbrice6 | 0:9e014841f2b7 | 72 | mctrl.stop(); |
cbrice6 | 0:9e014841f2b7 | 73 | |
cbrice6 | 0:9e014841f2b7 | 74 | tcurr = t.read(); |
cbrice6 | 0:9e014841f2b7 | 75 | while ((t.read() - tcurr) < turntime) { |
cbrice6 | 0:9e014841f2b7 | 76 | Lfront.speed(-s); |
cbrice6 | 0:9e014841f2b7 | 77 | Lback.speed(-s); |
cbrice6 | 0:9e014841f2b7 | 78 | Rfront.speed(s); |
cbrice6 | 0:9e014841f2b7 | 79 | Rback.speed(s); |
cbrice6 | 0:9e014841f2b7 | 80 | } |
cbrice6 | 0:9e014841f2b7 | 81 | |
cbrice6 | 0:9e014841f2b7 | 82 | mctrl.stop(); |
cbrice6 | 0:9e014841f2b7 | 83 | wait(0.02); |
cbrice6 | 0:9e014841f2b7 | 84 | } |
cbrice6 | 0:9e014841f2b7 | 85 | |
cbrice6 | 0:9e014841f2b7 | 86 | void mctrl::turnRight(float s) |
cbrice6 | 0:9e014841f2b7 | 87 | { |
cbrice6 | 0:9e014841f2b7 | 88 | mctrl.stop(); |
cbrice6 | 0:9e014841f2b7 | 89 | |
cbrice6 | 0:9e014841f2b7 | 90 | tcurr = t.read(); |
cbrice6 | 0:9e014841f2b7 | 91 | while ((t.read() - tcurr) < turntime) { |
cbrice6 | 0:9e014841f2b7 | 92 | Lfront.speed(s); |
cbrice6 | 0:9e014841f2b7 | 93 | Lback.speed(s); |
cbrice6 | 0:9e014841f2b7 | 94 | Rfront.speed(-s); |
cbrice6 | 0:9e014841f2b7 | 95 | Rback.speed(-s); |
cbrice6 | 0:9e014841f2b7 | 96 | } |
cbrice6 | 0:9e014841f2b7 | 97 | |
cbrice6 | 0:9e014841f2b7 | 98 | mctrl.stop(); |
cbrice6 | 0:9e014841f2b7 | 99 | wait(0.02); |
cbrice6 | 0:9e014841f2b7 | 100 | } |
cbrice6 | 0:9e014841f2b7 | 101 | |
cbrice6 | 0:9e014841f2b7 | 102 | void dist(int distance) // NOTE: by default "distance" is in mm... |
cbrice6 | 0:9e014841f2b7 | 103 | { |
cbrice6 | 0:9e014841f2b7 | 104 | if (distance < 150) { |
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 | 0:9e014841f2b7 | 110 | } |
cbrice6 | 0:9e014841f2b7 | 111 | } |
cbrice6 | 0:9e014841f2b7 | 112 | |
cbrice6 | 0:9e014841f2b7 | 113 | //------------------------------| |
cbrice6 | 0:9e014841f2b7 | 114 | // PIN INITIALIZATIONS (cont'd) | |
cbrice6 | 0:9e014841f2b7 | 115 | //------------------------------| |
cbrice6 | 0:9e014841f2b7 | 116 | // Setup Ultrasonic Sensor pins: |
cbrice6 | 0:9e014841f2b7 | 117 | ultrasonic usensor(p13, p14, .07, 1, &dist); // trigger to p13, echo to p14... |
cbrice6 | 0:9e014841f2b7 | 118 | // update every .07 secs w/ timeout after 1 sec... |
cbrice6 | 0:9e014841f2b7 | 119 | // call "dist" when the distance changes... |
cbrice6 | 0:9e014841f2b7 | 120 | |
cbrice6 | 0:9e014841f2b7 | 121 | //---------------| |
cbrice6 | 0:9e014841f2b7 | 122 | // MAIN FUNCTION | |
cbrice6 | 0:9e014841f2b7 | 123 | //---------------| |
cbrice6 | 0:9e014841f2b7 | 124 | int main() { |
cbrice6 | 0:9e014841f2b7 | 125 | // Initialize and calibrate the IMU: |
cbrice6 | 0:9e014841f2b7 | 126 | IMU.begin(); |
cbrice6 | 0:9e014841f2b7 | 127 | if (!IMU.begin()) { |
cbrice6 | 0:9e014841f2b7 | 128 | pc.printf("Failed to communicate with LSM9DS1.\n"); |
cbrice6 | 0:9e014841f2b7 | 129 | } |
cbrice6 | 0:9e014841f2b7 | 130 | IMU.calibrate(); |
cbrice6 | 0:9e014841f2b7 | 131 | |
cbrice6 | 0:9e014841f2b7 | 132 | // Initialize the Ultrasonic Sensor: |
cbrice6 | 0:9e014841f2b7 | 133 | usensor.startUpdates(); |
cbrice6 | 0:9e014841f2b7 | 134 | |
cbrice6 | 0:9e014841f2b7 | 135 | // Initialize the Timer: |
cbrice6 | 0:9e014841f2b7 | 136 | t.start(); |
cbrice6 | 0:9e014841f2b7 | 137 | |
cbrice6 | 0:9e014841f2b7 | 138 | while(1) { |
cbrice6 | 0:9e014841f2b7 | 139 | // Read IMU gyro: |
cbrice6 | 0:9e014841f2b7 | 140 | while(!IMU.gyroAvailable()); |
cbrice6 | 0:9e014841f2b7 | 141 | IMU.readGyro(); |
cbrice6 | 0:9e014841f2b7 | 142 | // X - IMU.calcGyro(IMU.gx) |
cbrice6 | 0:9e014841f2b7 | 143 | // Y - IMU.calcGyro(IMU.gx) |
cbrice6 | 0:9e014841f2b7 | 144 | // Z - IMU.calcGyro(IMU.gz) |
cbrice6 | 0:9e014841f2b7 | 145 | |
cbrice6 | 0:9e014841f2b7 | 146 | // Read Ultrasonic Sensor: |
cbrice6 | 0:9e014841f2b7 | 147 | usensor.checkDistance(); |
cbrice6 | 0:9e014841f2b7 | 148 | |
cbrice6 | 0:9e014841f2b7 | 149 | // ... |
cbrice6 | 0:9e014841f2b7 | 150 | |
cbrice6 | 0:9e014841f2b7 | 151 | wait(0.1); |
cbrice6 | 0:9e014841f2b7 | 152 | } |
cbrice6 | 0:9e014841f2b7 | 153 | } |