asdf
Dependencies: L3GD20 LSM303DLHC mbed
Diff: Main.cpp
- Revision:
- 2:997f57aee3b7
- Parent:
- 1:cfe6a6ad8dca
- Child:
- 3:1a8a7cc709cc
diff -r cfe6a6ad8dca -r 997f57aee3b7 Main.cpp --- a/Main.cpp Sat Mar 29 13:25:23 2014 +0000 +++ b/Main.cpp Thu Apr 03 13:15:40 2014 +0000 @@ -1,84 +1,207 @@ -#include "Core.h" - -#include <iostream> -using namespace std; - -IMUfilter imuFilter(0.01, 0.10f); +//#include "Core.h" -Ticker ticker; +#include "Sensors.h" +#include "Motors.h" +#include "Communication.h" +#include "Flooding.h" -void test() -{ - collectSample(); -} +#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(100); - //ledR.period_us(26.3); //28.5 // 26 - wait_us(100); - //ledL.period_us(26.3); //28.5 - ledF = 0.5; - wait_us(100); - ledR = 0.3; //31 & 26.2 - wait_us(100); - ledL = 0.651; //48 without cap - Motor.baud(115200); - //initMapping(); - //p.start(); - ticker.attach(&test, 0.002); - while(1) + 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) { - /* - switch(WIRELESS.getChar()) - { - case 'r': - WIRELESS.printf("Rights"); - turn_right(3); - break; - case 'l': - WIRELESS.printf("Lefts"); - turn_left(3); - break; - case 'f': - WIRELESS.printf("Front"); - forward(3); - break; - case 't': - WIRELESS.printf("%f", imuFilter.getYaw()); - break; - - }*/ + case M_NORTH: + moveNORTH(); + break; + + case M_SOUTH: + moveSOUTH(); + break; + + case M_WEST: + moveWEST(); + break; + + case M_EAST: + moveEAST(); + break; - // stop(); + } + + 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(); /* - compass.read(&ax, &ay, &az, &mx, &my, &mz); - gyro.read(&gx, &gy, &gz); - - curr = p.read(); + leftWeightedAvg.add(l); + rightWeightedAvg.add(r); + frontWeightedAvg.add(f); - imuFilter.updateFilter(gx * (PI/18000), gy * (PI/18000), (gz + 0.63f) * (PI/18000), ax, ay, az, .01); - imuFilter.computeEuler(); - last = curr; + leftExpAvg.add(l); + frontExpAvg.add(f); + */ + rightExpAvg.add(r); + frontExpAvg.add(f); + //leftBufferAvg.add(l); + //rightBufferAvg.add(r); + //frontBufferAvg.add(f); - WIRELESS.printf("%f %f \n\r", last-curr, 1000.0f * imuFilter.getYaw()); - wait_ms(10); - - //forward(5); - //return 0; + 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()); - //WIRELESS.printf("V: %f \tA: %f\n\r", new_value, accumulator); - - - forward(3); + 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; + } } \ No newline at end of file