Gal Yaroslavsky / MicroMouse-v1

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?

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