asdf
Dependencies: L3GD20 LSM303DLHC mbed
Diff: Main.cpp
- Revision:
- 7:95ebadc83fc7
- Parent:
- 6:6e96e93689df
--- a/Main.cpp Thu Apr 03 19:21:58 2014 +0000 +++ b/Main.cpp Thu Apr 03 23:57:31 2014 +0000 @@ -1,9 +1,10 @@ //#include "Core.h" #include "Sensors.h" -#include "Motors.h" +#include "PID.h" #include "Communication.h" #include "Flooding.h" +#include "Movement.h" #include "BufferAverage.h" #include "ExponentialAverage.h" @@ -20,36 +21,14 @@ wait(3); /* - - + 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: @@ -66,129 +45,38 @@ */ - initMapping(); + //initMapping(); initSensors(); - - setRightSpeed(3); setLeftSpeed(3); t.start(); //while (true) //while(SenseF.read() < 0.99) - while(frontExpAvg.average() < .8) // && wallRight() && wallLeft() - { - frontExpAvg.add(SenseF.read()); - - if ( wallRight() && wallLeft()) - PIDv = PID(linearize(SenseR.read()), linearize(SenseL.read())); - if ( wallRight() && !wallLeft()) - PIDv = PID(linearize(SenseR.read()),cal_R); - if ( !wallRight() && wallLeft()) - PIDv = PID(cal_L, linearize(SenseL.read())); - - if(PIDv < -0.1) - { - setRightSpeed(3); - setLeftSpeed(4); - wait_ms(20); + //while(true) + //{ + t.reset(); + while(frontExpAvg.average() < .8 && wallRight() && wallLeft()) + { + frontExpAvg.add(SenseF.read()); + + pid(); + } - else if(PIDv > .1) - { - setRightSpeed(4); //** Just flipped these values - setLeftSpeed(3); - wait_ms(20); - } - else - { - setRightSpeed(3); - setLeftSpeed(3); - } - //check_walls(); - //WIRELESS.printf("ERR: %f Front: %f\n\r", PIDv, SenseF.read()); - //WIRELESS.printf("R: %f L: %f\n\r", cal_R, cal_L); - } - - //setRightSpeed(-2); - //setLeftSpeed(-2); - - //stop_mov(); - wait_ms(20); - WIRELESS.printf("Time: %f\n\r", t.read()); - handbrake(); - wait_ms(20); - printMap(WIRELESS); + + float time = t.read(); + handbrake(); + wait_ms(200); + //updateMaze(); + //WIRELESS.printf("Time: %f Cells: %i\n\r", time, cells_traveled); + //printMap(WIRELESS); + //if (frontExpAvg.average() < .8 && !wallRight() && wallLeft()) + turn_right(3); + //if (frontExpAvg.average() < .8 && wallRight() && !wallLeft()) + // turn_left(3); + // } + + 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; - } -} \ No newline at end of file +