Peiling Yi
la4-2(unfinished)
Fork of
Accelerometer_example
by 
William Marsh
Revision 3:374bdcd8ee4c, committed 2018-02-24
- Comitter:
- Peilingyi
- Date:
- Sat Feb 24 18:12:30 2018 +0000
- Parent:
- 2:eebefb7c9690
- Commit message:
- lab4-part2
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r eebefb7c9690 -r 374bdcd8ee4c main.cpp
--- a/main.cpp Fri Feb 16 23:37:53 2018 +0000
+++ b/main.cpp Sat Feb 24 18:12:30 2018 +0000
@@ -7,165 +7,214 @@
#define MMA8451_I2C_ADDRESS (0x1d<<1)
-Thread thread1;
-DigitalOut ledr(LED1);
-DigitalOut ledg(LED2);
-timer t;
-
-void stationary
+int main(void)
{
- while(1)
- {
- if ((abs((x+y+z)-1))<threshold)
- {
- switch (state)
- case 0:
- ledr=1;
- ledg=1;
- state = 1;
- break;
- case 1:
- if ((abs(z-1)<=errorrange))//flat
+ MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS);
+ DigitalOut ledr(LED1);
+ DigitalOut ledg(LED2);
+ DigitalOut ledb(LED3);
+// PwmOut rled(LED1);
+ // PwmOut gled(LED2);
+// PwmOut bled(LED3);
+ Serial pc(USBTX, USBRX); // tx, rx
+ Timer time;
+
+
+ float x, y, z;
+ float errorrange = 0.1;
+ int filter = 3;
+ int countflag = 0;
+ int step =1;
+ int state = 1;
+ int timeflag = 0;
+ float t,t3;
+ ledr = 1;
+ ledg = 1;
+ ledb = 1;
+
+
+
+ pc.printf("MMA8451 ID: %d\n", acc.getWhoAmI());
+
+ while (true) {
+
+ x = acc.getAccX();
+ y = acc.getAccY();
+ z = acc.getAccZ();
+
+
+ Thread::wait(300);
+ // pc.printf("X: %1.2f, Y: %1.2f, Z: %1.2f\n\r", x, y, z);
+
+ switch (state)
+ {
+ case 1:
+ {
+ if ((timeflag ==0)&&(step ==1))
{
- if (countflag == 0)
- {
- pc.printf("flat");
- countflag = filter;
- ledg = 0;
- t.start();
- // state = 1;//for flat
- }
- else
- {
- countflag--;
- }
- }
+ time.start();
+ ledg =0;
+ timeflag =1;
+ pc.printf("flat start\n\r");
+ }
else
{
- t.stop();
- temp_t = t.read();
- if (temp_t>10)
+ t = time.read();
+ pc.printf("time:%1.3f\n\r",t);
+ if(t>10.00)
{
- state = 2;
- steps = 1;
- ledg = 0;
- ledr = 1;
+ time.stop();
+ time.reset();
+ step=2;
+ timeflag=0;
+ pc.printf("flat stop\n\r");
}
- else
+
+ }
+ break;
+ }
+
+ case 2:
+ {
+ if ((timeflag == 0)&&(step==2))
+ {
+ time.start();
+ timeflag=1;
+ pc.printf("right start\n\r");
+ }
+ else
+ {
+ t= time.read();
+ pc.printf("time:%1.3f\n\r",t);
+ if((t>2.0)&&(t<6.0))
{
- ledg = 1;
- ledr = 0;
- state =0;
- pc.printf("error!");
+ step=3;
+ timeflag=0;
+ pc.printf("right end\n\r");
}
-
+
+ }
+ break;
+ }
+
+ case 3:
+ {
+ if ((step==3)&&(timeflag==0))
+ {
+ t3 = time.read();
+ timeflag = 1;
+ pc.printf("up start\n\r");
}
-
- }
- break;
- case 2:
+ else
+ {
+ t = time.read();
+ pc.printf("time:%1.3f\n\r",t);
+ if(((t-t3)>4.0)&&((t-t3)<8))
+ {
+ step =1;
+ timeflag = 0;
+ state = 0;
+ pc.printf("up end\n\r");
+ time.stop();
+ time.reset();
+ }
+ }
+ break;
+ }
+ case 4:
+ {
+ pc.printf("error\n\r");
+ ledg = 1;
+ ledr = 0;
+ step =1;
+ timeflag = 0;
+ state = 0;
+ time.reset();
+ break;
+
+ }
+ }
+
+
+
+ float temp = (x+y+z);
+ if(((temp<1.2)&&(temp>0.8))||((temp<-0.8)&&(temp>-1.2)))
{
- if (abs(x+1.0f)<=errorrange)
+ if ((abs(z-1.00)<=errorrange))
{
+
if (countflag == 0)
{
- pc.printf("right");
+ pc.printf("flat");
countflag = filter;
- t.start();
-
+ state = 1;
+
}
else
{
countflag--;
}
- }
- else
- {
- t.stop();
- temp_t = t.read();
- if ((temp_t>=2) && (temp_t<=6))
- {
- state = 3;
- steps = 2;
- }
- else
- {
- ledg = 1;
- ledr = 0;
- pc.printf("error!");
- }
+ }
+
+ if (abs(1.00+x)<=errorrange)
+ {
+ if (countflag == 0)
+ {
+ pc.printf("right");
+ countflag = filter;
+ state = 2;
+ }
+ else
+ {
+ countflag--;
+ }
+ }
+
+
+ if (abs(y-1.00)<=errorrange)
+ {
+
+ if (countflag == 0)
+ {
+ pc.printf("up");
+ countflag = filter;
+ state = 3;
}
- }
- break;
- case 3:
- {
- if (abs(x+1.0f)<=errorrange)
+ else
{
- if (countflag == 0)
- {
- pc.printf("up");
- countflag = filter;
- t.start();
- }
- else
- {
- countflag--;
- }
- }
- else
+ countflag--;
+ }
+ }
+ if (abs(y+1.00)<=errorrange)
+ {
+ if (countflag == 0)
+ {
+ pc.printf("down");
+ countflag = filter;
+ state = 4;
+ }
+ else
{
- t.stop();
- temp_t = t.read();
- if ((temp_t>=4) && (temp_t<=8))
- {
- state = 0;
- }
- else
- {
- ledg = 1;
- ledr = 0;
- pc.printf("error!");
- }
-
+ countflag--;
}
- }
-
-
-
- }
+ }
+
+ if (abs(z+1.00)<=errorrange)
+ {
+ if (countflag == 0)
+ {
+ pc.printf("turn");
+ countflag = filter;
+ state = 4;
+ }
+ else
+ {
+ countflag--;
+ }
+ }
+
+ }
+ }
}
-int main(void)
-{
-
-
- MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS);
- PwmOut rled(LED1);
- PwmOut gled(LED2);
- PwmOut bled(LED3);
- Serial pc(USBTX, USBRX); // tx, rx
- ledg = 1;
- ledr = 1;
-
-
- thread1.start(callback(stationary ));
-
- pc.printf("MMA8451 ID: %d\n", acc.getWhoAmI());
-
- while (true) {
- float x, y, z;
-
- x = acc.getAccX();
- y = acc.getAccY();
- z = acc.getAccZ();
- rled = 1.0f - abs(x);
- gled = 1.0f - abs(y);
- bled = 1.0f - abs(z);
- Thread::wait(300);
-
-
- pc.printf("X: %1.2f, Y: %1.2f, Z: %1.2f\n", x, y, z);
-
- }
-}