asdf
Dependencies: L3GD20 LSM303DLHC mbed
Main.cpp
- Committer:
- goy5022
- Date:
- 2014-04-03
- Revision:
- 2:997f57aee3b7
- Parent:
- 1:cfe6a6ad8dca
- Child:
- 3:1a8a7cc709cc
File content as of revision 2:997f57aee3b7:
//#include "Core.h" #include "Sensors.h" #include "Motors.h" #include "Communication.h" #include "Flooding.h" #include "BufferAverage.h" #include "ExponentialAverage.h" //#include "WeightedAverage.h" int main() { Motor.baud(115200); ledF.period_us(26.2); //28.5 //27.9 wait_us(600); ledF.write(.6); stop_mov(); wait(5); /* STEVE!!!!!! WHENEVER YOU ARE IN A CELL AND YOU WANT TO KNOW WHAT THE NEXT BEST MOVE IS USE THE flood_findPath(x, y) where x and y are your current possition the function will return a MOVE type and you can use something similar to this: Move k = flood_findPath(xPos, yPos); switch(k) { case M_NORTH: moveNORTH(); break; case M_SOUTH: moveSOUTH(); break; case M_WEST: moveWEST(); break; case M_EAST: moveEAST(); break; } i've written the code for the following: moveNORTH() moveSOUTH() moveEAST() moveWEST() they are at the bottom of the page. take a look and read them. 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. TO THE BOTTOM */ for(int i = 0; i < 100; i++) { float l = SenseL.read(); float r = SenseR.read(); float f = SenseF.read(); /* leftWeightedAvg.add(l); rightWeightedAvg.add(r); frontWeightedAvg.add(f); leftExpAvg.add(l); frontExpAvg.add(f); */ rightExpAvg.add(r); frontExpAvg.add(f); //leftBufferAvg.add(l); //rightBufferAvg.add(r); //frontBufferAvg.add(f); WIRELESS.printf("%i || F: %f \n\r", i, frontExpAvg.average()); } float avgRight = rightExpAvg.average(); setRightSpeed(3); setLeftSpeed(3); while (true) //while(SenseL.read() < 0.99) { // frontExpAvg.add(SenseF.read()); //WIRELESS.printf("R: %f L: %f F: %f\n\r", SenseR.read(), SenseL.read(), SenseF.read()); float PIDv = PID(SenseR.read(), SenseL.read()); //WIRELESS.printf("ERR: %f Front: %f\n\r", PIDv, SenseF.read()); if(PIDv < -0.1) { setRightSpeed(3); setLeftSpeed(4); } else if(PIDv > .1) { setRightSpeed(4); setLeftSpeed(3); } else { setRightSpeed(3); setLeftSpeed(3); } } //setRightSpeed(-2); //setLeftSpeed(-2); //stop_mov(); stop_mov(); return 0; } void moveNORTH() { switch(orientation) { case NORTH: // go forward... break; case WEST: // turn right then forward break; case SOUTH: // turn around then forward break; case EAST: // turn left then forward break; } } void moveSOUTH() { switch(orientation) { case NORTH: // turn around then forward... break; case WEST: // turn left then forward break; case SOUTH: // go forward break; case EAST: // turn right then forward break; } } void moveEAST() { switch(orientation) { case NORTH: // turn right then forward... break; case WEST: // turn around then foward break; case SOUTH: // turn left then forward break; case EAST: // go forward break; } } void moveWEST() { switch(orientation) { case NORTH: // turn left then forward... break; case WEST: // go forward break; case SOUTH: // turn left then forward break; case EAST: // turn around then forward break; } }