asdf
Dependencies: L3GD20 LSM303DLHC mbed
Diff: Main.cpp
- Revision:
- 1:cfe6a6ad8dca
- Parent:
- 0:c2ec30f28676
- Child:
- 2:997f57aee3b7
diff -r c2ec30f28676 -r cfe6a6ad8dca Main.cpp --- a/Main.cpp Sat Mar 29 03:06:46 2014 +0000 +++ b/Main.cpp Sat Mar 29 13:25:23 2014 +0000 @@ -3,163 +3,81 @@ #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) - { +Ticker ticker; - - 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; - +void test() +{ + collectSample(); } -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); + 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.write(0.33f); + ledR = 0.3; //31 & 26.2 wait_us(100); - ledF.write(0.5f); - - initMapping(); - float tot = 0; - float p = 0; - float avg; + ledL = 0.651; //48 without cap + Motor.baud(115200); + //initMapping(); + //p.start(); + ticker.attach(&test, 0.002); while(1) { - /* switch(WIRELESS.getChar()) + /* + 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", initRight()); + WIRELESS.printf("%f", imuFilter.getYaw()); break; - } + }*/ + + // stop(); + + /* + compass.read(&ax, &ay, &az, &mx, &my, &mz); + gyro.read(&gx, &gy, &gz); + + curr = p.read(); + + imuFilter.updateFilter(gx * (PI/18000), gy * (PI/18000), (gz + 0.63f) * (PI/18000), ax, ay, az, .01); + imuFilter.computeEuler(); + + last = curr; + + WIRELESS.printf("%f %f \n\r", last-curr, 1000.0f * imuFilter.getYaw()); + wait_ms(10); + + //forward(5); + //return 0; */ - - //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); + //WIRELESS.printf("V: %f \tA: %f\n\r", new_value, accumulator); + + forward(3); } return 0;