Lab 4: Bluetooth Controlled Robot with IMU Position Readings
Project Description:
A two wheeled robot controlled by Bluetooth via a Adafruit LE UART Friend chip and a phone with the respective app. An onboard IMU takes gyroscope, accelerometer, and magnometer measurements and send them to the terminal through the mbed USB serial connection.
Controller
Imu
H-bridge
SD Card Reader
Bluetooth
Class D Audio Amp
Code for Bluetooth Controlled Robot with IMU Position Readings
/4180controllerdemo/main.cpp
#include "mbed.h" #include "Motor.h" #include "SDFileSystem.h" #include "wave_player.h" #include "SongPlayer.h" //#include "uLCD_4DGL.h" #include "LSM9DS1.h" #define PI 3.14159 // Earth's magnetic field varies by location. Add or subtract // a declination to get a more accurate heading. Calculate // your's here: // http://www.ngdc.noaa.gov/geomag-web/#declination #define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA. Serial pc(USBTX, USBRX); Motor a(p21, p20, p19); // pwm, fwd, rev Motor b(p22, p17, p16); // pwm, fwd, rev BusOut myled(LED1,LED2,LED3,LED4); Serial blue(p28,p27); AnalogOut speaker(p18); wave_player waver(&speaker); SDFileSystem sd(p5, p6, p7, p8, "sd"); int main() { LSM9DS1 IMU(p9, p10, 0xD6, 0x3C); //start IMU code IMU.begin(); if (!IMU.begin()) { pc.printf("Failed to communicate with LSM9DS1.\n"); } IMU.calibrate(1); IMU.calibrateMag(0); int ax=0, ay=0, hx=0, hy=0; float mx=0, my=0, heading=0; //end IMU code float sL = 0.0; float sR = 0.0; char bnum=0; char bhit=0; while(1) { 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? myled = bnum - '0'; //current button number will appear on LEDs switch (bnum) { case '1': //number button 1 if (bhit=='1') { //stop sL = 0.0; sR = 0.0; } else { //add release code here } break; case '2': //number button 1 if (bhit=='1') { //stop FILE *wave_file; wave_file=fopen("/sd/walker-grace-bitch.wav","r"); waver.play(wave_file); fclose(wave_file); } else { //add release code here } break; case '3': //number button 1 if (bhit=='1') { while(!IMU.tempAvailable()); //begin IMU read IMU.readTemp(); while(!IMU.magAvailable(X_AXIS)); IMU.readMag(); while(!IMU.accelAvailable()); IMU.readAccel(); while(!IMU.gyroAvailable()); IMU.readGyro(); //read X,Y +/-Gs and scale for #display pixels ax = (ax + IMU.calcAccel(IMU.ax) * 32.0)/2.0; ay = (ay -(IMU.calcAccel(IMU.ay) * 16.0))/2.0; mx = (float) IMU.mx; my = (float) IMU.my; if (my > 0) heading = 90 - (atan(mx / my) * (180 / 3.14)); //calculate heading else if (my < 0) heading = - (atan(mx / my) * (180 / 3.14)); else // hy = 0 { if (mx < 0) heading = 180; else heading = 0; } hx = (int)(17*mx); hy = (int)(17*my); pc.printf("\nIMU Temperature = %f C\n\r",25.0 + IMU.temperature/16.0); pc.printf(" X axis Y axis Z axis\n\r"); pc.printf("gyro: %9f %9f %9f in deg/s\n\r", IMU.calcGyro(IMU.gx), IMU.calcGyro(IMU.gy), IMU.calcGyro(IMU.gz)); pc.printf("accel: %9f %9f %9f in Gs\n\r", IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az)); pc.printf("mag: %9f %9f %9f in gauss\n\r", IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz)); } else { //add release code here } break; case '5': //button 5 up arrow if (bhit=='1') { //forwards sL = 0.75; sR = 0.75; } else { //add release code here } break; case '6': //button 6 down arrow if (bhit=='1') { //backwards sL = -0.75; sR = -0.75; } else { //add release code here } break; case '7': //button 7 left arrow if (bhit=='1') { //turn Left sL = -0.25; sR = 0.75; } else { //add release code here } break; case '8': //button 8 right arrow if (bhit=='1') { //turn right sL = 0.75; sR = -0.25; } else { //add release code here } break; default: break; } } } } a.speed(sL); b.speed(sR); wait(0.2); } //end IMU read and while loop }
Demo
Please log in to post comments.