Gal Yaroslavsky / MicroMouse-v1

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?

UserRevisionLine numberNew 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 }