asdf
Dependencies: L3GD20 LSM303DLHC mbed
Main.cpp@0:c2ec30f28676, 2014-03-29 (annotated)
- Committer:
- goy5022
- Date:
- Sat Mar 29 03:06:46 2014 +0000
- Revision:
- 0:c2ec30f28676
- Child:
- 1:cfe6a6ad8dca
dcsddf
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
goy5022 | 0:c2ec30f28676 | 1 | #include "Core.h" |
goy5022 | 0:c2ec30f28676 | 2 | |
goy5022 | 0:c2ec30f28676 | 3 | #include <iostream> |
goy5022 | 0:c2ec30f28676 | 4 | using namespace std; |
goy5022 | 0:c2ec30f28676 | 5 | |
goy5022 | 0:c2ec30f28676 | 6 | |
goy5022 | 0:c2ec30f28676 | 7 | PwmOut ledF(p23); |
goy5022 | 0:c2ec30f28676 | 8 | PwmOut ledR(p21); |
goy5022 | 0:c2ec30f28676 | 9 | PwmOut ledL(p22); |
goy5022 | 0:c2ec30f28676 | 10 | AnalogIn SenseR(p15); |
goy5022 | 0:c2ec30f28676 | 11 | AnalogIn SenseL(p16); |
goy5022 | 0:c2ec30f28676 | 12 | AnalogIn SenseF(p17); |
goy5022 | 0:c2ec30f28676 | 13 | IMUfilter imuFilter(0.01, 0.10f); |
goy5022 | 0:c2ec30f28676 | 14 | |
goy5022 | 0:c2ec30f28676 | 15 | Timer p; |
goy5022 | 0:c2ec30f28676 | 16 | float value = 0; |
goy5022 | 0:c2ec30f28676 | 17 | /* |
goy5022 | 0:c2ec30f28676 | 18 | float initRight() |
goy5022 | 0:c2ec30f28676 | 19 | { |
goy5022 | 0:c2ec30f28676 | 20 | |
goy5022 | 0:c2ec30f28676 | 21 | int best = 0; |
goy5022 | 0:c2ec30f28676 | 22 | for(int i = 0; i < 1000 && !done; i++) |
goy5022 | 0:c2ec30f28676 | 23 | { |
goy5022 | 0:c2ec30f28676 | 24 | wait_us(100); |
goy5022 | 0:c2ec30f28676 | 25 | ledR = ((float)i)/1000.0f; |
goy5022 | 0:c2ec30f28676 | 26 | WIRELESS.printf("\t%i\n\r", i); |
goy5022 | 0:c2ec30f28676 | 27 | float tot = 0; |
goy5022 | 0:c2ec30f28676 | 28 | for(int j = 0; j < 10; j++) |
goy5022 | 0:c2ec30f28676 | 29 | { |
goy5022 | 0:c2ec30f28676 | 30 | tot+= SenseR.read(); |
goy5022 | 0:c2ec30f28676 | 31 | } |
goy5022 | 0:c2ec30f28676 | 32 | |
goy5022 | 0:c2ec30f28676 | 33 | if(tot / 10.0f > 0.9f) |
goy5022 | 0:c2ec30f28676 | 34 | { |
goy5022 | 0:c2ec30f28676 | 35 | tot = 0; |
goy5022 | 0:c2ec30f28676 | 36 | for(int k = 0; k < 1000; k++) |
goy5022 | 0:c2ec30f28676 | 37 | { |
goy5022 | 0:c2ec30f28676 | 38 | tot+= SenseR.read(); |
goy5022 | 0:c2ec30f28676 | 39 | } |
goy5022 | 0:c2ec30f28676 | 40 | |
goy5022 | 0:c2ec30f28676 | 41 | if(tot / 1000 > 0.9f) |
goy5022 | 0:c2ec30f28676 | 42 | { |
goy5022 | 0:c2ec30f28676 | 43 | done = true; |
goy5022 | 0:c2ec30f28676 | 44 | best = i; |
goy5022 | 0:c2ec30f28676 | 45 | } |
goy5022 | 0:c2ec30f28676 | 46 | } |
goy5022 | 0:c2ec30f28676 | 47 | |
goy5022 | 0:c2ec30f28676 | 48 | } |
goy5022 | 0:c2ec30f28676 | 49 | |
goy5022 | 0:c2ec30f28676 | 50 | WIRELESS.printf("BEST: %i \n\r", best); |
goy5022 | 0:c2ec30f28676 | 51 | |
goy5022 | 0:c2ec30f28676 | 52 | return 0.0f; |
goy5022 | 0:c2ec30f28676 | 53 | } |
goy5022 | 0:c2ec30f28676 | 54 | */ |
goy5022 | 0:c2ec30f28676 | 55 | |
goy5022 | 0:c2ec30f28676 | 56 | |
goy5022 | 0:c2ec30f28676 | 57 | float modulate() |
goy5022 | 0:c2ec30f28676 | 58 | { |
goy5022 | 0:c2ec30f28676 | 59 | float tot = 0.0f; |
goy5022 | 0:c2ec30f28676 | 60 | for(float i = .1; i < 1.0f; i+= 0.05f) |
goy5022 | 0:c2ec30f28676 | 61 | { |
goy5022 | 0:c2ec30f28676 | 62 | |
goy5022 | 0:c2ec30f28676 | 63 | |
goy5022 | 0:c2ec30f28676 | 64 | tot = 0.0f; |
goy5022 | 0:c2ec30f28676 | 65 | wait_us(2000); |
goy5022 | 0:c2ec30f28676 | 66 | ledR.write(i); |
goy5022 | 0:c2ec30f28676 | 67 | wait_us(2000); |
goy5022 | 0:c2ec30f28676 | 68 | |
goy5022 | 0:c2ec30f28676 | 69 | for(int j = 0; j < 50; j++) |
goy5022 | 0:c2ec30f28676 | 70 | { |
goy5022 | 0:c2ec30f28676 | 71 | tot+= SenseR.read(); |
goy5022 | 0:c2ec30f28676 | 72 | } |
goy5022 | 0:c2ec30f28676 | 73 | |
goy5022 | 0:c2ec30f28676 | 74 | WIRELESS.printf("%f\t%f\n\r", i, (tot / 50.0f)); |
goy5022 | 0:c2ec30f28676 | 75 | |
goy5022 | 0:c2ec30f28676 | 76 | if((tot / 50.0f) >= 0.75f) |
goy5022 | 0:c2ec30f28676 | 77 | { |
goy5022 | 0:c2ec30f28676 | 78 | return i; |
goy5022 | 0:c2ec30f28676 | 79 | } |
goy5022 | 0:c2ec30f28676 | 80 | |
goy5022 | 0:c2ec30f28676 | 81 | } |
goy5022 | 0:c2ec30f28676 | 82 | |
goy5022 | 0:c2ec30f28676 | 83 | |
goy5022 | 0:c2ec30f28676 | 84 | return tot; |
goy5022 | 0:c2ec30f28676 | 85 | |
goy5022 | 0:c2ec30f28676 | 86 | } |
goy5022 | 0:c2ec30f28676 | 87 | |
goy5022 | 0:c2ec30f28676 | 88 | float oss() |
goy5022 | 0:c2ec30f28676 | 89 | { |
goy5022 | 0:c2ec30f28676 | 90 | ledF.write(0.0f); |
goy5022 | 0:c2ec30f28676 | 91 | wait_ms(1); |
goy5022 | 0:c2ec30f28676 | 92 | ledF = 0.5; |
goy5022 | 0:c2ec30f28676 | 93 | p.start(); |
goy5022 | 0:c2ec30f28676 | 94 | |
goy5022 | 0:c2ec30f28676 | 95 | float time = 0.0f; |
goy5022 | 0:c2ec30f28676 | 96 | while(p.read_ms() < 10) |
goy5022 | 0:c2ec30f28676 | 97 | { |
goy5022 | 0:c2ec30f28676 | 98 | wait_ms(1); |
goy5022 | 0:c2ec30f28676 | 99 | if(time == 0.0f && SenseF.read() < .9f) |
goy5022 | 0:c2ec30f28676 | 100 | { |
goy5022 | 0:c2ec30f28676 | 101 | time = p.read_ms(); |
goy5022 | 0:c2ec30f28676 | 102 | value = SenseF.read(); |
goy5022 | 0:c2ec30f28676 | 103 | ledF.write(0.0f); |
goy5022 | 0:c2ec30f28676 | 104 | } |
goy5022 | 0:c2ec30f28676 | 105 | } |
goy5022 | 0:c2ec30f28676 | 106 | |
goy5022 | 0:c2ec30f28676 | 107 | p.stop(); |
goy5022 | 0:c2ec30f28676 | 108 | p.reset(); |
goy5022 | 0:c2ec30f28676 | 109 | |
goy5022 | 0:c2ec30f28676 | 110 | |
goy5022 | 0:c2ec30f28676 | 111 | return 1.0f - (time/10.0f); |
goy5022 | 0:c2ec30f28676 | 112 | } |
goy5022 | 0:c2ec30f28676 | 113 | |
goy5022 | 0:c2ec30f28676 | 114 | |
goy5022 | 0:c2ec30f28676 | 115 | int main() |
goy5022 | 0:c2ec30f28676 | 116 | { |
goy5022 | 0:c2ec30f28676 | 117 | |
goy5022 | 0:c2ec30f28676 | 118 | Motor.baud(115200); |
goy5022 | 0:c2ec30f28676 | 119 | ledF.period_us(26.3); //28.5 |
goy5022 | 0:c2ec30f28676 | 120 | ledR.period_us(26.3); //28.5 |
goy5022 | 0:c2ec30f28676 | 121 | ledL.period_us(26.3); //28.5 |
goy5022 | 0:c2ec30f28676 | 122 | WIRELESS.printf("STARTING"); |
goy5022 | 0:c2ec30f28676 | 123 | ledL.write(0.33f); |
goy5022 | 0:c2ec30f28676 | 124 | wait_us(100); |
goy5022 | 0:c2ec30f28676 | 125 | ledR.write(0.33f); |
goy5022 | 0:c2ec30f28676 | 126 | wait_us(100); |
goy5022 | 0:c2ec30f28676 | 127 | ledF.write(0.5f); |
goy5022 | 0:c2ec30f28676 | 128 | |
goy5022 | 0:c2ec30f28676 | 129 | initMapping(); |
goy5022 | 0:c2ec30f28676 | 130 | float tot = 0; |
goy5022 | 0:c2ec30f28676 | 131 | float p = 0; |
goy5022 | 0:c2ec30f28676 | 132 | float avg; |
goy5022 | 0:c2ec30f28676 | 133 | while(1) |
goy5022 | 0:c2ec30f28676 | 134 | { |
goy5022 | 0:c2ec30f28676 | 135 | /* switch(WIRELESS.getChar()) |
goy5022 | 0:c2ec30f28676 | 136 | { |
goy5022 | 0:c2ec30f28676 | 137 | case 't': |
goy5022 | 0:c2ec30f28676 | 138 | WIRELESS.printf("%f", initRight()); |
goy5022 | 0:c2ec30f28676 | 139 | break; |
goy5022 | 0:c2ec30f28676 | 140 | |
goy5022 | 0:c2ec30f28676 | 141 | } |
goy5022 | 0:c2ec30f28676 | 142 | |
goy5022 | 0:c2ec30f28676 | 143 | */ |
goy5022 | 0:c2ec30f28676 | 144 | |
goy5022 | 0:c2ec30f28676 | 145 | |
goy5022 | 0:c2ec30f28676 | 146 | |
goy5022 | 0:c2ec30f28676 | 147 | //WIRELESS.printf("%f::\n\r", i); |
goy5022 | 0:c2ec30f28676 | 148 | compass.read(&ax, &ay, &az, &mx, &my, &mz); |
goy5022 | 0:c2ec30f28676 | 149 | gyro.read(&gx, &gy, &gz); |
goy5022 | 0:c2ec30f28676 | 150 | //double Gx = gx; double Gy = gy; double Gz = gz; |
goy5022 | 0:c2ec30f28676 | 151 | //double Ax = ax; double Ay = ay; double Az = az; |
goy5022 | 0:c2ec30f28676 | 152 | imuFilter.updateFilter(gx * (PI/18000), gy * (PI/18000), (gz + 0.66f) * (PI/18000), ax, ay, az); |
goy5022 | 0:c2ec30f28676 | 153 | imuFilter.computeEuler(); |
goy5022 | 0:c2ec30f28676 | 154 | |
goy5022 | 0:c2ec30f28676 | 155 | |
goy5022 | 0:c2ec30f28676 | 156 | |
goy5022 | 0:c2ec30f28676 | 157 | tot += gz; |
goy5022 | 0:c2ec30f28676 | 158 | p+= 1.0f; |
goy5022 | 0:c2ec30f28676 | 159 | avg = tot/p; |
goy5022 | 0:c2ec30f28676 | 160 | WIRELESS.printf("RAW: %f AVG: %f NOISE?: %f\n\r", gz, avg, 1000.0f * imuFilter.getYaw()); |
goy5022 | 0:c2ec30f28676 | 161 | wait_ms(10); |
goy5022 | 0:c2ec30f28676 | 162 | |
goy5022 | 0:c2ec30f28676 | 163 | } |
goy5022 | 0:c2ec30f28676 | 164 | |
goy5022 | 0:c2ec30f28676 | 165 | return 0; |
goy5022 | 0:c2ec30f28676 | 166 | } |