asdf
Dependencies: L3GD20 LSM303DLHC mbed
Diff: Main.cpp
- Revision:
- 0:c2ec30f28676
- Child:
- 1:cfe6a6ad8dca
diff -r 000000000000 -r c2ec30f28676 Main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Main.cpp Sat Mar 29 03:06:46 2014 +0000 @@ -0,0 +1,166 @@ +#include "Core.h" + +#include <iostream> +using namespace std; + + +PwmOut ledF(p23); +PwmOut ledR(p21); +PwmOut ledL(p22); +AnalogIn SenseR(p15); +AnalogIn SenseL(p16); +AnalogIn SenseF(p17); +IMUfilter imuFilter(0.01, 0.10f); + +Timer p; + float value = 0; +/* +float initRight() +{ + + int best = 0; + for(int i = 0; i < 1000 && !done; i++) + { + wait_us(100); + ledR = ((float)i)/1000.0f; + WIRELESS.printf("\t%i\n\r", i); + float tot = 0; + for(int j = 0; j < 10; j++) + { + tot+= SenseR.read(); + } + + if(tot / 10.0f > 0.9f) + { + tot = 0; + for(int k = 0; k < 1000; k++) + { + tot+= SenseR.read(); + } + + if(tot / 1000 > 0.9f) + { + done = true; + best = i; + } + } + + } + + WIRELESS.printf("BEST: %i \n\r", best); + + return 0.0f; +} +*/ + + +float modulate() +{ + float tot = 0.0f; + for(float i = .1; i < 1.0f; i+= 0.05f) + { + + + tot = 0.0f; + wait_us(2000); + ledR.write(i); + wait_us(2000); + + for(int j = 0; j < 50; j++) + { + tot+= SenseR.read(); + } + + WIRELESS.printf("%f\t%f\n\r", i, (tot / 50.0f)); + + if((tot / 50.0f) >= 0.75f) + { + return i; + } + + } + + + return tot; + +} + +float oss() +{ + ledF.write(0.0f); + wait_ms(1); + ledF = 0.5; + p.start(); + + float time = 0.0f; + while(p.read_ms() < 10) + { + wait_ms(1); + if(time == 0.0f && SenseF.read() < .9f) + { + time = p.read_ms(); + value = SenseF.read(); + ledF.write(0.0f); + } + } + + p.stop(); + p.reset(); + + + return 1.0f - (time/10.0f); +} + + +int main() +{ + + Motor.baud(115200); + ledF.period_us(26.3); //28.5 + ledR.period_us(26.3); //28.5 + ledL.period_us(26.3); //28.5 + WIRELESS.printf("STARTING"); + ledL.write(0.33f); + wait_us(100); + ledR.write(0.33f); + wait_us(100); + ledF.write(0.5f); + + initMapping(); + float tot = 0; + float p = 0; + float avg; + while(1) + { + /* switch(WIRELESS.getChar()) + { + case 't': + WIRELESS.printf("%f", initRight()); + break; + + } + + */ + + + + //WIRELESS.printf("%f::\n\r", i); + compass.read(&ax, &ay, &az, &mx, &my, &mz); + gyro.read(&gx, &gy, &gz); + //double Gx = gx; double Gy = gy; double Gz = gz; + //double Ax = ax; double Ay = ay; double Az = az; + imuFilter.updateFilter(gx * (PI/18000), gy * (PI/18000), (gz + 0.66f) * (PI/18000), ax, ay, az); + imuFilter.computeEuler(); + + + + tot += gz; + p+= 1.0f; + avg = tot/p; + WIRELESS.printf("RAW: %f AVG: %f NOISE?: %f\n\r", gz, avg, 1000.0f * imuFilter.getYaw()); + wait_ms(10); + + } + + return 0; +} \ No newline at end of file