asdf
Dependencies: L3GD20 LSM303DLHC mbed
Main.cpp@6:6e96e93689df, 2014-04-03 (annotated)
- Committer:
- goy5022
- Date:
- Thu Apr 03 19:21:58 2014 +0000
- Revision:
- 6:6e96e93689df
- Parent:
- 5:9e504a5a1f48
- Child:
- 7:95ebadc83fc7
pid working single pid working communicaiton up starting timing going to clean up main;
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 | 6:6e96e93689df | 70 | initSensors(); |
goy5022 | 1:cfe6a6ad8dca | 71 | |
goy5022 | 2:997f57aee3b7 | 72 | |
goy5022 | 2:997f57aee3b7 | 73 | setRightSpeed(3); |
goy5022 | 2:997f57aee3b7 | 74 | setLeftSpeed(3); |
goy5022 | 6:6e96e93689df | 75 | t.start(); |
goy5022 | 2:997f57aee3b7 | 76 | |
goy5022 | 3:1a8a7cc709cc | 77 | //while (true) |
goy5022 | 5:9e504a5a1f48 | 78 | //while(SenseF.read() < 0.99) |
goy5022 | 6:6e96e93689df | 79 | while(frontExpAvg.average() < .8) // && wallRight() && wallLeft() |
goy5022 | 2:997f57aee3b7 | 80 | { |
goy5022 | 4:ecfe2115e9a4 | 81 | frontExpAvg.add(SenseF.read()); |
goy5022 | 2:997f57aee3b7 | 82 | |
goy5022 | 6:6e96e93689df | 83 | if ( wallRight() && wallLeft()) |
goy5022 | 6:6e96e93689df | 84 | PIDv = PID(linearize(SenseR.read()), linearize(SenseL.read())); |
goy5022 | 6:6e96e93689df | 85 | if ( wallRight() && !wallLeft()) |
goy5022 | 6:6e96e93689df | 86 | PIDv = PID(linearize(SenseR.read()),cal_R); |
goy5022 | 6:6e96e93689df | 87 | if ( !wallRight() && wallLeft()) |
goy5022 | 6:6e96e93689df | 88 | PIDv = PID(cal_L, linearize(SenseL.read())); |
goy5022 | 0:c2ec30f28676 | 89 | |
goy5022 | 2:997f57aee3b7 | 90 | if(PIDv < -0.1) |
goy5022 | 2:997f57aee3b7 | 91 | { |
goy5022 | 4:ecfe2115e9a4 | 92 | setRightSpeed(3); |
goy5022 | 4:ecfe2115e9a4 | 93 | setLeftSpeed(4); |
goy5022 | 3:1a8a7cc709cc | 94 | wait_ms(20); |
goy5022 | 2:997f57aee3b7 | 95 | } |
goy5022 | 2:997f57aee3b7 | 96 | else if(PIDv > .1) |
goy5022 | 2:997f57aee3b7 | 97 | { |
goy5022 | 4:ecfe2115e9a4 | 98 | setRightSpeed(4); //** Just flipped these values |
goy5022 | 4:ecfe2115e9a4 | 99 | setLeftSpeed(3); |
goy5022 | 3:1a8a7cc709cc | 100 | wait_ms(20); |
goy5022 | 2:997f57aee3b7 | 101 | } |
goy5022 | 2:997f57aee3b7 | 102 | else |
goy5022 | 2:997f57aee3b7 | 103 | { |
goy5022 | 2:997f57aee3b7 | 104 | setRightSpeed(3); |
goy5022 | 2:997f57aee3b7 | 105 | setLeftSpeed(3); |
goy5022 | 3:1a8a7cc709cc | 106 | } |
goy5022 | 6:6e96e93689df | 107 | //check_walls(); |
goy5022 | 6:6e96e93689df | 108 | //WIRELESS.printf("ERR: %f Front: %f\n\r", PIDv, SenseF.read()); |
goy5022 | 6:6e96e93689df | 109 | //WIRELESS.printf("R: %f L: %f\n\r", cal_R, cal_L); |
goy5022 | 0:c2ec30f28676 | 110 | } |
goy5022 | 0:c2ec30f28676 | 111 | |
goy5022 | 2:997f57aee3b7 | 112 | //setRightSpeed(-2); |
goy5022 | 2:997f57aee3b7 | 113 | //setLeftSpeed(-2); |
goy5022 | 2:997f57aee3b7 | 114 | |
goy5022 | 2:997f57aee3b7 | 115 | //stop_mov(); |
goy5022 | 3:1a8a7cc709cc | 116 | wait_ms(20); |
goy5022 | 6:6e96e93689df | 117 | WIRELESS.printf("Time: %f\n\r", t.read()); |
goy5022 | 6:6e96e93689df | 118 | handbrake(); |
goy5022 | 6:6e96e93689df | 119 | wait_ms(20); |
goy5022 | 5:9e504a5a1f48 | 120 | printMap(WIRELESS); |
goy5022 | 0:c2ec30f28676 | 121 | return 0; |
goy5022 | 2:997f57aee3b7 | 122 | } |
goy5022 | 2:997f57aee3b7 | 123 | void moveNORTH() |
goy5022 | 2:997f57aee3b7 | 124 | { |
goy5022 | 2:997f57aee3b7 | 125 | switch(orientation) |
goy5022 | 2:997f57aee3b7 | 126 | { |
goy5022 | 2:997f57aee3b7 | 127 | case NORTH: |
goy5022 | 2:997f57aee3b7 | 128 | // go forward... |
goy5022 | 2:997f57aee3b7 | 129 | break; |
goy5022 | 2:997f57aee3b7 | 130 | case WEST: |
goy5022 | 2:997f57aee3b7 | 131 | // turn right then forward |
goy5022 | 2:997f57aee3b7 | 132 | break; |
goy5022 | 2:997f57aee3b7 | 133 | case SOUTH: |
goy5022 | 2:997f57aee3b7 | 134 | // turn around then forward |
goy5022 | 2:997f57aee3b7 | 135 | break; |
goy5022 | 2:997f57aee3b7 | 136 | case EAST: |
goy5022 | 2:997f57aee3b7 | 137 | // turn left then forward |
goy5022 | 2:997f57aee3b7 | 138 | break; |
goy5022 | 2:997f57aee3b7 | 139 | } |
goy5022 | 2:997f57aee3b7 | 140 | } |
goy5022 | 2:997f57aee3b7 | 141 | void moveSOUTH() |
goy5022 | 2:997f57aee3b7 | 142 | { |
goy5022 | 2:997f57aee3b7 | 143 | switch(orientation) |
goy5022 | 2:997f57aee3b7 | 144 | { |
goy5022 | 2:997f57aee3b7 | 145 | case NORTH: |
goy5022 | 2:997f57aee3b7 | 146 | // turn around then forward... |
goy5022 | 2:997f57aee3b7 | 147 | break; |
goy5022 | 2:997f57aee3b7 | 148 | case WEST: |
goy5022 | 2:997f57aee3b7 | 149 | // turn left then forward |
goy5022 | 2:997f57aee3b7 | 150 | break; |
goy5022 | 2:997f57aee3b7 | 151 | case SOUTH: |
goy5022 | 2:997f57aee3b7 | 152 | // go forward |
goy5022 | 2:997f57aee3b7 | 153 | break; |
goy5022 | 2:997f57aee3b7 | 154 | case EAST: |
goy5022 | 2:997f57aee3b7 | 155 | // turn right then forward |
goy5022 | 2:997f57aee3b7 | 156 | break; |
goy5022 | 2:997f57aee3b7 | 157 | } |
goy5022 | 2:997f57aee3b7 | 158 | } |
goy5022 | 2:997f57aee3b7 | 159 | void moveEAST() |
goy5022 | 2:997f57aee3b7 | 160 | { |
goy5022 | 2:997f57aee3b7 | 161 | switch(orientation) |
goy5022 | 2:997f57aee3b7 | 162 | { |
goy5022 | 2:997f57aee3b7 | 163 | case NORTH: |
goy5022 | 2:997f57aee3b7 | 164 | // turn right then forward... |
goy5022 | 2:997f57aee3b7 | 165 | break; |
goy5022 | 2:997f57aee3b7 | 166 | case WEST: |
goy5022 | 2:997f57aee3b7 | 167 | // turn around then foward |
goy5022 | 2:997f57aee3b7 | 168 | break; |
goy5022 | 2:997f57aee3b7 | 169 | case SOUTH: |
goy5022 | 2:997f57aee3b7 | 170 | // turn left then forward |
goy5022 | 2:997f57aee3b7 | 171 | break; |
goy5022 | 2:997f57aee3b7 | 172 | case EAST: |
goy5022 | 2:997f57aee3b7 | 173 | // go forward |
goy5022 | 2:997f57aee3b7 | 174 | break; |
goy5022 | 2:997f57aee3b7 | 175 | } |
goy5022 | 2:997f57aee3b7 | 176 | } |
goy5022 | 2:997f57aee3b7 | 177 | void moveWEST() |
goy5022 | 2:997f57aee3b7 | 178 | { |
goy5022 | 2:997f57aee3b7 | 179 | switch(orientation) |
goy5022 | 2:997f57aee3b7 | 180 | { |
goy5022 | 2:997f57aee3b7 | 181 | case NORTH: |
goy5022 | 2:997f57aee3b7 | 182 | // turn left then forward... |
goy5022 | 2:997f57aee3b7 | 183 | break; |
goy5022 | 2:997f57aee3b7 | 184 | case WEST: |
goy5022 | 2:997f57aee3b7 | 185 | // go forward |
goy5022 | 2:997f57aee3b7 | 186 | break; |
goy5022 | 2:997f57aee3b7 | 187 | case SOUTH: |
goy5022 | 2:997f57aee3b7 | 188 | // turn left then forward |
goy5022 | 2:997f57aee3b7 | 189 | break; |
goy5022 | 2:997f57aee3b7 | 190 | case EAST: |
goy5022 | 2:997f57aee3b7 | 191 | // turn around then forward |
goy5022 | 2:997f57aee3b7 | 192 | break; |
goy5022 | 2:997f57aee3b7 | 193 | } |
goy5022 | 0:c2ec30f28676 | 194 | } |