Azra Ismail / Mbed 2 deprecated Lab4miniproject

A robot that can detect objects and determine if the object is magnetic while drawing a map of its motion and the object's position.

Dependencies:   4DGL-uLCD-SE Motordriver mbed-rtos mbed

main.cpp@1:5a36b4e912da, 2016-11-02 (annotated)

Committer:
aismail1997
Date:
Wed Nov 02 13:37:33 2016 +0000
Revision:
1:5a36b4e912da
Parent:
0:ec413378ee3a 
lab4

Who changed what in which revision?

UserRevisionLine numberNew contents of line
aismail1997 0:ec413378ee3a 1 #include "mbed.h"
aismail1997 0:ec413378ee3a 2 #include "motordriver.h"
aismail1997 0:ec413378ee3a 3 #include "LSM9DS1.h"
aismail1997 0:ec413378ee3a 4 #include "uLCD_4DGL.h"
aismail1997 0:ec413378ee3a 5 #include "rtos.h"
aismail1997 0:ec413378ee3a 6
aismail1997 0:ec413378ee3a 7 // LCD stuff
aismail1997 0:ec413378ee3a 8 uLCD_4DGL uLCD(p13,p14,p15); // serial tx, serial rx, reset pin;
aismail1997 0:ec413378ee3a 9
aismail1997 0:ec413378ee3a 10 // motor stuff
aismail1997 0:ec413378ee3a 11 Motor left(p21, p26, p25, 1); // pwm, fwd, rev, has brake feature (pb,b2,b2)
aismail1997 0:ec413378ee3a 12 Motor right(p22, p24, p23, 1);
aismail1997 0:ec413378ee3a 13
aismail1997 0:ec413378ee3a 14 // sonar stuff
aismail1997 0:ec413378ee3a 15 // sonar 1
aismail1997 0:ec413378ee3a 16 DigitalOut trigger(p6);
aismail1997 0:ec413378ee3a 17 DigitalIn echo(p7);
aismail1997 0:ec413378ee3a 18 int distance = 0;
aismail1997 0:ec413378ee3a 19 int correction = 0;
aismail1997 0:ec413378ee3a 20 Timer sonar;
aismail1997 0:ec413378ee3a 21 // sonar 2
aismail1997 0:ec413378ee3a 22 /*
aismail1997 0:ec413378ee3a 23 DigitalOut trigger2(p12); // fix this
aismail1997 0:ec413378ee3a 24 DigitalIn echo2(p11); // fix this
aismail1997 0:ec413378ee3a 25 int distance2 = 0;
aismail1997 0:ec413378ee3a 26 int correction2 = 0;
aismail1997 0:ec413378ee3a 27 Timer sonar2;
aismail1997 0:ec413378ee3a 28 */
aismail1997 0:ec413378ee3a 29 // bluetooth stuff
aismail1997 0:ec413378ee3a 30 Serial pc(USBTX, USBRX); // and IMU
aismail1997 0:ec413378ee3a 31 Serial blue(p28,p27);
aismail1997 0:ec413378ee3a 32
aismail1997 0:ec413378ee3a 33 // Mutex for LCD
aismail1997 0:ec413378ee3a 34 Mutex lcd_mutex;
aismail1997 0:ec413378ee3a 35
aismail1997 0:ec413378ee3a 36 // IMU stuff
aismail1997 0:ec413378ee3a 37 LSM9DS1 imu(p9, p10, 0xD6, 0x3C);
aismail1997 0:ec413378ee3a 38 int old_y = 0;
aismail1997 0:ec413378ee3a 39 int old_x = SIZE_X/2;
aismail1997 0:ec413378ee3a 40
aismail1997 0:ec413378ee3a 41 void sonar_thread(void const *args)
aismail1997 0:ec413378ee3a 42 {
aismail1997 0:ec413378ee3a 43 // SETUP SONAR 1
aismail1997 0:ec413378ee3a 44 sonar.reset();
aismail1997 0:ec413378ee3a 45 sonar.start(); // start timer
aismail1997 0:ec413378ee3a 46 // min software polling delay to read echo pin
aismail1997 0:ec413378ee3a 47 while (echo==2) {};
aismail1997 0:ec413378ee3a 48 sonar.stop(); // stop timer
aismail1997 0:ec413378ee3a 49 correction = sonar.read_us();
aismail1997 0:ec413378ee3a 50 /* // SETUP SONAR 2
aismail1997 0:ec413378ee3a 51 sonar2.reset();
aismail1997 0:ec413378ee3a 52 sonar2.start();
aismail1997 0:ec413378ee3a 53 while (echo2==2) {};
aismail1997 0:ec413378ee3a 54 sonar2.stop();
aismail1997 0:ec413378ee3a 55 correction2 = sonar2.read_us();
aismail1997 0:ec413378ee3a 56 */
aismail1997 0:ec413378ee3a 57 while(true) { // thread loop
aismail1997 0:ec413378ee3a 58 // SONAR CODE
aismail1997 0:ec413378ee3a 59 trigger = 1; // trigger sonar to send a ping
aismail1997 0:ec413378ee3a 60 sonar.reset();
aismail1997 0:ec413378ee3a 61 wait_us(10.0);
aismail1997 0:ec413378ee3a 62 trigger = 0;
aismail1997 0:ec413378ee3a 63 while (echo==0) {}; //wait for echo high
aismail1997 0:ec413378ee3a 64 sonar.start(); //echo high, so start timer
aismail1997 0:ec413378ee3a 65 while (echo==1) {}; //wait for echo low
aismail1997 0:ec413378ee3a 66 sonar.stop(); //stop timer and read value
aismail1997 0:ec413378ee3a 67 //subtract software overhead timer delay and scale to cm
aismail1997 0:ec413378ee3a 68 distance = (sonar.read_us()-correction)/58.0;
aismail1997 0:ec413378ee3a 69
aismail1997 0:ec413378ee3a 70 /* // SONAR 2 CODE
aismail1997 0:ec413378ee3a 71 trigger2 = 1;
aismail1997 0:ec413378ee3a 72 sonar2.reset();
aismail1997 0:ec413378ee3a 73 wait_us(10.0);
aismail1997 0:ec413378ee3a 74 trigger2 = 0;
aismail1997 0:ec413378ee3a 75 while (echo2==0) {};
aismail1997 0:ec413378ee3a 76 sonar2.start();
aismail1997 0:ec413378ee3a 77 while (echo2==1) {};
aismail1997 0:ec413378ee3a 78 sonar2.stop();
aismail1997 0:ec413378ee3a 79 distance2 = (sonar2.read_us()-correction2)/58.0; */
aismail1997 0:ec413378ee3a 80 //wait(0.2);
aismail1997 0:ec413378ee3a 81 // if object detected using sonar 1 & 2 - draw a circle
aismail1997 0:ec413378ee3a 82 if (distance <= 8) {
aismail1997 0:ec413378ee3a 83 lcd_mutex.lock();
aismail1997 0:ec413378ee3a 84 uLCD.filled_circle(old_x-2, old_y + 4, 2, RED);
aismail1997 0:ec413378ee3a 85 lcd_mutex.unlock();
aismail1997 0:ec413378ee3a 86 }
aismail1997 0:ec413378ee3a 87 //pc.printf("1: %d cm \n\r",distance);
aismail1997 0:ec413378ee3a 88 Thread::wait(1000); // wait till thread is done
aismail1997 0:ec413378ee3a 89 }
aismail1997 0:ec413378ee3a 90 }
aismail1997 0:ec413378ee3a 91
aismail1997 0:ec413378ee3a 92 void imu_thread(void const *args)
aismail1997 0:ec413378ee3a 93 {
aismail1997 0:ec413378ee3a 94 // SETUP IMU
aismail1997 0:ec413378ee3a 95 imu.begin();
aismail1997 0:ec413378ee3a 96 //if (!imu.begin()) {
aismail1997 0:ec413378ee3a 97 //pc.printf("Failed to communicate with LSM9DS1.\n");
aismail1997 0:ec413378ee3a 98 //}
aismail1997 0:ec413378ee3a 99 imu.calibrate();
aismail1997 0:ec413378ee3a 100 int magnitude = 0;
aismail1997 0:ec413378ee3a 101 //pc.baud(9600);
aismail1997 0:ec413378ee3a 102
aismail1997 0:ec413378ee3a 103 while(true) { // thread loop
aismail1997 0:ec413378ee3a 104 // IMU CODE
aismail1997 0:ec413378ee3a 105 imu.readMag();
aismail1997 0:ec413378ee3a 106 // determine magnitude of the magnetic field
aismail1997 0:ec413378ee3a 107 magnitude = (imu.mx*imu.mx + imu.my*imu.my + imu.mz*imu.mz);
aismail1997 0:ec413378ee3a 108 //pc.printf("mag: %d %d %d %d\n\n\r", imu.mx, imu.my, imu.mz, magnitude);
aismail1997 0:ec413378ee3a 109 if (magnitude > 30000000) {
aismail1997 0:ec413378ee3a 110 lcd_mutex.lock();
aismail1997 0:ec413378ee3a 111 uLCD.filled_circle(old_x+2, old_y + 4, 2, LGREY);
aismail1997 0:ec413378ee3a 112 lcd_mutex.unlock();
aismail1997 0:ec413378ee3a 113 }
aismail1997 0:ec413378ee3a 114 Thread::wait(200); // wait till thread is done
aismail1997 0:ec413378ee3a 115 }
aismail1997 0:ec413378ee3a 116 }
aismail1997 0:ec413378ee3a 117
aismail1997 0:ec413378ee3a 118 int main()
aismail1997 0:ec413378ee3a 119 {
aismail1997 0:ec413378ee3a 120 // pc print
aismail1997 0:ec413378ee3a 121 //pc.baud(9600);
aismail1997 0:ec413378ee3a 122
aismail1997 0:ec413378ee3a 123 // lcd initialization
aismail1997 0:ec413378ee3a 124 uLCD.cls();
aismail1997 0:ec413378ee3a 125 int y = 0;
aismail1997 0:ec413378ee3a 126 int x = SIZE_X/2;
aismail1997 0:ec413378ee3a 127 float theta = 0;
aismail1997 0:ec413378ee3a 128
aismail1997 0:ec413378ee3a 129 // bluetooth stuff
aismail1997 0:ec413378ee3a 130 char bnum=0;
aismail1997 0:ec413378ee3a 131 char bhit=0;
aismail1997 0:ec413378ee3a 132 float s = 0;
aismail1997 0:ec413378ee3a 133
aismail1997 0:ec413378ee3a 134 Thread t2(imu_thread); //start imu thread
aismail1997 0:ec413378ee3a 135 Thread t1(sonar_thread); //start sonar thread
aismail1997 0:ec413378ee3a 136
aismail1997 0:ec413378ee3a 137 while(1) {
aismail1997 0:ec413378ee3a 138 // BLUETOOTH CODE
aismail1997 0:ec413378ee3a 139 if (blue.getc()=='!') {
aismail1997 0:ec413378ee3a 140 if (blue.getc()=='B') { //button data packet
aismail1997 0:ec413378ee3a 141 bnum = blue.getc(); //button number
aismail1997 0:ec413378ee3a 142 bhit = blue.getc(); //1=hit, 0=release
aismail1997 0:ec413378ee3a 143 if (blue.getc()==char(~('!' + 'B' + bnum + bhit))) { //checksum OK?
aismail1997 0:ec413378ee3a 144 switch (bnum) {
aismail1997 0:ec413378ee3a 145 case '1': //number button 1
aismail1997 0:ec413378ee3a 146 if (bhit=='1') {
aismail1997 0:ec413378ee3a 147 // MOTOR CODE
aismail1997 0:ec413378ee3a 148 // move forward
aismail1997 0:ec413378ee3a 149 left.speed(0.5);
aismail1997 0:ec413378ee3a 150 right.speed(0.5);
aismail1997 0:ec413378ee3a 151 wait(0.4);
aismail1997 0:ec413378ee3a 152 //}
aismail1997 0:ec413378ee3a 153 left.stop(1);
aismail1997 0:ec413378ee3a 154 right.stop(1);
aismail1997 0:ec413378ee3a 155 y = 6*cos(theta);
aismail1997 0:ec413378ee3a 156 x = 6*sin(theta);
aismail1997 0:ec413378ee3a 157 lcd_mutex.lock();
aismail1997 0:ec413378ee3a 158 uLCD.line(old_x,old_y,old_x+x,old_y+y,GREEN);
aismail1997 0:ec413378ee3a 159 lcd_mutex.unlock();
aismail1997 0:ec413378ee3a 160 old_x += x;
aismail1997 0:ec413378ee3a 161 old_y += y;
aismail1997 0:ec413378ee3a 162 wait(0.25);
aismail1997 0:ec413378ee3a 163 }
aismail1997 0:ec413378ee3a 164 break;
aismail1997 0:ec413378ee3a 165 case '2': //number button 2
aismail1997 0:ec413378ee3a 166 if (bhit=='1') {
aismail1997 0:ec413378ee3a 167 theta += -3.14/6;
aismail1997 0:ec413378ee3a 168 // MOTOR CODE
aismail1997 0:ec413378ee3a 169 // turn right
aismail1997 0:ec413378ee3a 170 s=(-1)*(0.55);
aismail1997 0:ec413378ee3a 171 left.speed(0.55);
aismail1997 0:ec413378ee3a 172 right.speed(s);
aismail1997 0:ec413378ee3a 173 wait(0.25);
aismail1997 0:ec413378ee3a 174 left.stop(1);
aismail1997 0:ec413378ee3a 175 right.stop(1);
aismail1997 0:ec413378ee3a 176 wait(0.25);
aismail1997 0:ec413378ee3a 177 }
aismail1997 0:ec413378ee3a 178 break;
aismail1997 0:ec413378ee3a 179 case '3': //number button 3
aismail1997 0:ec413378ee3a 180 if (bhit=='1') {
aismail1997 0:ec413378ee3a 181 theta += 3.14/6;
aismail1997 0:ec413378ee3a 182 // MOTOR CODE
aismail1997 0:ec413378ee3a 183 // turn left
aismail1997 0:ec413378ee3a 184 s=(-1)*(0.55);
aismail1997 0:ec413378ee3a 185 left.speed(s);
aismail1997 0:ec413378ee3a 186 right.speed(0.55);
aismail1997 0:ec413378ee3a 187 wait(0.25);
aismail1997 0:ec413378ee3a 188 left.stop(1);
aismail1997 0:ec413378ee3a 189 right.stop(1);
aismail1997 0:ec413378ee3a 190 wait(0.25);
aismail1997 0:ec413378ee3a 191 }
aismail1997 0:ec413378ee3a 192 break;
aismail1997 0:ec413378ee3a 193 case '4': //number button 4
aismail1997 0:ec413378ee3a 194 if (bhit=='1') {
aismail1997 0:ec413378ee3a 195 // MOTOR CODE
aismail1997 0:ec413378ee3a 196 // move back
aismail1997 0:ec413378ee3a 197 s=(-1)*(0.5);
aismail1997 0:ec413378ee3a 198 left.speed(s);
aismail1997 0:ec413378ee3a 199 right.speed(s);
aismail1997 0:ec413378ee3a 200 wait(0.4);
aismail1997 0:ec413378ee3a 201 left.stop(1);
aismail1997 0:ec413378ee3a 202 right.stop(1);
aismail1997 0:ec413378ee3a 203 y = 6*cos(theta);
aismail1997 0:ec413378ee3a 204 x = 6*sin(theta);
aismail1997 0:ec413378ee3a 205 lcd_mutex.lock();
aismail1997 0:ec413378ee3a 206 uLCD.line(old_x,old_y,old_x-x,old_y-y,RED);
aismail1997 0:ec413378ee3a 207 lcd_mutex.unlock();
aismail1997 0:ec413378ee3a 208 old_x += -x;
aismail1997 0:ec413378ee3a 209 old_y += -y;
aismail1997 0:ec413378ee3a 210 wait(0.25);
aismail1997 0:ec413378ee3a 211 }
aismail1997 0:ec413378ee3a 212 break;
aismail1997 0:ec413378ee3a 213 }
aismail1997 0:ec413378ee3a 214 }
aismail1997 0:ec413378ee3a 215 }
aismail1997 0:ec413378ee3a 216 }
aismail1997 0:ec413378ee3a 217 Thread::wait(200);
aismail1997 0:ec413378ee3a 218 }
aismail1997 0:ec413378ee3a 219 }