Revision 0:e79311aae7ed, committed 2015-02-21

Comitter:

Spilly

Date:

Sat Feb 21 01:14:46 2015 +0000

Child:

1:7c2a82f12ae8

Commit message:

Crude GPS navigation. Determines whether to turn right, left, or go straight based on goal position, current position, and heading.

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/GPS.lib	Sat Feb 21 01:14:46 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/SamClarke/code/GPS/#94e2037a8bc8

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/L3GD20.lib	Sat Feb 21 01:14:46 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/bclaus/code/L3GD20/#62dfce144cf7

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LSM303DLHC.lib	Sat Feb 21 01:14:46 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/bclaus/code/LSM303DLHC/#612f7d5a822d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Feb 21 01:14:46 2015 +0000
@@ -0,0 +1,226 @@
+#include "mbed.h"
+#include "GPS.h"
+#include "modSensData.h"
+
+//Radius of the earth in meters
+#define EARTHRADIUS 6378100.0f
+//Tolerance for heading actual and heading needed
+#define HEADDIFF 5.0f
+//Period in seconds of the the main loop
+#define PERIOD 1
+
+//GPS
+GPS gps(D9, D7);
+
+//X-Bee
+Serial xBee(PTC15, PTC14);
+
+
+float startPos[2], curPos[2], goalPos[2] = {35.478593, -81.981878}, polarVector[2];
+
+void setGoalPos(float lat, float lon);
+void makeVector(void);
+char whichWay(float magHead, float calcHead);
+void testYaw(void);
+
+int main()
+{
+    xBee.baud(9600);
+    
+    //uncomment to check math for calculating needed heading
+    //testYaw();
+    
+    xBee.printf("\nI'm Alive...\n");
+    
+    //Setup the GPS
+    gps.Init();
+    
+    gps.parseData();
+    
+    //wait until we have a gps fix
+    while(gps.fixtype == 0)
+    {
+        xBee.printf("fix %d\n", gps.fixtype);
+        gps.parseData();
+        wait(.2);    
+    }
+    
+    //set starting position
+    curPos[0] = gps.latitude;
+    curPos[1] = gps.longitude;
+    
+    //Convert starting position and goal position to a vector
+    makeVector();
+    
+    //printf("lat %f\tlon %f\thead %f\talt %f\tspd %f\tfix %d\tsat %d\n", gps.latitude, gps.longitude, gps.heading, gps.altitude, gps.speed, gps.fixtype, gps.satellites);
+    //printf("magn %f\tangle %f\tlat %f\tlon %f\tgoalLat %f\tgoalLon %f\n", polarVector[0], polarVector[1], gps.latitude, gps.longitude, goalPos[0], goalPos[1]);
+    
+    while (1) 
+    {
+        // Process / check GPS data
+        gps.parseData();
+        //printf("lat %f\tlon %f\thead %f\talt %f\tspd %f\tfix %d\tsat %d\n", gps.latitude, gps.longitude, gps.heading, gps.altitude, gps.speed, gps.fixtype, gps.satellites);
+        
+        //update current position
+        curPos[0] = gps.latitude;
+        curPos[1] = gps.longitude;
+        
+        makeVector();
+        
+        //get data from IMU and do calculations to determine heading
+        updateAngles();
+        
+        //decide which way to go
+        char direction = whichWay(yaw, polarVector[1]);
+        xBee.printf("dist %f\tneed %f\tcurrent %f\tdir %c\tlat %f\tlon %f\tgoalLat %f\tgoalLon %f\n", polarVector[0], polarVector[1], yaw, direction, gps.latitude, gps.longitude, goalPos[0], goalPos[1]);
+        wait(PERIOD);
+    }
+}
+
+//create polar vector based on two sets of latitude and longitude
+void makeVector(void)
+{
+    float arcLength[2];
+    
+    //arc length = radius * angle
+    //Y
+    arcLength[1] = EARTHRADIUS * (goalPos[0] - curPos[0]);
+    //X
+    arcLength[0] = EARTHRADIUS * (curPos[1] - goalPos[1]);
+    
+    //arcLength[0] = EARTHRADIUS * (curPos[1] - goalPos[1]);
+    //arcLength[1] = EARTHRADIUS * (curPos[0] - goalPos[0]);
+    //calculate magnitude of vector
+    polarVector[0] = sqrt((arcLength[0] * arcLength[0]) + (arcLength[1] * arcLength[1]));
+    
+    //Use arcTan(-x/y) b/c we want our heading to be in respect to North (North = 0 degrees, East = 90 deg, etc.)
+    polarVector[1] = (RADTODEGREE * (atan2(-arcLength[0], arcLength[1])));
+    
+    //make negative angles positive
+    if(polarVector[1] < 0) polarVector[1] = polarVector[1] + 360;
+}
+
+//Decide which direction to turn based on current compass heading and heading required
+char whichWay(float magHead, float calcHead)
+{
+    
+    float absOfDiff = sqrt((calcHead - magHead) * (calcHead - magHead));
+    
+    //Is the heading off enough to care?
+    if((absOfDiff >= HEADDIFF) && (absOfDiff <= (360.0f - HEADDIFF)))
+    {
+        if(calcHead > magHead)
+        {
+            if((calcHead - magHead) < 180)
+            {
+                return 'R';
+            }
+            return 'L';
+        }
+        else if((magHead - calcHead) < 180)
+        {
+            return 'L';
+        }
+        return 'R';
+    }
+    return 'G';
+}
+
+//Test for verifying heading
+void testYaw(void)
+{
+    //set test position to indicate a necessary heading of:
+    curPos[0] = 34.833716f;
+    curPos[1] = -82.404188f;
+    
+    //due north
+    goalPos[0] = 35.833716f;
+    goalPos[1] = -82.404188f;
+    
+    //Convert starting position and goal position to a vector
+    makeVector();
+    
+    xBee.printf("dist %fmeters\tangle %fdegrees due north\n", polarVector[0], polarVector[1]);
+    
+    //due south
+    goalPos[0] = 33.833716f;
+    goalPos[1] = -82.404188f;
+    
+    
+    //Convert starting position and goal position to a vector
+    makeVector();
+    
+    xBee.printf("dist %fmeters\tangle %fdegrees due south\n", polarVector[0], polarVector[1]);
+    
+    //due east
+    goalPos[0] = 34.833716f;
+    goalPos[1] = -81.404188f;
+    
+    //Convert starting position and goal position to a vector
+    makeVector();
+    
+    xBee.printf("dist %fmeters\tangle %fdegrees due east\n", polarVector[0], polarVector[1]);
+    
+    
+    //due west
+    goalPos[0] = 34.833716f;
+    goalPos[1] = -83.404188f;
+    
+    //Convert starting position and goal position to a vector
+    makeVector();
+    
+    xBee.printf("dist %fmeters\tangle %fdegrees due west\n", polarVector[0], polarVector[1]);
+    
+    //north east
+    goalPos[0] = 35.833716f;
+    goalPos[1] = -81.404188f;
+    
+    //Convert starting position and goal position to a vector
+    makeVector();
+    
+    xBee.printf("dist %fmeters\tangle %fdegrees north east\n", polarVector[0], polarVector[1]);
+    
+    
+    //north west
+    goalPos[0] = 35.833716f;
+    goalPos[1] = -83.404188f;
+    
+    
+    //Convert starting position and goal position to a vector
+    makeVector();
+    
+    xBee.printf("dist %fmeters\tangle %fdegrees north west\n", polarVector[0], polarVector[1]);
+    
+    //south east
+    goalPos[0] = 33.833716f;
+    goalPos[1] = -81.404188f;
+    
+    //Convert starting position and goal position to a vector
+    makeVector();
+    
+    xBee.printf("dist %fmeters\tangle %fdegrees south east\n", polarVector[0], polarVector[1]);
+    
+    
+    //south west
+    goalPos[0] = 33.833716f;
+    goalPos[1] = -83.404188f;
+    
+    
+    //Convert starting position and goal position to a vector
+    makeVector();
+    
+    xBee.printf("dist %fmeters\tangle %fdegrees south west\n", polarVector[0], polarVector[1]);
+    
+    while(1);
+    
+    /*
+    //below is useful when checking compass heading
+    while(1)
+    {
+        updateAngles();
+        char direction = whichWay(yaw, polarVector[1]);
+        //printf("accX %f\taccY %f\taccZ %f\tmagX %f\tmagY %f\tmagZ %f\troll %f\tpitch %f\tyaw %f\tturn %c\n", accel[0], accel[1], accel[2], magnetom[0], magnetom[1], magnetom[2], roll, pitch, yaw, direction);
+        wait(1);
+    }
+    */
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Sat Feb 21 01:14:46 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/9ad691361fac
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/modSensData.h	Sat Feb 21 01:14:46 2015 +0000
@@ -0,0 +1,187 @@
+#include "mbed.h"
+#include "math.h"
+#include "L3GD20.h"
+#include "LSM303DLHC.h"
+
+L3GD20 gyro(PTE25, PTE24);
+LSM303DLHC compass(PTE25, PTE24);
+
+#define XBIAS 0
+#define YBIAS 0
+#define ZBIAS 0
+
+#define M_PI            3.1415926535897932384626433832795f
+#define TWO_PI          6.283185307179586476925286766559f
+
+#define RADTODEGREE     57.295779513082320876798154814105f
+
+#define GRAVITY       1.0f
+
+Timer t1;
+
+float vC[3], cC[3], magnetom[3], inertialAccel[3];
+
+float aP[3], vP[3], cP[3], deltaT, timeStamp, degGyro[3], angPos[3], prevAngPos[3];
+
+double roll = 0, pitch = 0, yaw = 0;
+
+float magCheck;
+
+float accel[3], bias[3], magnDiff;
+
+int printStamp = 0, stopStamp = 0, dangerAhead = 0, howLong = 0, count[3], pos[5][3]= { {0,0,0}, {0,0,0}, {0,0,0}, {0,0,0}, {0,0,0} };
+
+int flag = 0, signBit = 0;
+
+/********************************************************************************************************************************/
+//                                          Get Accelerometer and Magnetometer Data
+/********************************************************************************************************************************/
+
+void readIMU()
+{
+    compass.read(&accel[0], &accel[1], &accel[2], &magnetom[0], &magnetom[1], &magnetom[2]);
+    gyro.read(&degGyro[0], &degGyro[1], &degGyro[2]);
+}
+
+/********************************************************************************************************************************/
+//                                                      Compass Heading
+/********************************************************************************************************************************/
+
+void Compass_Heading()
+{
+  float mag_x;
+  float mag_y;
+  float cos_roll;
+  float sin_roll;
+  float cos_pitch;
+  float sin_pitch;
+  
+  //saves a few processor cycles by calculating and storing values
+  cos_roll = cos(roll);
+  sin_roll = sin(roll);
+  cos_pitch = cos(pitch);
+  sin_pitch = sin(pitch);
+  
+  // Tilt compensated magnetic field X
+  mag_x = magnetom[0] * cos_pitch + magnetom[1] * sin_roll * sin_pitch + magnetom[2] * cos_roll * sin_pitch;
+  // Tilt compensated magnetic field Y
+  mag_y = magnetom[1] * cos_roll - magnetom[2] * sin_roll;
+  // Magnetic Heading
+  yaw = atan2(-mag_x, mag_y);
+  
+  //make negative angles positive
+  if(yaw < 0) yaw = yaw + TWO_PI;
+  
+  yaw = yaw * RADTODEGREE;
+}
+
+/********************************************************************************************************************************/
+//                                                      getAttitude
+/********************************************************************************************************************************/
+
+void getAttitude()
+{
+  float temp1[3];
+  float temp2[3];
+  float xAxis[3] = {1.0f, 0.0f, 0.0f};
+  
+  // GET PITCH
+  // Using y-z-plane-component/x-component of gravity vector    
+  pitch = -atan2(accel[0], sqrt((accel[1] * accel[1]) + (accel[2] * accel[2])));
+  
+  //printf("P = %f", pitch);
+  
+  // GET ROLL
+  // Compensate pitch of gravity vector 
+  temp1[0] = (accel[1] * xAxis[2]) - (accel[2] * xAxis[1]);
+  temp1[1] = (accel[2] * xAxis[0]) - (accel[0] * xAxis[2]);
+  temp1[2] = (accel[0] * xAxis[1]) - (accel[1] * xAxis[0]);
+  
+  temp2[0] = (xAxis[1] * temp1[2]) - (xAxis[2] * temp1[1]);
+  temp2[1] = (xAxis[2] * temp1[0]) - (xAxis[0] * temp1[2]);
+  temp2[2] = (xAxis[0] * temp1[1]) - (xAxis[1] * temp1[0]);
+  
+  // Since we compensated for pitch, x-z-plane-component equals z-component:
+  roll = atan2(temp2[1], temp2[2]);
+}
+
+
+/********************************************************************************************************************************/
+//                                                      compensateAcc
+/********************************************************************************************************************************/
+/*
+void compensateAcc() 
+{
+    // Compensate accelerometer error
+    accel[0] = (accel[0] - ACCEL_X_OFFSET) * ACCEL_X_SCALE;
+    accel[1] = (accel[1] - ACCEL_Y_OFFSET) * ACCEL_Y_SCALE;
+    accel[2] = (accel[2] - ACCEL_Z_OFFSET) * ACCEL_Z_SCALE;
+}
+*/
+
+/********************************************************************************************************************************/
+//                                                      rotateBTI
+/********************************************************************************************************************************/
+
+//rotation from the body frame to the inertial frame
+void rotateBTI()
+{
+  float cos_roll, sin_roll, cos_pitch, sin_pitch, cos_yaw, sin_yaw;
+  
+  float gravity[3] = {0, 0, -GRAVITY};
+  
+  float rIB[3][3];  
+  
+  //saves a few processor cycles by calculating and storing values
+  
+  //Serial.print("Roll");
+  cos_roll = cos(roll);
+  //Serial.print(cos_roll, 4);
+  //Serial.print("    ");
+  sin_roll = sin(roll);
+  //Serial.print(sin_roll, 4);
+  //Serial.print("    ");
+  
+  //Serial.print("Pitch");
+  cos_pitch = cos(pitch);
+  //Serial.print(cos_pitch, 4);
+  //Serial.print("    ");
+  sin_pitch = sin(pitch);
+  //Serial.print(sin_pitch, 4);
+  //Serial.print("    ");
+  
+  //Serial.print("YAW");
+  cos_yaw = cos(yaw);
+  //Serial.print(cos_yaw, 4);
+  //Serial.print("    ");
+  sin_yaw = sin(yaw);
+  //Serial.print(sin_yaw, 4);
+  //Serial.print("    ");
+  
+  rIB[0][0] = cos_yaw * cos_pitch;
+  rIB[0][1] = (cos_yaw * sin_roll * sin_pitch) - (cos_roll * sin_yaw);
+  rIB[0][2] = (sin_roll * sin_yaw) + (cos_roll * cos_yaw * sin_pitch);
+  rIB[1][0] = cos_pitch * sin_yaw;
+  rIB[1][1] = (cos_roll * cos_yaw) + (sin_roll * sin_yaw * sin_pitch);
+  rIB[1][2] = (cos_roll * sin_yaw * sin_pitch) - (cos_yaw * sin_roll);
+  rIB[2][0] = -1 * sin_pitch;
+  rIB[2][1] = cos_pitch * sin_roll;
+  rIB[2][2] = cos_roll * cos_pitch;
+  
+  for(int i = 0;  i < 3; i++)
+  {
+    aP[i] = inertialAccel[i];
+    inertialAccel[i] = rIB[i][0] * accel[0] + rIB[i][1] * accel[1] + rIB[i][2] * accel[2];
+    inertialAccel[i] = inertialAccel[i] + gravity[i];
+  }
+}
+
+
+
+void updateAngles()
+{
+    readIMU();
+    getAttitude();
+    Compass_Heading();    
+    //rotateBTI();
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/move.h	Sat Feb 21 01:14:46 2015 +0000
@@ -0,0 +1,57 @@
+//L298n connections
+DigitalOut pinI1(D5);
+DigitalOut pinI2(D6);
+DigitalOut pinI3(D10);
+DigitalOut pinI4(D11);
+PwmOut ENA(D12);    //Left
+PwmOut ENB(D13);    //Right
+
+void goStop(float valueOne, float valueTwo)
+{
+    pinI1 = 0;
+    pinI2 = 0;
+    pinI3 = 0;
+    pinI4 = 0;
+    ENA =  valueOne;
+    ENB = valueTwo;
+}
+
+void goForward(float valueOne, float valueTwo)
+{
+    pinI1 = 1;
+    pinI2 = 0;
+    pinI3 = 0;
+    pinI4 = 1;
+    ENA =  valueOne;
+    ENB = valueTwo;
+}
+
+void goBackward(float valueOne, float valueTwo)
+{
+    pinI1 = 0;
+    pinI2 = 1;
+    pinI3 = 1;
+    pinI4 = 0;
+    ENA =  valueOne;
+    ENB = valueTwo;
+}
+
+void goLeft(float valueOne, float valueTwo)
+{
+    pinI1 = 0;
+    pinI2 = 1;
+    pinI3 = 0;
+    pinI4 = 1;
+    ENA =  valueOne;
+    ENB = valueTwo;
+}
+
+void goRight(float valueOne, float valueTwo)
+{
+    pinI1 = 1;
+    pinI2 = 0;
+    pinI3 = 1;
+    pinI4 = 0;
+    ENA =  valueOne;
+    ENB = valueTwo;
+}