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:
- 11:805ef606a29d
- Parent:
- 9:36807f7e58fb
- Child:
- 12:dfa473dd9311
diff -r 72363392ecb5 -r 805ef606a29d main.cpp --- a/main.cpp Mon May 02 04:36:24 2016 +0000 +++ b/main.cpp Mon May 02 04:39:20 2016 +0000 @@ -105,17 +105,21 @@ ymag = IMU.calcMag(IMU.my); zmag = IMU.calcMag(IMU.mz); printAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz)); + + // Thread::wait(50); //Wait + } +} +void pos_thread(void const *args) { + while(1) { shiftArrays(); imu[0] = xaccel; compass[0] = head; velX[0] += imu[0]*cos(compass[0]) * deltat; velY[0] += imu[0]*sin(compass[0]) * deltat; posX[0] += velX[0] * deltat; - posY[0] += velY[0] * deltat; - // Thread::wait(50); //Wait + posY[0] += velY[0] * deltat; } -} - +} void left_thread(void const *args) { while (1) { leftDist = infraredL * 80.0f; @@ -197,6 +201,7 @@ int main() { //Test t.start(); + Thread posi(pos_thread); Thread left(left_thread); Thread right(right_thread); Thread front(front_thread);