asdf
Dependencies: L3GD20 LSM303DLHC mbed
Main.cpp@5:9e504a5a1f48, 2014-04-03 (annotated)
- Committer:
- goy5022
- Date:
- Thu Apr 03 18:47:30 2014 +0000
- Revision:
- 5:9e504a5a1f48
- Parent:
- 4:ecfe2115e9a4
- Child:
- 6:6e96e93689df
pid working, wall detection working, stopping not perfect;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
goy5022 | 2:997f57aee3b7 | 1 | //#include "Core.h" |
goy5022 | 0:c2ec30f28676 | 2 | |
goy5022 | 2:997f57aee3b7 | 3 | #include "Sensors.h" |
goy5022 | 2:997f57aee3b7 | 4 | #include "Motors.h" |
goy5022 | 2:997f57aee3b7 | 5 | #include "Communication.h" |
goy5022 | 2:997f57aee3b7 | 6 | #include "Flooding.h" |
goy5022 | 0:c2ec30f28676 | 7 | |
goy5022 | 2:997f57aee3b7 | 8 | #include "BufferAverage.h" |
goy5022 | 2:997f57aee3b7 | 9 | #include "ExponentialAverage.h" |
goy5022 | 2:997f57aee3b7 | 10 | //#include "WeightedAverage.h" |
goy5022 | 0:c2ec30f28676 | 11 | |
goy5022 | 0:c2ec30f28676 | 12 | int main() |
goy5022 | 0:c2ec30f28676 | 13 | { |
goy5022 | 0:c2ec30f28676 | 14 | Motor.baud(115200); |
goy5022 | 3:1a8a7cc709cc | 15 | ledF.period_us(26.3); //28.5 //27.9 |
goy5022 | 2:997f57aee3b7 | 16 | wait_us(600); |
goy5022 | 3:1a8a7cc709cc | 17 | ledF.write(.5); |
goy5022 | 2:997f57aee3b7 | 18 | stop_mov(); |
goy5022 | 2:997f57aee3b7 | 19 | |
goy5022 | 2:997f57aee3b7 | 20 | |
goy5022 | 3:1a8a7cc709cc | 21 | wait(3); |
goy5022 | 2:997f57aee3b7 | 22 | /* |
goy5022 | 2:997f57aee3b7 | 23 | |
goy5022 | 2:997f57aee3b7 | 24 | |
goy5022 | 2:997f57aee3b7 | 25 | STEVE!!!!!! |
goy5022 | 2:997f57aee3b7 | 26 | WHENEVER YOU ARE IN A CELL AND YOU WANT TO KNOW WHAT THE NEXT BEST MOVE IS |
goy5022 | 2:997f57aee3b7 | 27 | USE THE flood_findPath(x, y) where x and y are your current possition |
goy5022 | 2:997f57aee3b7 | 28 | |
goy5022 | 2:997f57aee3b7 | 29 | the function will return a MOVE type |
goy5022 | 2:997f57aee3b7 | 30 | and you can use something similar to this: |
goy5022 | 2:997f57aee3b7 | 31 | |
goy5022 | 2:997f57aee3b7 | 32 | Move k = flood_findPath(xPos, yPos); |
goy5022 | 2:997f57aee3b7 | 33 | switch(k) |
goy5022 | 0:c2ec30f28676 | 34 | { |
goy5022 | 2:997f57aee3b7 | 35 | case M_NORTH: |
goy5022 | 2:997f57aee3b7 | 36 | moveNORTH(); |
goy5022 | 2:997f57aee3b7 | 37 | break; |
goy5022 | 2:997f57aee3b7 | 38 | |
goy5022 | 2:997f57aee3b7 | 39 | case M_SOUTH: |
goy5022 | 2:997f57aee3b7 | 40 | moveSOUTH(); |
goy5022 | 2:997f57aee3b7 | 41 | break; |
goy5022 | 2:997f57aee3b7 | 42 | |
goy5022 | 2:997f57aee3b7 | 43 | case M_WEST: |
goy5022 | 2:997f57aee3b7 | 44 | moveWEST(); |
goy5022 | 2:997f57aee3b7 | 45 | break; |
goy5022 | 2:997f57aee3b7 | 46 | |
goy5022 | 2:997f57aee3b7 | 47 | case M_EAST: |
goy5022 | 2:997f57aee3b7 | 48 | moveEAST(); |
goy5022 | 2:997f57aee3b7 | 49 | break; |
goy5022 | 1:cfe6a6ad8dca | 50 | |
goy5022 | 2:997f57aee3b7 | 51 | } |
goy5022 | 2:997f57aee3b7 | 52 | |
goy5022 | 2:997f57aee3b7 | 53 | i've written the code for the following: |
goy5022 | 2:997f57aee3b7 | 54 | |
goy5022 | 2:997f57aee3b7 | 55 | |
goy5022 | 2:997f57aee3b7 | 56 | moveNORTH() |
goy5022 | 2:997f57aee3b7 | 57 | moveSOUTH() |
goy5022 | 2:997f57aee3b7 | 58 | moveEAST() |
goy5022 | 2:997f57aee3b7 | 59 | moveWEST() |
goy5022 | 2:997f57aee3b7 | 60 | |
goy5022 | 2:997f57aee3b7 | 61 | they are at the bottom of the page. take a look and read them. |
goy5022 | 2:997f57aee3b7 | 62 | |
goy5022 | 2:997f57aee3b7 | 63 | |
goy5022 | 2:997f57aee3b7 | 64 | REMEMBER TO MOVE THEM TO THE TOP OF THE PAGE IF YOU PLAN TO USE THEM IN THE MAIN FUNCTION, OR VICEVERSA MOVE THE MAIN FUNC. |
goy5022 | 2:997f57aee3b7 | 65 | TO THE BOTTOM |
goy5022 | 2:997f57aee3b7 | 66 | |
goy5022 | 2:997f57aee3b7 | 67 | */ |
goy5022 | 2:997f57aee3b7 | 68 | |
goy5022 | 5:9e504a5a1f48 | 69 | initMapping(); |
goy5022 | 2:997f57aee3b7 | 70 | |
goy5022 | 2:997f57aee3b7 | 71 | for(int i = 0; i < 100; i++) |
goy5022 | 2:997f57aee3b7 | 72 | { |
goy5022 | 2:997f57aee3b7 | 73 | float l = SenseL.read(); |
goy5022 | 2:997f57aee3b7 | 74 | float r = SenseR.read(); |
goy5022 | 2:997f57aee3b7 | 75 | float f = SenseF.read(); |
goy5022 | 1:cfe6a6ad8dca | 76 | |
goy5022 | 1:cfe6a6ad8dca | 77 | /* |
goy5022 | 2:997f57aee3b7 | 78 | leftWeightedAvg.add(l); |
goy5022 | 2:997f57aee3b7 | 79 | rightWeightedAvg.add(r); |
goy5022 | 2:997f57aee3b7 | 80 | frontWeightedAvg.add(f); |
goy5022 | 1:cfe6a6ad8dca | 81 | |
goy5022 | 1:cfe6a6ad8dca | 82 | |
goy5022 | 2:997f57aee3b7 | 83 | leftExpAvg.add(l); |
goy5022 | 2:997f57aee3b7 | 84 | frontExpAvg.add(f); |
goy5022 | 2:997f57aee3b7 | 85 | */ |
goy5022 | 4:ecfe2115e9a4 | 86 | leftExpAvg.add(l); |
goy5022 | 2:997f57aee3b7 | 87 | rightExpAvg.add(r); |
goy5022 | 2:997f57aee3b7 | 88 | frontExpAvg.add(f); |
goy5022 | 2:997f57aee3b7 | 89 | //leftBufferAvg.add(l); |
goy5022 | 2:997f57aee3b7 | 90 | //rightBufferAvg.add(r); |
goy5022 | 2:997f57aee3b7 | 91 | //frontBufferAvg.add(f); |
goy5022 | 1:cfe6a6ad8dca | 92 | |
goy5022 | 2:997f57aee3b7 | 93 | WIRELESS.printf("%i || F: %f \n\r", i, frontExpAvg.average()); |
goy5022 | 2:997f57aee3b7 | 94 | } |
goy5022 | 4:ecfe2115e9a4 | 95 | cal_R = abs(log(1-rightExpAvg.average())); |
goy5022 | 4:ecfe2115e9a4 | 96 | cal_L = abs(log(1-leftExpAvg.average())); |
goy5022 | 4:ecfe2115e9a4 | 97 | //cal_F = abs(log(1-frontExpAvg.average())); |
goy5022 | 4:ecfe2115e9a4 | 98 | //float avgRight = rightExpAvg.average(); |
goy5022 | 2:997f57aee3b7 | 99 | |
goy5022 | 2:997f57aee3b7 | 100 | |
goy5022 | 2:997f57aee3b7 | 101 | setRightSpeed(3); |
goy5022 | 2:997f57aee3b7 | 102 | setLeftSpeed(3); |
goy5022 | 2:997f57aee3b7 | 103 | |
goy5022 | 2:997f57aee3b7 | 104 | |
goy5022 | 3:1a8a7cc709cc | 105 | //while (true) |
goy5022 | 5:9e504a5a1f48 | 106 | //while(SenseF.read() < 0.99) |
goy5022 | 5:9e504a5a1f48 | 107 | while(frontExpAvg.average() < .8 && wallRight() && wallLeft()) // |
goy5022 | 2:997f57aee3b7 | 108 | { |
goy5022 | 4:ecfe2115e9a4 | 109 | frontExpAvg.add(SenseF.read()); |
goy5022 | 2:997f57aee3b7 | 110 | |
goy5022 | 4:ecfe2115e9a4 | 111 | //WIRELESS.printf("R: %f L: %f\n\r", cal_R, cal_L); |
goy5022 | 5:9e504a5a1f48 | 112 | float PIDv = PID(linearize(SenseR.read()), linearize(SenseL.read())); |
goy5022 | 2:997f57aee3b7 | 113 | //WIRELESS.printf("ERR: %f Front: %f\n\r", PIDv, SenseF.read()); |
goy5022 | 0:c2ec30f28676 | 114 | |
goy5022 | 2:997f57aee3b7 | 115 | if(PIDv < -0.1) |
goy5022 | 2:997f57aee3b7 | 116 | { |
goy5022 | 4:ecfe2115e9a4 | 117 | setRightSpeed(3); |
goy5022 | 4:ecfe2115e9a4 | 118 | setLeftSpeed(4); |
goy5022 | 3:1a8a7cc709cc | 119 | wait_ms(20); |
goy5022 | 3:1a8a7cc709cc | 120 | |
goy5022 | 2:997f57aee3b7 | 121 | } |
goy5022 | 2:997f57aee3b7 | 122 | else if(PIDv > .1) |
goy5022 | 2:997f57aee3b7 | 123 | { |
goy5022 | 4:ecfe2115e9a4 | 124 | setRightSpeed(4); //** Just flipped these values |
goy5022 | 4:ecfe2115e9a4 | 125 | setLeftSpeed(3); |
goy5022 | 3:1a8a7cc709cc | 126 | wait_ms(20); |
goy5022 | 2:997f57aee3b7 | 127 | } |
goy5022 | 2:997f57aee3b7 | 128 | else |
goy5022 | 2:997f57aee3b7 | 129 | { |
goy5022 | 2:997f57aee3b7 | 130 | setRightSpeed(3); |
goy5022 | 2:997f57aee3b7 | 131 | setLeftSpeed(3); |
goy5022 | 3:1a8a7cc709cc | 132 | } |
goy5022 | 3:1a8a7cc709cc | 133 | //check_walls(); |
goy5022 | 0:c2ec30f28676 | 134 | } |
goy5022 | 0:c2ec30f28676 | 135 | |
goy5022 | 2:997f57aee3b7 | 136 | //setRightSpeed(-2); |
goy5022 | 2:997f57aee3b7 | 137 | //setLeftSpeed(-2); |
goy5022 | 2:997f57aee3b7 | 138 | |
goy5022 | 2:997f57aee3b7 | 139 | //stop_mov(); |
goy5022 | 3:1a8a7cc709cc | 140 | wait_ms(20); |
goy5022 | 5:9e504a5a1f48 | 141 | handbrake(); |
goy5022 | 5:9e504a5a1f48 | 142 | |
goy5022 | 5:9e504a5a1f48 | 143 | printMap(WIRELESS); |
goy5022 | 0:c2ec30f28676 | 144 | return 0; |
goy5022 | 2:997f57aee3b7 | 145 | } |
goy5022 | 2:997f57aee3b7 | 146 | void moveNORTH() |
goy5022 | 2:997f57aee3b7 | 147 | { |
goy5022 | 2:997f57aee3b7 | 148 | switch(orientation) |
goy5022 | 2:997f57aee3b7 | 149 | { |
goy5022 | 2:997f57aee3b7 | 150 | case NORTH: |
goy5022 | 2:997f57aee3b7 | 151 | // go forward... |
goy5022 | 2:997f57aee3b7 | 152 | break; |
goy5022 | 2:997f57aee3b7 | 153 | case WEST: |
goy5022 | 2:997f57aee3b7 | 154 | // turn right then forward |
goy5022 | 2:997f57aee3b7 | 155 | break; |
goy5022 | 2:997f57aee3b7 | 156 | case SOUTH: |
goy5022 | 2:997f57aee3b7 | 157 | // turn around then forward |
goy5022 | 2:997f57aee3b7 | 158 | break; |
goy5022 | 2:997f57aee3b7 | 159 | case EAST: |
goy5022 | 2:997f57aee3b7 | 160 | // turn left then forward |
goy5022 | 2:997f57aee3b7 | 161 | break; |
goy5022 | 2:997f57aee3b7 | 162 | } |
goy5022 | 2:997f57aee3b7 | 163 | } |
goy5022 | 2:997f57aee3b7 | 164 | void moveSOUTH() |
goy5022 | 2:997f57aee3b7 | 165 | { |
goy5022 | 2:997f57aee3b7 | 166 | switch(orientation) |
goy5022 | 2:997f57aee3b7 | 167 | { |
goy5022 | 2:997f57aee3b7 | 168 | case NORTH: |
goy5022 | 2:997f57aee3b7 | 169 | // turn around then forward... |
goy5022 | 2:997f57aee3b7 | 170 | break; |
goy5022 | 2:997f57aee3b7 | 171 | case WEST: |
goy5022 | 2:997f57aee3b7 | 172 | // turn left then forward |
goy5022 | 2:997f57aee3b7 | 173 | break; |
goy5022 | 2:997f57aee3b7 | 174 | case SOUTH: |
goy5022 | 2:997f57aee3b7 | 175 | // go forward |
goy5022 | 2:997f57aee3b7 | 176 | break; |
goy5022 | 2:997f57aee3b7 | 177 | case EAST: |
goy5022 | 2:997f57aee3b7 | 178 | // turn right then forward |
goy5022 | 2:997f57aee3b7 | 179 | break; |
goy5022 | 2:997f57aee3b7 | 180 | } |
goy5022 | 2:997f57aee3b7 | 181 | } |
goy5022 | 2:997f57aee3b7 | 182 | void moveEAST() |
goy5022 | 2:997f57aee3b7 | 183 | { |
goy5022 | 2:997f57aee3b7 | 184 | switch(orientation) |
goy5022 | 2:997f57aee3b7 | 185 | { |
goy5022 | 2:997f57aee3b7 | 186 | case NORTH: |
goy5022 | 2:997f57aee3b7 | 187 | // turn right then forward... |
goy5022 | 2:997f57aee3b7 | 188 | break; |
goy5022 | 2:997f57aee3b7 | 189 | case WEST: |
goy5022 | 2:997f57aee3b7 | 190 | // turn around then foward |
goy5022 | 2:997f57aee3b7 | 191 | break; |
goy5022 | 2:997f57aee3b7 | 192 | case SOUTH: |
goy5022 | 2:997f57aee3b7 | 193 | // turn left then forward |
goy5022 | 2:997f57aee3b7 | 194 | break; |
goy5022 | 2:997f57aee3b7 | 195 | case EAST: |
goy5022 | 2:997f57aee3b7 | 196 | // go forward |
goy5022 | 2:997f57aee3b7 | 197 | break; |
goy5022 | 2:997f57aee3b7 | 198 | } |
goy5022 | 2:997f57aee3b7 | 199 | } |
goy5022 | 2:997f57aee3b7 | 200 | void moveWEST() |
goy5022 | 2:997f57aee3b7 | 201 | { |
goy5022 | 2:997f57aee3b7 | 202 | switch(orientation) |
goy5022 | 2:997f57aee3b7 | 203 | { |
goy5022 | 2:997f57aee3b7 | 204 | case NORTH: |
goy5022 | 2:997f57aee3b7 | 205 | // turn left then forward... |
goy5022 | 2:997f57aee3b7 | 206 | break; |
goy5022 | 2:997f57aee3b7 | 207 | case WEST: |
goy5022 | 2:997f57aee3b7 | 208 | // go forward |
goy5022 | 2:997f57aee3b7 | 209 | break; |
goy5022 | 2:997f57aee3b7 | 210 | case SOUTH: |
goy5022 | 2:997f57aee3b7 | 211 | // turn left then forward |
goy5022 | 2:997f57aee3b7 | 212 | break; |
goy5022 | 2:997f57aee3b7 | 213 | case EAST: |
goy5022 | 2:997f57aee3b7 | 214 | // turn around then forward |
goy5022 | 2:997f57aee3b7 | 215 | break; |
goy5022 | 2:997f57aee3b7 | 216 | } |
goy5022 | 0:c2ec30f28676 | 217 | } |