David Spillman / Mbed 2 deprecated GPSNavigationNew

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   GPS2 L3GD20 LSM303DLHC2 PID mbed SDFileSystem

Fork of GPSNavigation by David Spillman

navigation.h@5:40ac894e0fa7, 2015-04-05 (annotated)

Committer:
Spilly
Date:
Sun Apr 05 01:45:29 2015 +0000
Revision:
5:40ac894e0fa7
Child:
8:c77ab7615b21
Updated commands

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Spilly 5:40ac894e0fa7 1 /**************************************************************************************************************************************************************
Spilly 5:40ac894e0fa7 2 // Created by: Ryan Spillman
Spilly 5:40ac894e0fa7 3 //
Spilly 5:40ac894e0fa7 4 // Last updated 4/4/2015
Spilly 5:40ac894e0fa7 5 //
Spilly 5:40ac894e0fa7 6 // Contains function that outputs difference in compass heading(magHead) and heading required(calcHead)
Spilly 5:40ac894e0fa7 7 // Also contains functions for controlling L298n H-bridge
Spilly 5:40ac894e0fa7 8 **************************************************************************************************************************************************************/
Spilly 5:40ac894e0fa7 9
Spilly 5:40ac894e0fa7 10 #define navigation_h
Spilly 5:40ac894e0fa7 11
Spilly 5:40ac894e0fa7 12 //Tolerance for heading actual and heading needed
Spilly 5:40ac894e0fa7 13 #define HEADDIFF 0.000000000000000000f
Spilly 5:40ac894e0fa7 14
Spilly 5:40ac894e0fa7 15 //L298n connections
Spilly 5:40ac894e0fa7 16 DigitalOut pinI1(D10);
Spilly 5:40ac894e0fa7 17 DigitalOut pinI2(D11);
Spilly 5:40ac894e0fa7 18 DigitalOut pinI3(D12);
Spilly 5:40ac894e0fa7 19 DigitalOut pinI4(D13);
Spilly 5:40ac894e0fa7 20 PwmOut ENA(D6); //Left
Spilly 5:40ac894e0fa7 21 PwmOut ENB(D7); //Right
Spilly 5:40ac894e0fa7 22
Spilly 5:40ac894e0fa7 23 //Outputs difference in compass heading(magHead) and heading required(calcHead)
Spilly 5:40ac894e0fa7 24 //negative is left and positive is right
Spilly 5:40ac894e0fa7 25 float whichWay(float magHead, float calcHead)
Spilly 5:40ac894e0fa7 26 {
Spilly 5:40ac894e0fa7 27 float magDiff;
Spilly 5:40ac894e0fa7 28
Spilly 5:40ac894e0fa7 29 float absOfDiff = sqrt((calcHead - magHead) * (calcHead - magHead));
Spilly 5:40ac894e0fa7 30
Spilly 5:40ac894e0fa7 31 //Is the heading off enough to care?
Spilly 5:40ac894e0fa7 32 if((absOfDiff >= HEADDIFF) && (absOfDiff <= (360 - HEADDIFF)))
Spilly 5:40ac894e0fa7 33 //if(1)
Spilly 5:40ac894e0fa7 34 {
Spilly 5:40ac894e0fa7 35 //quadrant I
Spilly 5:40ac894e0fa7 36 if(calcHead < 90)
Spilly 5:40ac894e0fa7 37 {
Spilly 5:40ac894e0fa7 38 if(calcHead < magHead)
Spilly 5:40ac894e0fa7 39 {
Spilly 5:40ac894e0fa7 40 if(magHead < (180 + calcHead))
Spilly 5:40ac894e0fa7 41 {
Spilly 5:40ac894e0fa7 42 //turn left need negative
Spilly 5:40ac894e0fa7 43 magDiff = calcHead - magHead;
Spilly 5:40ac894e0fa7 44 }
Spilly 5:40ac894e0fa7 45 else
Spilly 5:40ac894e0fa7 46 {
Spilly 5:40ac894e0fa7 47 //turn right need positive
Spilly 5:40ac894e0fa7 48 magDiff = calcHead - magHead + 360;
Spilly 5:40ac894e0fa7 49 }
Spilly 5:40ac894e0fa7 50 }
Spilly 5:40ac894e0fa7 51 else
Spilly 5:40ac894e0fa7 52 {
Spilly 5:40ac894e0fa7 53 if(magHead < (180 + calcHead))
Spilly 5:40ac894e0fa7 54 {
Spilly 5:40ac894e0fa7 55 //turn left need negative
Spilly 5:40ac894e0fa7 56 magDiff = calcHead - magHead;
Spilly 5:40ac894e0fa7 57 }
Spilly 5:40ac894e0fa7 58 else
Spilly 5:40ac894e0fa7 59 {
Spilly 5:40ac894e0fa7 60 //turn right need positive
Spilly 5:40ac894e0fa7 61 magDiff = calcHead - magHead + 360;
Spilly 5:40ac894e0fa7 62 }
Spilly 5:40ac894e0fa7 63 }
Spilly 5:40ac894e0fa7 64 }
Spilly 5:40ac894e0fa7 65 //quadrant II
Spilly 5:40ac894e0fa7 66 else if(calcHead < 180)
Spilly 5:40ac894e0fa7 67 {
Spilly 5:40ac894e0fa7 68 if(calcHead < magHead)
Spilly 5:40ac894e0fa7 69 {
Spilly 5:40ac894e0fa7 70 if(magHead < (180 + calcHead))
Spilly 5:40ac894e0fa7 71 {
Spilly 5:40ac894e0fa7 72 //turn left need negative
Spilly 5:40ac894e0fa7 73 magDiff = calcHead - magHead;
Spilly 5:40ac894e0fa7 74 }
Spilly 5:40ac894e0fa7 75 else
Spilly 5:40ac894e0fa7 76 {
Spilly 5:40ac894e0fa7 77 //turn right need positive
Spilly 5:40ac894e0fa7 78 magDiff = calcHead - magHead + 360;
Spilly 5:40ac894e0fa7 79 }
Spilly 5:40ac894e0fa7 80 }
Spilly 5:40ac894e0fa7 81 else
Spilly 5:40ac894e0fa7 82 {
Spilly 5:40ac894e0fa7 83 if(magHead < (180 + calcHead))
Spilly 5:40ac894e0fa7 84 {
Spilly 5:40ac894e0fa7 85 //turn left need negative
Spilly 5:40ac894e0fa7 86 magDiff = calcHead - magHead;
Spilly 5:40ac894e0fa7 87 }
Spilly 5:40ac894e0fa7 88 else
Spilly 5:40ac894e0fa7 89 {
Spilly 5:40ac894e0fa7 90 //turn right need positive
Spilly 5:40ac894e0fa7 91 magDiff = calcHead - magHead + 360;
Spilly 5:40ac894e0fa7 92 }
Spilly 5:40ac894e0fa7 93 }
Spilly 5:40ac894e0fa7 94 }
Spilly 5:40ac894e0fa7 95 //quadrant III
Spilly 5:40ac894e0fa7 96 else if(calcHead < 270)
Spilly 5:40ac894e0fa7 97 {
Spilly 5:40ac894e0fa7 98 if(calcHead < magHead)
Spilly 5:40ac894e0fa7 99 {
Spilly 5:40ac894e0fa7 100 if(magHead < (180 + calcHead))
Spilly 5:40ac894e0fa7 101 {
Spilly 5:40ac894e0fa7 102 //turn left need negative
Spilly 5:40ac894e0fa7 103 magDiff = calcHead - magHead;
Spilly 5:40ac894e0fa7 104 }
Spilly 5:40ac894e0fa7 105 else
Spilly 5:40ac894e0fa7 106 {
Spilly 5:40ac894e0fa7 107 //turn right need positive
Spilly 5:40ac894e0fa7 108 magDiff = calcHead - magHead + 360;
Spilly 5:40ac894e0fa7 109 }
Spilly 5:40ac894e0fa7 110 }
Spilly 5:40ac894e0fa7 111 else
Spilly 5:40ac894e0fa7 112 {
Spilly 5:40ac894e0fa7 113 /*
Spilly 5:40ac894e0fa7 114 if(magHead < (180 + calcHead))
Spilly 5:40ac894e0fa7 115 {
Spilly 5:40ac894e0fa7 116 //turn left need negative
Spilly 5:40ac894e0fa7 117 magDiff = calcHead - magHead;
Spilly 5:40ac894e0fa7 118 }
Spilly 5:40ac894e0fa7 119 else
Spilly 5:40ac894e0fa7 120 {
Spilly 5:40ac894e0fa7 121
Spilly 5:40ac894e0fa7 122 //turn right need positive
Spilly 5:40ac894e0fa7 123 magDiff = calcHead - magHead + 360;
Spilly 5:40ac894e0fa7 124 }
Spilly 5:40ac894e0fa7 125 */
Spilly 5:40ac894e0fa7 126 magDiff = calcHead - magHead - 90;
Spilly 5:40ac894e0fa7 127 }
Spilly 5:40ac894e0fa7 128 }
Spilly 5:40ac894e0fa7 129 //quadrant IV
Spilly 5:40ac894e0fa7 130 else
Spilly 5:40ac894e0fa7 131 {
Spilly 5:40ac894e0fa7 132
Spilly 5:40ac894e0fa7 133 if(calcHead < magHead)
Spilly 5:40ac894e0fa7 134 {
Spilly 5:40ac894e0fa7 135 magDiff = calcHead - magHead;
Spilly 5:40ac894e0fa7 136 }
Spilly 5:40ac894e0fa7 137 else
Spilly 5:40ac894e0fa7 138 {
Spilly 5:40ac894e0fa7 139 if(magHead < (calcHead - 180))
Spilly 5:40ac894e0fa7 140 {
Spilly 5:40ac894e0fa7 141
Spilly 5:40ac894e0fa7 142 magDiff = calcHead - 360 - magHead;
Spilly 5:40ac894e0fa7 143 }
Spilly 5:40ac894e0fa7 144 else
Spilly 5:40ac894e0fa7 145 {
Spilly 5:40ac894e0fa7 146 //turn right need positive
Spilly 5:40ac894e0fa7 147 magDiff = calcHead - magHead;
Spilly 5:40ac894e0fa7 148 }
Spilly 5:40ac894e0fa7 149 }
Spilly 5:40ac894e0fa7 150 }
Spilly 5:40ac894e0fa7 151 return magDiff;
Spilly 5:40ac894e0fa7 152 }
Spilly 5:40ac894e0fa7 153 return 0;
Spilly 5:40ac894e0fa7 154 }
Spilly 5:40ac894e0fa7 155
Spilly 5:40ac894e0fa7 156 void goStop(float valueOne, float valueTwo)
Spilly 5:40ac894e0fa7 157 {
Spilly 5:40ac894e0fa7 158 pinI1 = 0;
Spilly 5:40ac894e0fa7 159 pinI2 = 0;
Spilly 5:40ac894e0fa7 160 pinI3 = 0;
Spilly 5:40ac894e0fa7 161 pinI4 = 0;
Spilly 5:40ac894e0fa7 162 ENA = valueOne;
Spilly 5:40ac894e0fa7 163 ENB = valueTwo;
Spilly 5:40ac894e0fa7 164 }
Spilly 5:40ac894e0fa7 165
Spilly 5:40ac894e0fa7 166 void goForward(float valueOne, float valueTwo)
Spilly 5:40ac894e0fa7 167 {
Spilly 5:40ac894e0fa7 168 pinI1 = 1;
Spilly 5:40ac894e0fa7 169 pinI2 = 0;
Spilly 5:40ac894e0fa7 170 pinI3 = 0;
Spilly 5:40ac894e0fa7 171 pinI4 = 1;
Spilly 5:40ac894e0fa7 172 ENA = valueOne;
Spilly 5:40ac894e0fa7 173 ENB = valueTwo;
Spilly 5:40ac894e0fa7 174 }
Spilly 5:40ac894e0fa7 175
Spilly 5:40ac894e0fa7 176 void goBackward(float valueOne, float valueTwo)
Spilly 5:40ac894e0fa7 177 {
Spilly 5:40ac894e0fa7 178 pinI1 = 0;
Spilly 5:40ac894e0fa7 179 pinI2 = 1;
Spilly 5:40ac894e0fa7 180 pinI3 = 1;
Spilly 5:40ac894e0fa7 181 pinI4 = 0;
Spilly 5:40ac894e0fa7 182 ENA = valueOne;
Spilly 5:40ac894e0fa7 183 ENB = valueTwo;
Spilly 5:40ac894e0fa7 184 }
Spilly 5:40ac894e0fa7 185
Spilly 5:40ac894e0fa7 186 void goLeft(float valueOne, float valueTwo)
Spilly 5:40ac894e0fa7 187 {
Spilly 5:40ac894e0fa7 188 pinI1 = 0;
Spilly 5:40ac894e0fa7 189 pinI2 = 1;
Spilly 5:40ac894e0fa7 190 pinI3 = 0;
Spilly 5:40ac894e0fa7 191 pinI4 = 1;
Spilly 5:40ac894e0fa7 192 ENA = valueOne;
Spilly 5:40ac894e0fa7 193 ENB = valueTwo;
Spilly 5:40ac894e0fa7 194 }
Spilly 5:40ac894e0fa7 195
Spilly 5:40ac894e0fa7 196 void goRight(float valueOne, float valueTwo)
Spilly 5:40ac894e0fa7 197 {
Spilly 5:40ac894e0fa7 198 pinI1 = 1;
Spilly 5:40ac894e0fa7 199 pinI2 = 0;
Spilly 5:40ac894e0fa7 200 pinI3 = 1;
Spilly 5:40ac894e0fa7 201 pinI4 = 0;
Spilly 5:40ac894e0fa7 202 ENA = valueOne;
Spilly 5:40ac894e0fa7 203 ENB = valueTwo;
Spilly 5:40ac894e0fa7 204 }