asdf
Dependencies: L3GD20 LSM303DLHC mbed
Main.cpp@2:997f57aee3b7, 2014-04-03 (annotated)
- Committer:
- goy5022
- Date:
- Thu Apr 03 13:15:40 2014 +0000
- Revision:
- 2:997f57aee3b7
- Parent:
- 1:cfe6a6ad8dca
- Child:
- 3:1a8a7cc709cc
Gal last touched this inappropriately
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 | 1:cfe6a6ad8dca | 15 | ledF.period_us(26.2); //28.5 //27.9 |
goy5022 | 2:997f57aee3b7 | 16 | wait_us(600); |
goy5022 | 2:997f57aee3b7 | 17 | ledF.write(.6); |
goy5022 | 2:997f57aee3b7 | 18 | stop_mov(); |
goy5022 | 2:997f57aee3b7 | 19 | |
goy5022 | 2:997f57aee3b7 | 20 | |
goy5022 | 2:997f57aee3b7 | 21 | wait(5); |
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 | 2:997f57aee3b7 | 69 | |
goy5022 | 2:997f57aee3b7 | 70 | for(int i = 0; i < 100; i++) |
goy5022 | 2:997f57aee3b7 | 71 | { |
goy5022 | 2:997f57aee3b7 | 72 | float l = SenseL.read(); |
goy5022 | 2:997f57aee3b7 | 73 | float r = SenseR.read(); |
goy5022 | 2:997f57aee3b7 | 74 | float f = SenseF.read(); |
goy5022 | 1:cfe6a6ad8dca | 75 | |
goy5022 | 1:cfe6a6ad8dca | 76 | /* |
goy5022 | 2:997f57aee3b7 | 77 | leftWeightedAvg.add(l); |
goy5022 | 2:997f57aee3b7 | 78 | rightWeightedAvg.add(r); |
goy5022 | 2:997f57aee3b7 | 79 | frontWeightedAvg.add(f); |
goy5022 | 1:cfe6a6ad8dca | 80 | |
goy5022 | 1:cfe6a6ad8dca | 81 | |
goy5022 | 2:997f57aee3b7 | 82 | leftExpAvg.add(l); |
goy5022 | 2:997f57aee3b7 | 83 | frontExpAvg.add(f); |
goy5022 | 2:997f57aee3b7 | 84 | */ |
goy5022 | 2:997f57aee3b7 | 85 | rightExpAvg.add(r); |
goy5022 | 2:997f57aee3b7 | 86 | frontExpAvg.add(f); |
goy5022 | 2:997f57aee3b7 | 87 | //leftBufferAvg.add(l); |
goy5022 | 2:997f57aee3b7 | 88 | //rightBufferAvg.add(r); |
goy5022 | 2:997f57aee3b7 | 89 | //frontBufferAvg.add(f); |
goy5022 | 1:cfe6a6ad8dca | 90 | |
goy5022 | 2:997f57aee3b7 | 91 | WIRELESS.printf("%i || F: %f \n\r", i, frontExpAvg.average()); |
goy5022 | 2:997f57aee3b7 | 92 | } |
goy5022 | 2:997f57aee3b7 | 93 | |
goy5022 | 2:997f57aee3b7 | 94 | float avgRight = rightExpAvg.average(); |
goy5022 | 2:997f57aee3b7 | 95 | |
goy5022 | 2:997f57aee3b7 | 96 | |
goy5022 | 2:997f57aee3b7 | 97 | setRightSpeed(3); |
goy5022 | 2:997f57aee3b7 | 98 | setLeftSpeed(3); |
goy5022 | 2:997f57aee3b7 | 99 | |
goy5022 | 2:997f57aee3b7 | 100 | |
goy5022 | 2:997f57aee3b7 | 101 | while (true) |
goy5022 | 2:997f57aee3b7 | 102 | //while(SenseL.read() < 0.99) |
goy5022 | 2:997f57aee3b7 | 103 | { |
goy5022 | 2:997f57aee3b7 | 104 | // frontExpAvg.add(SenseF.read()); |
goy5022 | 2:997f57aee3b7 | 105 | |
goy5022 | 2:997f57aee3b7 | 106 | //WIRELESS.printf("R: %f L: %f F: %f\n\r", SenseR.read(), SenseL.read(), SenseF.read()); |
goy5022 | 0:c2ec30f28676 | 107 | |
goy5022 | 2:997f57aee3b7 | 108 | float PIDv = PID(SenseR.read(), SenseL.read()); |
goy5022 | 2:997f57aee3b7 | 109 | //WIRELESS.printf("ERR: %f Front: %f\n\r", PIDv, SenseF.read()); |
goy5022 | 0:c2ec30f28676 | 110 | |
goy5022 | 2:997f57aee3b7 | 111 | if(PIDv < -0.1) |
goy5022 | 2:997f57aee3b7 | 112 | { |
goy5022 | 2:997f57aee3b7 | 113 | setRightSpeed(3); |
goy5022 | 2:997f57aee3b7 | 114 | setLeftSpeed(4); |
goy5022 | 2:997f57aee3b7 | 115 | } |
goy5022 | 2:997f57aee3b7 | 116 | else if(PIDv > .1) |
goy5022 | 2:997f57aee3b7 | 117 | { |
goy5022 | 2:997f57aee3b7 | 118 | setRightSpeed(4); |
goy5022 | 2:997f57aee3b7 | 119 | setLeftSpeed(3); |
goy5022 | 2:997f57aee3b7 | 120 | } |
goy5022 | 2:997f57aee3b7 | 121 | else |
goy5022 | 2:997f57aee3b7 | 122 | { |
goy5022 | 2:997f57aee3b7 | 123 | setRightSpeed(3); |
goy5022 | 2:997f57aee3b7 | 124 | setLeftSpeed(3); |
goy5022 | 2:997f57aee3b7 | 125 | } |
goy5022 | 0:c2ec30f28676 | 126 | } |
goy5022 | 0:c2ec30f28676 | 127 | |
goy5022 | 2:997f57aee3b7 | 128 | //setRightSpeed(-2); |
goy5022 | 2:997f57aee3b7 | 129 | //setLeftSpeed(-2); |
goy5022 | 2:997f57aee3b7 | 130 | |
goy5022 | 2:997f57aee3b7 | 131 | //stop_mov(); |
goy5022 | 2:997f57aee3b7 | 132 | |
goy5022 | 2:997f57aee3b7 | 133 | stop_mov(); |
goy5022 | 0:c2ec30f28676 | 134 | return 0; |
goy5022 | 2:997f57aee3b7 | 135 | } |
goy5022 | 2:997f57aee3b7 | 136 | void moveNORTH() |
goy5022 | 2:997f57aee3b7 | 137 | { |
goy5022 | 2:997f57aee3b7 | 138 | switch(orientation) |
goy5022 | 2:997f57aee3b7 | 139 | { |
goy5022 | 2:997f57aee3b7 | 140 | case NORTH: |
goy5022 | 2:997f57aee3b7 | 141 | // go forward... |
goy5022 | 2:997f57aee3b7 | 142 | break; |
goy5022 | 2:997f57aee3b7 | 143 | case WEST: |
goy5022 | 2:997f57aee3b7 | 144 | // turn right then forward |
goy5022 | 2:997f57aee3b7 | 145 | break; |
goy5022 | 2:997f57aee3b7 | 146 | case SOUTH: |
goy5022 | 2:997f57aee3b7 | 147 | // turn around then forward |
goy5022 | 2:997f57aee3b7 | 148 | break; |
goy5022 | 2:997f57aee3b7 | 149 | case EAST: |
goy5022 | 2:997f57aee3b7 | 150 | // turn left then forward |
goy5022 | 2:997f57aee3b7 | 151 | break; |
goy5022 | 2:997f57aee3b7 | 152 | } |
goy5022 | 2:997f57aee3b7 | 153 | } |
goy5022 | 2:997f57aee3b7 | 154 | void moveSOUTH() |
goy5022 | 2:997f57aee3b7 | 155 | { |
goy5022 | 2:997f57aee3b7 | 156 | switch(orientation) |
goy5022 | 2:997f57aee3b7 | 157 | { |
goy5022 | 2:997f57aee3b7 | 158 | case NORTH: |
goy5022 | 2:997f57aee3b7 | 159 | // turn around then forward... |
goy5022 | 2:997f57aee3b7 | 160 | break; |
goy5022 | 2:997f57aee3b7 | 161 | case WEST: |
goy5022 | 2:997f57aee3b7 | 162 | // turn left then forward |
goy5022 | 2:997f57aee3b7 | 163 | break; |
goy5022 | 2:997f57aee3b7 | 164 | case SOUTH: |
goy5022 | 2:997f57aee3b7 | 165 | // go forward |
goy5022 | 2:997f57aee3b7 | 166 | break; |
goy5022 | 2:997f57aee3b7 | 167 | case EAST: |
goy5022 | 2:997f57aee3b7 | 168 | // turn right then forward |
goy5022 | 2:997f57aee3b7 | 169 | break; |
goy5022 | 2:997f57aee3b7 | 170 | } |
goy5022 | 2:997f57aee3b7 | 171 | } |
goy5022 | 2:997f57aee3b7 | 172 | void moveEAST() |
goy5022 | 2:997f57aee3b7 | 173 | { |
goy5022 | 2:997f57aee3b7 | 174 | switch(orientation) |
goy5022 | 2:997f57aee3b7 | 175 | { |
goy5022 | 2:997f57aee3b7 | 176 | case NORTH: |
goy5022 | 2:997f57aee3b7 | 177 | // turn right then forward... |
goy5022 | 2:997f57aee3b7 | 178 | break; |
goy5022 | 2:997f57aee3b7 | 179 | case WEST: |
goy5022 | 2:997f57aee3b7 | 180 | // turn around then foward |
goy5022 | 2:997f57aee3b7 | 181 | break; |
goy5022 | 2:997f57aee3b7 | 182 | case SOUTH: |
goy5022 | 2:997f57aee3b7 | 183 | // turn left then forward |
goy5022 | 2:997f57aee3b7 | 184 | break; |
goy5022 | 2:997f57aee3b7 | 185 | case EAST: |
goy5022 | 2:997f57aee3b7 | 186 | // go forward |
goy5022 | 2:997f57aee3b7 | 187 | break; |
goy5022 | 2:997f57aee3b7 | 188 | } |
goy5022 | 2:997f57aee3b7 | 189 | } |
goy5022 | 2:997f57aee3b7 | 190 | void moveWEST() |
goy5022 | 2:997f57aee3b7 | 191 | { |
goy5022 | 2:997f57aee3b7 | 192 | switch(orientation) |
goy5022 | 2:997f57aee3b7 | 193 | { |
goy5022 | 2:997f57aee3b7 | 194 | case NORTH: |
goy5022 | 2:997f57aee3b7 | 195 | // turn left then forward... |
goy5022 | 2:997f57aee3b7 | 196 | break; |
goy5022 | 2:997f57aee3b7 | 197 | case WEST: |
goy5022 | 2:997f57aee3b7 | 198 | // go forward |
goy5022 | 2:997f57aee3b7 | 199 | break; |
goy5022 | 2:997f57aee3b7 | 200 | case SOUTH: |
goy5022 | 2:997f57aee3b7 | 201 | // turn left then forward |
goy5022 | 2:997f57aee3b7 | 202 | break; |
goy5022 | 2:997f57aee3b7 | 203 | case EAST: |
goy5022 | 2:997f57aee3b7 | 204 | // turn around then forward |
goy5022 | 2:997f57aee3b7 | 205 | break; |
goy5022 | 2:997f57aee3b7 | 206 | } |
goy5022 | 0:c2ec30f28676 | 207 | } |