Gal Yaroslavsky / MicroMouse-v1

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?

UserRevisionLine numberNew 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 }