Robot using IMU and IR sensor data to drive autonomously and create a map
Dependencies: mbed mbed-rtos LSM9DS1_Library_cal Motor
Diff: main.cpp
- Revision:
- 6:a51f4ac6f42b
- Parent:
- 4:18e9f16cc53c
- Child:
- 7:cbccf5c5da6d
--- a/main.cpp Sun Apr 10 22:03:16 2016 +0000 +++ b/main.cpp Sun Apr 24 21:08:30 2016 +0000 @@ -4,15 +4,17 @@ #define DECLINATION -4.94 #define PI 3.14159 - +// Timer example: https://developer.mbed.org/handbook/Timer AnalogIn infraredR(p20); //Right infrared distance sensor AnalogIn infraredL(p19); //Left infrared distance sensor AnalogIn infraredF(p17); //Front infrared distance sensor Serial pc(USBTX, USBRX); - +Timer t; + float leftDist, rightDist, frontDist; //Distances from robot to obstacles float xaccel, yaccel, zaccel; //Acceleration in the x, y, and z directions float xmag, ymag, zmag, head; //Magnetic readings +int ldist, rdist, fdist; void printAttitude(float ax, float ay, float az, float mx, float my, float mz) { @@ -44,6 +46,10 @@ void IMU_thread(void const *args) { LSM9DS1 IMU(p28, p27, 0xD6, 0x3C); + IMU.begin(); + if (!IMU.begin()) { + pc.printf("Failed to communicate with LSM9DS1.\n"); + } IMU.calibrate(1); IMU.calibrateMag(0); while(1) { @@ -54,6 +60,7 @@ xaccel = IMU.calcAccel(IMU.ax); yaccel = IMU.calcAccel(IMU.ay); zaccel = IMU.calcAccel(IMU.az); + pc.printf("%f\r\n",zaccel); xmag = IMU.calcMag(IMU.mx); ymag = IMU.calcMag(IMU.my); zmag = IMU.calcMag(IMU.mz); @@ -64,31 +71,42 @@ void left_thread(void const *args) { while (1) { - leftDist = infraredL * 3.3f; + leftDist = infraredL * 80.0f; + leftDist = 80.0f - leftDist; + ldist = (int)leftDist; Thread::wait(500); //wait 1/2 second before updating + //pc.printf("Left distance %d\r\n", ldist); } } void right_thread(void const *args) { while (1) { - rightDist = infraredR * 3.3f; + rightDist = infraredR * 80.0f; + rightDist = 80.0f - rightDist; + rdist = (int)rightDist; + rdist = (int)rightDist; Thread::wait(500); //wait 1/2 second before updating + //pc.printf("Right distance %d\r\n", rdist); } } void front_thread(void const *args) { while (1) { - frontDist = infraredF * 3.3f; + frontDist = infraredF * 80.0f; + frontDist = 80.0f - frontDist; + fdist = (int)frontDist; Thread::wait(500); //wait 1/2 second before updating + pc.printf("Front distance %d\r\n", fdist); } } int main() { //Test + t.begin(); Thread left(left_thread); Thread right(right_thread); Thread front(front_thread); - + Thread IMU(IMU_thread); while(1) { }