main.cpp

00001 #include "mbed.h"
00002 #include "rtos.h"
00003 #include "LSM9DS1.h"
00004 #define DECLINATION -4.94
00005 #define PI 3.14159
00006 #include "Motor.h"
00007 
00008 
00009 // Timer example: https://developer.mbed.org/handbook/Timer
00010 AnalogIn infraredR(p20); //Right infrared distance sensor
00011 AnalogIn infraredL(p19); //Left infrared distance sensor
00012 AnalogIn infraredF(p17); //Front infrared distance sensor
00013 Serial pc(USBTX, USBRX);
00014 Timer t;
00015 char str[60];                           //buffer for xbee messages
00016 float maxspeed = 0.4;
00017 float minspeed = -0.4;
00018 float leftDist, rightDist, frontDist;   //Distances from robot to obstacles
00019 float xaccel, yaccel, zaccel;           //Acceleration in the x, y, and z directions
00020 float xmag, ymag, zmag, head;           //Magnetic readings
00021 int ldist, rdist, fdist;                
00022 Serial xbee(p28,p27);                   //xbee over serial
00023 Mutex stdio_mutex;
00024 float heading;
00025 //dual H bridge Polulu
00026 Motor MotorRi(p21, p23, p24);
00027 Motor MotorLe(p22, p26, p25);
00028 float ti = 0, timelast = 0, deltat;
00029 
00030 float imu[] = {0, 0, 0};
00031 float velX[] = {0, 0, 0};
00032 float velY[] = {0, 0, 0};
00033 float posX[] = {0, 0, 0};
00034 float posY[] = {0, 0, 0};
00035 float compass[] = {0, 0, 0};
00036 
00037 void printAttitude(float ax, float ay, float az, float mx, float my, float mz)
00038 {
00039     float roll = atan2(ay, az);
00040     float pitch = atan2(-ax, sqrt(ay * ay + az * az));
00041 // touchy trig stuff to use arctan to get compass heading (scale is 0..360)
00042     mx = -mx;
00043     if (my == 0.0)
00044         heading = (mx < 0.0) ? 180.0 : 0.0;
00045     else
00046         heading = atan2(mx, my)*360.0/(2.0*PI);
00047     //pc.printf("heading atan=%f \n\r",heading);
00048     heading -= DECLINATION; //correct for geo location
00049     if(heading>180.0) heading = heading - 360.0;
00050     else if(heading<-180.0) heading = 360.0 + heading;
00051     else if(heading<0.0) heading = 360.0  + heading;
00052 
00053 
00054     // Convert everything from radians to degrees:
00055     //heading *= 180.0 / PI;
00056     pitch *= 180.0 / PI;
00057     roll  *= 180.0 / PI;
00058 
00059     //pc.printf("Pitch: %f,    Roll: %f degress\n\r",pitch,roll);
00060     //pc.printf("Magnetic Heading: %f degress\n\r",heading);
00061     head = heading * PI / 180.0;
00062 }
00063 
00064 void shiftArrays() {
00065     int i;
00066     for (i = 2; i > 0; i--) {
00067         posX[i] = posX[i - 1];
00068         posY[i] = posY[i - 1];
00069         velX[i] = velX[i - 1];
00070         velY[i] = velY[i - 1];
00071         imu[i] = imu[i - 1];
00072         compass[i] = compass[i-1];
00073     }
00074 }
00075 
00076 void IMU_thread(void const *args) {
00077     LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);
00078     IMU.begin();
00079     if (!IMU.begin()) {
00080         pc.printf("Failed to communicate with LSM9DS1.\n");
00081     }
00082     IMU.calibrate(1);
00083     //pc.printf("calibrated accel, now do mag\r\n");
00084     IMU.calibrateMag(0);
00085     //pc.printf("calibrated mag\r\n");
00086     while(1) {
00087         //pc.printf("check accelAvailable\r\n");
00088         while(!IMU.accelAvailable());
00089         //pc.printf("got accel\r\n");
00090         IMU.readAccel();
00091         while(!IMU.magAvailable(X_AXIS));
00092         //pc.printf("got mag\r\n");
00093         IMU.readMag();
00094         xaccel = IMU.calcAccel(IMU.ax);
00095         if (abs(xaccel) < 0.13)
00096             xaccel = 0.0;
00097         yaccel = IMU.calcAccel(IMU.ay);
00098         if (abs(yaccel) < 0.13)
00099             yaccel = 0.0;
00100         zaccel = IMU.calcAccel(IMU.az);
00101         if (abs(zaccel) < 0.13)
00102             zaccel = 0.0;
00103         //pc.printf("%f\r\n",zaccel);
00104         xmag = IMU.calcMag(IMU.mx);
00105         ymag = IMU.calcMag(IMU.my);
00106         zmag = IMU.calcMag(IMU.mz);
00107         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));
00108         
00109        // Thread::wait(50); //Wait 
00110     }    
00111 }
00112 /*
00113 void pos_thread(void const *args) {
00114     while(1) {
00115         
00116         shiftArrays();
00117         imu[0] = xaccel;
00118 
00119         velX[0] += imu[0]*cos(compass[0])  * deltat;
00120         velY[0] += imu[0]*sin(compass[0])  * deltat;
00121         posX[0] += velX[0] * .1;
00122         posY[0] += velY[0] * .1;  
00123         wait(.1);  
00124     }    
00125 }    */
00126 void left_thread(void const *args) {
00127     while (1) {
00128         leftDist = infraredL * 80.0f;  
00129         leftDist = 80.0f - leftDist;
00130         ldist = (int)leftDist;
00131         Thread::wait(500);     //wait 1/2 second before updating
00132         }
00133 }
00134 
00135 void right_thread(void const *args) {
00136     while (1) {
00137         rightDist = infraredR * 80.0f;
00138         rightDist = 80.0f - rightDist;
00139         rdist = (int)rightDist;
00140         Thread::wait(500);     //wait 1/2 second before updating
00141         }
00142 }
00143 
00144 void front_thread(void const *args) {
00145     while (1) {
00146         frontDist = infraredF * 80.0f;
00147         frontDist = 80.0f - frontDist;
00148         fdist = (int)frontDist;
00149         Thread::wait(500);     //wait 1/2 second before updating
00150         }
00151 }    
00152 
00153 void send_thread(void const *args) {
00154     xbee.baud(9600);
00155     while (1) {
00156         ti = t.read();
00157         deltat = ti-timelast;
00158         stdio_mutex.lock();
00159         sprintf (str, "%0.6f,%0.6f,%0.6f,%d,%d,%d,%0.6f\n", posX[0], posY[0], heading, fdist, ldist, rdist, deltat);   //buffer data to be sent to computer
00160 //        pc.printf("%0.6f,%0.6f,%0.6f\r\n",xaccel,yaccel,deltat);
00161         pc.printf(str);
00162 //        pc.printf("%0.6f\r\n", zmag*9.8);
00163         xbee.printf(str);
00164         stdio_mutex.unlock();
00165         timelast = ti;
00166         Thread::wait(500);
00167     }
00168 }        
00169 
00170 void motor_thread(void const *args) {
00171     Thread::wait(5000);        //allow gyro and accel to calibrate on level surface
00172     MotorRi.speed(0.4);
00173     MotorLe.speed(-0.4);
00174     Thread::wait(3000);         //allow mag to calibrate by spinning
00175     MotorRi.speed(0.0);
00176     MotorLe.speed(-0.0);
00177     Thread::wait(10000);
00178     while(1) {
00179         if (fdist > 40) {                   //if there's room to go forward
00180             posX[0] += cos(head) * 0.0001;
00181             posY[0] += sin(head) * 0.0001;
00182             if (ldist < 20) {
00183                 MotorRi.speed(0.1);
00184                 MotorLe.speed(0.3);
00185                 Thread::wait(40);
00186             }
00187             else if (rdist < 20) {
00188                 MotorRi.speed(0.3);
00189                 MotorLe.speed(0.1);
00190                 Thread::wait(40);
00191             }
00192             else {
00193                 MotorRi.speed(0.3);
00194                 MotorLe.speed(0.3);
00195             }
00196         }    
00197         else if (rdist < ldist) {               //hard turn to the left
00198             MotorRi.speed(0.3);
00199             MotorLe.speed(-0.3); 
00200             Thread::wait(500);         
00201         }
00202         else if (ldist < rdist) {               //hard turn to the right
00203             MotorRi.speed(-0.4);
00204             MotorLe.speed(0.4);   
00205             Thread::wait(500);        
00206         }
00207     }
00208 }        
00209 
00210 int main() {
00211     //Test
00212     t.start();
00213     //Thread posi(pos_thread);
00214     Thread left(left_thread);
00215     Thread right(right_thread);
00216     Thread front(front_thread);
00217     Thread IMU(IMU_thread);
00218     Thread Mot(motor_thread);
00219     Thread send(send_thread);
00220     while(1){  
00221          //do stuff 
00222         }
00223 }