Azra Ismail
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
Diff: main.cpp
- Revision:
- 0:ec413378ee3a
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed Nov 02 13:34:10 2016 +0000
@@ -0,0 +1,219 @@
+#include "mbed.h"
+#include "motordriver.h"
+#include "LSM9DS1.h"
+#include "uLCD_4DGL.h"
+#include "rtos.h"
+
+// LCD stuff
+uLCD_4DGL uLCD(p13,p14,p15); // serial tx, serial rx, reset pin;
+
+// motor stuff
+Motor left(p21, p26, p25, 1); // pwm, fwd, rev, has brake feature (pb,b2,b2)
+Motor right(p22, p24, p23, 1);
+
+// sonar stuff
+// sonar 1
+DigitalOut trigger(p6);
+DigitalIn echo(p7);
+int distance = 0;
+int correction = 0;
+Timer sonar;
+// sonar 2
+/*
+DigitalOut trigger2(p12); // fix this
+DigitalIn echo2(p11); // fix this
+int distance2 = 0;
+int correction2 = 0;
+Timer sonar2;
+*/
+// bluetooth stuff
+Serial pc(USBTX, USBRX); // and IMU
+Serial blue(p28,p27);
+
+// Mutex for LCD
+Mutex lcd_mutex;
+
+// IMU stuff
+LSM9DS1 imu(p9, p10, 0xD6, 0x3C);
+int old_y = 0;
+int old_x = SIZE_X/2;
+
+void sonar_thread(void const *args)
+{
+ // SETUP SONAR 1
+ sonar.reset();
+ sonar.start(); // start timer
+ // min software polling delay to read echo pin
+ while (echo==2) {};
+ sonar.stop(); // stop timer
+ correction = sonar.read_us();
+ /* // SETUP SONAR 2
+ sonar2.reset();
+ sonar2.start();
+ while (echo2==2) {};
+ sonar2.stop();
+ correction2 = sonar2.read_us();
+*/
+ while(true) { // thread loop
+ // SONAR CODE
+ trigger = 1; // trigger sonar to send a ping
+ sonar.reset();
+ wait_us(10.0);
+ trigger = 0;
+ while (echo==0) {}; //wait for echo high
+ sonar.start(); //echo high, so start timer
+ while (echo==1) {}; //wait for echo low
+ sonar.stop(); //stop timer and read value
+ //subtract software overhead timer delay and scale to cm
+ distance = (sonar.read_us()-correction)/58.0;
+
+ /* // SONAR 2 CODE
+ trigger2 = 1;
+ sonar2.reset();
+ wait_us(10.0);
+ trigger2 = 0;
+ while (echo2==0) {};
+ sonar2.start();
+ while (echo2==1) {};
+ sonar2.stop();
+ distance2 = (sonar2.read_us()-correction2)/58.0; */
+ //wait(0.2);
+ // if object detected using sonar 1 & 2 - draw a circle
+ if (distance <= 8) {
+ lcd_mutex.lock();
+ uLCD.filled_circle(old_x-2, old_y + 4, 2, RED);
+ lcd_mutex.unlock();
+ }
+ //pc.printf("1: %d cm \n\r",distance);
+ Thread::wait(1000); // wait till thread is done
+ }
+}
+
+void imu_thread(void const *args)
+{
+ // SETUP IMU
+ imu.begin();
+ //if (!imu.begin()) {
+ //pc.printf("Failed to communicate with LSM9DS1.\n");
+ //}
+ imu.calibrate();
+ int magnitude = 0;
+ //pc.baud(9600);
+
+ while(true) { // thread loop
+ // IMU CODE
+ imu.readMag();
+ // determine magnitude of the magnetic field
+ magnitude = (imu.mx*imu.mx + imu.my*imu.my + imu.mz*imu.mz);
+ //pc.printf("mag: %d %d %d %d\n\n\r", imu.mx, imu.my, imu.mz, magnitude);
+ if (magnitude > 30000000) {
+ lcd_mutex.lock();
+ uLCD.filled_circle(old_x+2, old_y + 4, 2, LGREY);
+ lcd_mutex.unlock();
+ }
+ Thread::wait(200); // wait till thread is done
+ }
+}
+
+int main()
+{
+ // pc print
+ //pc.baud(9600);
+
+ // lcd initialization
+ uLCD.cls();
+ int y = 0;
+ int x = SIZE_X/2;
+ float theta = 0;
+
+ // bluetooth stuff
+ char bnum=0;
+ char bhit=0;
+ float s = 0;
+
+ Thread t2(imu_thread); //start imu thread
+ Thread t1(sonar_thread); //start sonar thread
+
+ while(1) {
+ // BLUETOOTH CODE
+ if (blue.getc()=='!') {
+ if (blue.getc()=='B') { //button data packet
+ bnum = blue.getc(); //button number
+ bhit = blue.getc(); //1=hit, 0=release
+ if (blue.getc()==char(~('!' + 'B' + bnum + bhit))) { //checksum OK?
+ switch (bnum) {
+ case '1': //number button 1
+ if (bhit=='1') {
+ // MOTOR CODE
+ // move forward
+ left.speed(0.5);
+ right.speed(0.5);
+ wait(0.4);
+ //}
+ left.stop(1);
+ right.stop(1);
+ y = 6*cos(theta);
+ x = 6*sin(theta);
+ lcd_mutex.lock();
+ uLCD.line(old_x,old_y,old_x+x,old_y+y,GREEN);
+ lcd_mutex.unlock();
+ old_x += x;
+ old_y += y;
+ wait(0.25);
+ }
+ break;
+ case '2': //number button 2
+ if (bhit=='1') {
+ theta += -3.14/6;
+ // MOTOR CODE
+ // turn right
+ s=(-1)*(0.55);
+ left.speed(0.55);
+ right.speed(s);
+ wait(0.25);
+ left.stop(1);
+ right.stop(1);
+ wait(0.25);
+ }
+ break;
+ case '3': //number button 3
+ if (bhit=='1') {
+ theta += 3.14/6;
+ // MOTOR CODE
+ // turn left
+ s=(-1)*(0.55);
+ left.speed(s);
+ right.speed(0.55);
+ wait(0.25);
+ left.stop(1);
+ right.stop(1);
+ wait(0.25);
+ }
+ break;
+ case '4': //number button 4
+ if (bhit=='1') {
+ // MOTOR CODE
+ // move back
+ s=(-1)*(0.5);
+ left.speed(s);
+ right.speed(s);
+ wait(0.4);
+ left.stop(1);
+ right.stop(1);
+ y = 6*cos(theta);
+ x = 6*sin(theta);
+ lcd_mutex.lock();
+ uLCD.line(old_x,old_y,old_x-x,old_y-y,RED);
+ lcd_mutex.unlock();
+ old_x += -x;
+ old_y += -y;
+ wait(0.25);
+ }
+ break;
+ }
+ }
+ }
+ }
+ Thread::wait(200);
+ }
+}
\ No newline at end of file