Gal Yaroslavsky / MicroMouse-v1

asdf

Dependencies:   L3GD20 LSM303DLHC mbed

Main.cpp@4:ecfe2115e9a4, 2014-04-03 (annotated)

Committer:
goy5022
Date:
Thu Apr 03 17:34:00 2014 +0000
Revision:
4:ecfe2115e9a4
Parent:
3:1a8a7cc709cc
Child:
5:9e504a5a1f48
pid works great and it's not messed

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 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 4:ecfe2115e9a4 85 leftExpAvg.add(l);
goy5022 2:997f57aee3b7 86 rightExpAvg.add(r);
goy5022 2:997f57aee3b7 87 frontExpAvg.add(f);
goy5022 2:997f57aee3b7 88 //leftBufferAvg.add(l);
goy5022 2:997f57aee3b7 89 //rightBufferAvg.add(r);
goy5022 2:997f57aee3b7 90 //frontBufferAvg.add(f);
goy5022 1:cfe6a6ad8dca 91
goy5022 2:997f57aee3b7 92 WIRELESS.printf("%i || F: %f \n\r", i, frontExpAvg.average());
goy5022 2:997f57aee3b7 93 }
goy5022 4:ecfe2115e9a4 94 cal_R = abs(log(1-rightExpAvg.average()));
goy5022 4:ecfe2115e9a4 95 cal_L = abs(log(1-leftExpAvg.average()));
goy5022 4:ecfe2115e9a4 96 //cal_F = abs(log(1-frontExpAvg.average()));
goy5022 4:ecfe2115e9a4 97 //float avgRight = rightExpAvg.average();
goy5022 2:997f57aee3b7 98
goy5022 2:997f57aee3b7 99
goy5022 2:997f57aee3b7 100 setRightSpeed(3);
goy5022 2:997f57aee3b7 101 setLeftSpeed(3);
goy5022 2:997f57aee3b7 102
goy5022 2:997f57aee3b7 103
goy5022 3:1a8a7cc709cc 104 //while (true)
goy5022 3:1a8a7cc709cc 105 while(SenseF.read() < 0.99)
goy5022 4:ecfe2115e9a4 106 //while(frontExpAvg.average() < .90 ) //&& wallRight() && wallLeft()
goy5022 2:997f57aee3b7 107 {
goy5022 4:ecfe2115e9a4 108 frontExpAvg.add(SenseF.read());
goy5022 2:997f57aee3b7 109
goy5022 4:ecfe2115e9a4 110 //WIRELESS.printf("R: %f L: %f\n\r", cal_R, cal_L);
goy5022 4:ecfe2115e9a4 111 float PIDv = PID(abs(log(1-SenseR.read())), abs(log(1-SenseL.read())));
goy5022 2:997f57aee3b7 112 //WIRELESS.printf("ERR: %f Front: %f\n\r", PIDv, SenseF.read());
goy5022 0:c2ec30f28676 113
goy5022 2:997f57aee3b7 114 if(PIDv < -0.1)
goy5022 2:997f57aee3b7 115 {
goy5022 4:ecfe2115e9a4 116 setRightSpeed(3);
goy5022 4:ecfe2115e9a4 117 setLeftSpeed(4);
goy5022 3:1a8a7cc709cc 118 wait_ms(20);
goy5022 3:1a8a7cc709cc 119
goy5022 2:997f57aee3b7 120 }
goy5022 2:997f57aee3b7 121 else if(PIDv > .1)
goy5022 2:997f57aee3b7 122 {
goy5022 4:ecfe2115e9a4 123 setRightSpeed(4); //** Just flipped these values
goy5022 4:ecfe2115e9a4 124 setLeftSpeed(3);
goy5022 3:1a8a7cc709cc 125 wait_ms(20);
goy5022 2:997f57aee3b7 126 }
goy5022 2:997f57aee3b7 127 else
goy5022 2:997f57aee3b7 128 {
goy5022 2:997f57aee3b7 129 setRightSpeed(3);
goy5022 2:997f57aee3b7 130 setLeftSpeed(3);
goy5022 3:1a8a7cc709cc 131 }
goy5022 3:1a8a7cc709cc 132 //check_walls();
goy5022 0:c2ec30f28676 133 }
goy5022 0:c2ec30f28676 134
goy5022 2:997f57aee3b7 135 //setRightSpeed(-2);
goy5022 2:997f57aee3b7 136 //setLeftSpeed(-2);
goy5022 2:997f57aee3b7 137
goy5022 2:997f57aee3b7 138 //stop_mov();
goy5022 3:1a8a7cc709cc 139 wait_ms(20);
goy5022 3:1a8a7cc709cc 140 handbrake();
goy5022 0:c2ec30f28676 141 return 0;
goy5022 2:997f57aee3b7 142 }
goy5022 2:997f57aee3b7 143 void moveNORTH()
goy5022 2:997f57aee3b7 144 {
goy5022 2:997f57aee3b7 145 switch(orientation)
goy5022 2:997f57aee3b7 146 {
goy5022 2:997f57aee3b7 147 case NORTH:
goy5022 2:997f57aee3b7 148 // go forward...
goy5022 2:997f57aee3b7 149 break;
goy5022 2:997f57aee3b7 150 case WEST:
goy5022 2:997f57aee3b7 151 // turn right then forward
goy5022 2:997f57aee3b7 152 break;
goy5022 2:997f57aee3b7 153 case SOUTH:
goy5022 2:997f57aee3b7 154 // turn around then forward
goy5022 2:997f57aee3b7 155 break;
goy5022 2:997f57aee3b7 156 case EAST:
goy5022 2:997f57aee3b7 157 // turn left then forward
goy5022 2:997f57aee3b7 158 break;
goy5022 2:997f57aee3b7 159 }
goy5022 2:997f57aee3b7 160 }
goy5022 2:997f57aee3b7 161 void moveSOUTH()
goy5022 2:997f57aee3b7 162 {
goy5022 2:997f57aee3b7 163 switch(orientation)
goy5022 2:997f57aee3b7 164 {
goy5022 2:997f57aee3b7 165 case NORTH:
goy5022 2:997f57aee3b7 166 // turn around then forward...
goy5022 2:997f57aee3b7 167 break;
goy5022 2:997f57aee3b7 168 case WEST:
goy5022 2:997f57aee3b7 169 // turn left then forward
goy5022 2:997f57aee3b7 170 break;
goy5022 2:997f57aee3b7 171 case SOUTH:
goy5022 2:997f57aee3b7 172 // go forward
goy5022 2:997f57aee3b7 173 break;
goy5022 2:997f57aee3b7 174 case EAST:
goy5022 2:997f57aee3b7 175 // turn right then forward
goy5022 2:997f57aee3b7 176 break;
goy5022 2:997f57aee3b7 177 }
goy5022 2:997f57aee3b7 178 }
goy5022 2:997f57aee3b7 179 void moveEAST()
goy5022 2:997f57aee3b7 180 {
goy5022 2:997f57aee3b7 181 switch(orientation)
goy5022 2:997f57aee3b7 182 {
goy5022 2:997f57aee3b7 183 case NORTH:
goy5022 2:997f57aee3b7 184 // turn right then forward...
goy5022 2:997f57aee3b7 185 break;
goy5022 2:997f57aee3b7 186 case WEST:
goy5022 2:997f57aee3b7 187 // turn around then foward
goy5022 2:997f57aee3b7 188 break;
goy5022 2:997f57aee3b7 189 case SOUTH:
goy5022 2:997f57aee3b7 190 // turn left then forward
goy5022 2:997f57aee3b7 191 break;
goy5022 2:997f57aee3b7 192 case EAST:
goy5022 2:997f57aee3b7 193 // go forward
goy5022 2:997f57aee3b7 194 break;
goy5022 2:997f57aee3b7 195 }
goy5022 2:997f57aee3b7 196 }
goy5022 2:997f57aee3b7 197 void moveWEST()
goy5022 2:997f57aee3b7 198 {
goy5022 2:997f57aee3b7 199 switch(orientation)
goy5022 2:997f57aee3b7 200 {
goy5022 2:997f57aee3b7 201 case NORTH:
goy5022 2:997f57aee3b7 202 // turn left then forward...
goy5022 2:997f57aee3b7 203 break;
goy5022 2:997f57aee3b7 204 case WEST:
goy5022 2:997f57aee3b7 205 // go forward
goy5022 2:997f57aee3b7 206 break;
goy5022 2:997f57aee3b7 207 case SOUTH:
goy5022 2:997f57aee3b7 208 // turn left then forward
goy5022 2:997f57aee3b7 209 break;
goy5022 2:997f57aee3b7 210 case EAST:
goy5022 2:997f57aee3b7 211 // turn around then forward
goy5022 2:997f57aee3b7 212 break;
goy5022 2:997f57aee3b7 213 }
goy5022 0:c2ec30f28676 214 }