Gal Yaroslavsky / MicroMouse-v1

asdf

Dependencies:   L3GD20 LSM303DLHC mbed

Main.cpp@3:1a8a7cc709cc, 2014-04-03 (annotated)

Committer:
goy5022
Date:
Thu Apr 03 15:52:54 2014 +0000
Revision:
3:1a8a7cc709cc
Parent:
2:997f57aee3b7
Child:
4:ecfe2115e9a4
PID is a little spotty but is working. Also handbrake just added

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 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 3:1a8a7cc709cc 101 //while (true)
goy5022 3:1a8a7cc709cc 102 while(SenseF.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 3:1a8a7cc709cc 113 setRightSpeed(4);
goy5022 3:1a8a7cc709cc 114 setLeftSpeed(3);
goy5022 3:1a8a7cc709cc 115 wait_ms(20);
goy5022 3:1a8a7cc709cc 116
goy5022 2:997f57aee3b7 117 }
goy5022 2:997f57aee3b7 118 else if(PIDv > .1)
goy5022 2:997f57aee3b7 119 {
goy5022 3:1a8a7cc709cc 120 setRightSpeed(3);
goy5022 3:1a8a7cc709cc 121 setLeftSpeed(4);
goy5022 3:1a8a7cc709cc 122 wait_ms(20);
goy5022 2:997f57aee3b7 123 }
goy5022 2:997f57aee3b7 124 else
goy5022 2:997f57aee3b7 125 {
goy5022 2:997f57aee3b7 126 setRightSpeed(3);
goy5022 2:997f57aee3b7 127 setLeftSpeed(3);
goy5022 3:1a8a7cc709cc 128 }
goy5022 3:1a8a7cc709cc 129 //check_walls();
goy5022 0:c2ec30f28676 130 }
goy5022 0:c2ec30f28676 131
goy5022 2:997f57aee3b7 132 //setRightSpeed(-2);
goy5022 2:997f57aee3b7 133 //setLeftSpeed(-2);
goy5022 2:997f57aee3b7 134
goy5022 2:997f57aee3b7 135 //stop_mov();
goy5022 3:1a8a7cc709cc 136 wait_ms(20);
goy5022 3:1a8a7cc709cc 137 handbrake();
goy5022 0:c2ec30f28676 138 return 0;
goy5022 2:997f57aee3b7 139 }
goy5022 2:997f57aee3b7 140 void moveNORTH()
goy5022 2:997f57aee3b7 141 {
goy5022 2:997f57aee3b7 142 switch(orientation)
goy5022 2:997f57aee3b7 143 {
goy5022 2:997f57aee3b7 144 case NORTH:
goy5022 2:997f57aee3b7 145 // go forward...
goy5022 2:997f57aee3b7 146 break;
goy5022 2:997f57aee3b7 147 case WEST:
goy5022 2:997f57aee3b7 148 // turn right then forward
goy5022 2:997f57aee3b7 149 break;
goy5022 2:997f57aee3b7 150 case SOUTH:
goy5022 2:997f57aee3b7 151 // turn around then forward
goy5022 2:997f57aee3b7 152 break;
goy5022 2:997f57aee3b7 153 case EAST:
goy5022 2:997f57aee3b7 154 // turn left then forward
goy5022 2:997f57aee3b7 155 break;
goy5022 2:997f57aee3b7 156 }
goy5022 2:997f57aee3b7 157 }
goy5022 2:997f57aee3b7 158 void moveSOUTH()
goy5022 2:997f57aee3b7 159 {
goy5022 2:997f57aee3b7 160 switch(orientation)
goy5022 2:997f57aee3b7 161 {
goy5022 2:997f57aee3b7 162 case NORTH:
goy5022 2:997f57aee3b7 163 // turn around then forward...
goy5022 2:997f57aee3b7 164 break;
goy5022 2:997f57aee3b7 165 case WEST:
goy5022 2:997f57aee3b7 166 // turn left then forward
goy5022 2:997f57aee3b7 167 break;
goy5022 2:997f57aee3b7 168 case SOUTH:
goy5022 2:997f57aee3b7 169 // go forward
goy5022 2:997f57aee3b7 170 break;
goy5022 2:997f57aee3b7 171 case EAST:
goy5022 2:997f57aee3b7 172 // turn right then forward
goy5022 2:997f57aee3b7 173 break;
goy5022 2:997f57aee3b7 174 }
goy5022 2:997f57aee3b7 175 }
goy5022 2:997f57aee3b7 176 void moveEAST()
goy5022 2:997f57aee3b7 177 {
goy5022 2:997f57aee3b7 178 switch(orientation)
goy5022 2:997f57aee3b7 179 {
goy5022 2:997f57aee3b7 180 case NORTH:
goy5022 2:997f57aee3b7 181 // turn right then forward...
goy5022 2:997f57aee3b7 182 break;
goy5022 2:997f57aee3b7 183 case WEST:
goy5022 2:997f57aee3b7 184 // turn around then foward
goy5022 2:997f57aee3b7 185 break;
goy5022 2:997f57aee3b7 186 case SOUTH:
goy5022 2:997f57aee3b7 187 // turn left then forward
goy5022 2:997f57aee3b7 188 break;
goy5022 2:997f57aee3b7 189 case EAST:
goy5022 2:997f57aee3b7 190 // go forward
goy5022 2:997f57aee3b7 191 break;
goy5022 2:997f57aee3b7 192 }
goy5022 2:997f57aee3b7 193 }
goy5022 2:997f57aee3b7 194 void moveWEST()
goy5022 2:997f57aee3b7 195 {
goy5022 2:997f57aee3b7 196 switch(orientation)
goy5022 2:997f57aee3b7 197 {
goy5022 2:997f57aee3b7 198 case NORTH:
goy5022 2:997f57aee3b7 199 // turn left then forward...
goy5022 2:997f57aee3b7 200 break;
goy5022 2:997f57aee3b7 201 case WEST:
goy5022 2:997f57aee3b7 202 // go forward
goy5022 2:997f57aee3b7 203 break;
goy5022 2:997f57aee3b7 204 case SOUTH:
goy5022 2:997f57aee3b7 205 // turn left then forward
goy5022 2:997f57aee3b7 206 break;
goy5022 2:997f57aee3b7 207 case EAST:
goy5022 2:997f57aee3b7 208 // turn around then forward
goy5022 2:997f57aee3b7 209 break;
goy5022 2:997f57aee3b7 210 }
goy5022 0:c2ec30f28676 211 }