Chawisorn samrit
kor bork wa cop koy ma
Fork of
testIMU2_copy2
by 
OX
Diff: main.cpp
- Revision:
- 2:6bc2c5d68446
- Parent:
- 0:77a7d1a1c6db
- Child:
- 3:ee0df78b0dd3
diff -r 77a7d1a1c6db -r 6bc2c5d68446 main.cpp
--- a/main.cpp Sat Dec 03 18:21:47 2016 +0000
+++ b/main.cpp Sun Dec 04 09:19:53 2016 +0000
@@ -5,7 +5,7 @@
ZMU9250 a;
Serial pc(D1, D0);
-float yaw_z, pitch_y, roll_x;
+float yaaw_z, piitch_y, rooll_x;
int ir,ip,iy;
int l;
int xlr,yfb,ztd;
@@ -13,11 +13,11 @@
char rt;
int cube1(int x, int y, int z);
-int seccon(float roll ,float pitch, float yaw);
+int seccon(float rooll ,float piitch, float yaaw);
int round(int rt_x, int rt_y, int rt_z);
-DigitalIn op_roll(USER_BUTTON);
+DigitalIn op_rooll(USER_BUTTON);
int main(){
@@ -25,22 +25,25 @@
while(1){
wait(0.1);
a.Update();
- yaw_z = a.Yaw();
- pitch_y = a.Pitch();
- roll_x = a.Roll();
+ yaaw_z = a.Yaw();
+ piitch_y = a.Pitch();
+ rooll_x = a.Roll();
-// pc.printf("Yaw_z %f\t Pitch_y %f\t Roll_x %f\n", yaw_z, pitch_y, roll_x);
+ // pc.printf("yaaw_z %f\t piitch_y %f\t rooll_x %f\n\n\n\r", yaaw_z, piitch_y, rooll_x);
//l = cube1(3,3,3);
-
- //pc.printf("%d\t Yaw_z %f\t Pitch_y %f\t Roll_x %f\n",l , yaw_z, pitch_y, roll_x); //add
-
- pc.printf("%d\t Yaw_z %f\t Pitch_y %f\t Roll_x %f\t %d\t %c\t %d\t \r\n ",round(5,5,5), yaw_z, pitch_y, roll_x, sata, rt, l); //round
+//
+// pc.printf("%d\t yaaw_z %f\t piitch_y %f\t rooll_x %f\n\n\n\r",l , yaaw_z, piitch_y, rooll_x); //add
+//
+ pc.printf("%d\t yaaw_z %f\t piitch_y %f\t rooll_x %f\t %d\t %c\t %d\t xlr %d\t yfb %d\t ztd %d\t \n\n\n\r ",round(5,5,5), yaaw_z, piitch_y, rooll_x, sata, rt, l,xlr ,yfb, ztd); //round
- // pc.printf( "%d\n" ,seccon(roll_x, pitch_y, yaw_z));
-// pc.printf("========\n"); //it
- if(op_roll == 0){
-
+ // pc.printf( "%d\n" ,seccon(rooll_x, piitch_y, yaaw_z));
+ // pc.printf("========\n");
+ //it
+ if(op_rooll == 0){
+ xlr = 0;
+ yfb = 0;
+ ztd = 0;
l = cube1(3,3,3);
}
@@ -48,59 +51,47 @@
}
}
- int cube1(int x, int y, int z){
-
- if( ((pitch_y > -30) && (pitch_y < 30)) ){ // vCC L leg D
-
- if( ((yaw_z > 36) && (yaw_z < 113)) && ((roll_x > -40) && (roll_x < 40)) ){
- x++; //R x+
- xlr = 1;
-
-
- }
- if( ((yaw_z > -101) && (yaw_z < 35)) && ((roll_x > -40) && (roll_x < 40)) ){
- y--; //B y+
- yfb = -1;
- }
- if( ((yaw_z > 110) && (yaw_z < 179)) && ((roll_x > -40) && (roll_x < 40)) ){
- y++; //F y-
- yfb = 1;
-
- }
- if( ((yaw_z > -180) && (yaw_z < -100)) && ((roll_x > -40) && (roll_x < 40)) ){
- x--; //L x-
- xlr = -1;
-
- }
- if((roll_x > 41) && (roll_x < 130) ){
- z++; //T z+
- ztd = 1;
-
- }
- if( (roll_x > -130) && (roll_x < -41)){
- z--; //D z-
- ztd = -1;
-
- }
+ int cube1(int x, int y, int z){
+ // vCC LEFT
+ if( ((yaaw_z > 36) && (yaaw_z < 113)) && ( ((rooll_x > -40) && (rooll_x < 40)) ) ){
+ x++; //R
+ xlr = 1;
}
-
-
+ if( ((yaaw_z > -74) && (yaaw_z < 35)) && ( ((rooll_x > -40) && (rooll_x < 40)) ) ){
+ y--; //B
+ yfb = -1;
+ }
+ if( ((yaaw_z > 110) && (yaaw_z < 179)) &&( ((rooll_x > -40) && (rooll_x < 40)) ) ){
+ y++; //F
+ yfb = 1;
+ }
+ if( ((yaaw_z > -180) && (yaaw_z < -75)) &&( ((rooll_x > -40) && (rooll_x < 40)) ) ){
+ x--; //L
+ xlr = -1;
+ }
+ if((rooll_x > 41) && (rooll_x < 130) ){
+ z++; //T
+ ztd = 1;
+ }
+ if( (rooll_x > -130) && (rooll_x < -41)){
+ z--; //D
+ ztd = -1;
+ }
return (x*100+y*10+z);
}
-
- int seccon(float roll ,float pitch, float yaw){
- if( ( ((yaw > -50) && (yaw < -30)) || ((yaw > -145) && (yaw < -125)) || ((yaw > 140) && (yaw < 160))||((yaw > 0) && (yaw < -20)) )
- && ((roll_x > -10 )&& (roll_x < 10))
- && (( pitch > -10 )&&(pitch < 10)) //L T R D
+ int seccon(float rooll ,float piitch, float yaaw){
+ if( ( ((yaaw > -50) && (yaaw < -30)) || ((yaaw > -145) && (yaaw < -125)) || ((yaaw > 140) && (yaaw < 160))||((yaaw > 0) && (yaaw < -20)) )
+ && ((rooll_x > -10 )&& (rooll_x < 10))
+ && (( piitch > -10 )&&(piitch < 10)) //L T R D
){
- ir = roll;
- ip = pitch;
- iy = yaw;
+ ir = rooll;
+ ip = piitch;
+ iy = yaaw;
return 1;
}
else{
@@ -111,9 +102,9 @@
int round(int rt_x, int rt_y, int rt_z){
- if( (pitch_y > -30) && (pitch_y < 30) ){ // vCC L leg D
+ if( (piitch_y > -30) && (piitch_y < 30) ){ // vCC L leg D
- if( ((yaw_z > 36) && (yaw_z < 113)) && ( ((roll_x > -40) && (roll_x < 40)) ) ){
+ if( ((yaaw_z > 36) && (yaaw_z < 113)) && ( ((rooll_x > -40) && (rooll_x < 40)) ) ){
//R x+
if(xlr == 1){
sata = 0;
@@ -139,8 +130,8 @@
sata = 90; //x
rt ='x';
}
- }
- if( ((yaw_z > -74) && (yaw_z < 35)) && ( ((roll_x > -40) && (roll_x < 40)) ) ){
+ }
+ if( ((yaaw_z > -74) && (yaaw_z < 35)) && ( ((rooll_x > -40) && (rooll_x < 40)) ) ){
//B y+
if(xlr == 1){
sata = 270;
@@ -167,7 +158,7 @@
rt = 'x';
}
}
- if( ((yaw_z > 110) && (yaw_z < 179)) &&( ((roll_x > -40) && (roll_x < 40)) ) ){
+ if( ((yaaw_z > 110) && (yaaw_z < 179)) &&( ((rooll_x > -40) && (rooll_x < 40)) ) ){
//F y-
if(xlr == 1){
sata = 90;
@@ -194,7 +185,7 @@
rt = 'x';
}
}
- if( ((yaw_z > -180) && (yaw_z < -75)) &&( ((roll_x > -40) && (roll_x < 40)) ) ){
+ if( ((yaaw_z > -180) && (yaaw_z < -75)) &&( ((rooll_x > -40) && (rooll_x < 40)) ) ){
//L x-
if(xlr == 1){
sata = 180;
@@ -221,7 +212,7 @@
rt = 'x';
}
}
- if((roll_x > 41) && (roll_x < 130) ){
+ if((rooll_x > 41) && (rooll_x < 130) ){
//T z+
if(xlr == 1){
sata = 90; //x
@@ -248,7 +239,7 @@
rt = 'x';
}
}
- if( (roll_x > -130) && (roll_x < -41)){
+ if( (rooll_x > -130) && (rooll_x < -41)){
//D z-
if(xlr == 1){
sata = 270;
@@ -277,29 +268,29 @@
}
}
-
+ int k1,k2,rtt_x,rtt_y,rtt_z;
+ k1 = sin(sata*PI/180);
+ k2 = cos(sata*PI/180);
rt_x = rt_x - 3;
rt_y = rt_y - 3;
rt_z = rt_z - 3;
if(rt == 'x'){
- rt_x = rt_x + 3; //x
- rt_y = (rt_y * cos(sata*(PI/180))+ rt_z * sin(sata*(PI/180)) )+ 3;
- rt_z = (-rt_y * sin(sata*(PI/180))+ rt_z * cos(sata*(PI/180)) ) + 3;
+ rtt_x = rt_x ; //x
+ rtt_y = ((rt_y * k2)- (rt_z * k1 ));
+ rtt_z = ((rt_y * k1)+ (rt_z * k2 ));
}
if(rt == 'y'){
- rt_x = (rt_x * cos(sata*(PI/180)) - rt_z * sin(sata*(PI/180)) )+ 3; // y
- rt_y = rt_y + 3;
- rt_z = (rt_x * sin(sata*(PI/180)) + rt_z * cos(sata*(PI/180)) )+ 3;
+ rtt_x = ((rt_x * k2) + (rt_z * k1 )); // y
+ rtt_y = rt_y ;
+ rtt_z = ((-rt_x * k1) + (rt_z * k2 ));
}
if(rt == 'z'){
- rt_x = (rt_x * cos(sata*(PI/180))+ rt_y * sin(sata*(PI/180)) )+ 3; //z
- rt_y = (rt_y * cos(sata*(PI/180))- rt_x * sin(sata*(PI/180)) )+ 3;
- rt_z = rt_z + 3;
+ rtt_x = ((rt_x * k2)- (rt_y * k1 )); //z
+ rtt_y = ((rt_x * k1)+ (rt_y * k2 ));
+ rtt_z = rt_z;
}
- pc.printf("rt_x %d\t", rt_x);
- pc.printf("rt_y %d\t", rt_y);
- pc.printf("rt_z %d \n\n\n\n\r", rt_z);
+
- return (rt_x*100 + rt_y*10 + rt_z);
+ return (rtt_x*100 + rtt_y*10 + rtt_z + 333);
}
\ No newline at end of file