asdf
Dependencies: L3GD20 LSM303DLHC mbed
Diff: Main.cpp
- Revision:
- 4:ecfe2115e9a4
- Parent:
- 3:1a8a7cc709cc
- Child:
- 5:9e504a5a1f48
diff -r 1a8a7cc709cc -r ecfe2115e9a4 Main.cpp --- a/Main.cpp Thu Apr 03 15:52:54 2014 +0000 +++ b/Main.cpp Thu Apr 03 17:34:00 2014 +0000 @@ -82,6 +82,7 @@ leftExpAvg.add(l); frontExpAvg.add(f); */ + leftExpAvg.add(l); rightExpAvg.add(r); frontExpAvg.add(f); //leftBufferAvg.add(l); @@ -90,8 +91,10 @@ WIRELESS.printf("%i || F: %f \n\r", i, frontExpAvg.average()); } - - float avgRight = rightExpAvg.average(); + cal_R = abs(log(1-rightExpAvg.average())); + cal_L = abs(log(1-leftExpAvg.average())); + //cal_F = abs(log(1-frontExpAvg.average())); + //float avgRight = rightExpAvg.average(); setRightSpeed(3); @@ -100,25 +103,25 @@ //while (true) while(SenseF.read() < 0.99) + //while(frontExpAvg.average() < .90 ) //&& wallRight() && wallLeft() { -// frontExpAvg.add(SenseF.read()); + 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("R: %f L: %f\n\r", cal_R, cal_L); + float PIDv = PID(abs(log(1-SenseR.read())), abs(log(1-SenseL.read()))); //WIRELESS.printf("ERR: %f Front: %f\n\r", PIDv, SenseF.read()); if(PIDv < -0.1) { - setRightSpeed(4); - setLeftSpeed(3); + setRightSpeed(3); + setLeftSpeed(4); wait_ms(20); } else if(PIDv > .1) { - setRightSpeed(3); - setLeftSpeed(4); + setRightSpeed(4); //** Just flipped these values + setLeftSpeed(3); wait_ms(20); } else