Chawisorn samrit
kor bork wa cop koy ma
Fork of
testIMU2_copy2
by 
OX
Diff: main.cpp
- Revision:
- 3:ee0df78b0dd3
- Parent:
- 2:6bc2c5d68446
--- a/main.cpp Sun Dec 04 09:19:53 2016 +0000
+++ b/main.cpp Thu Dec 07 08:04:06 2017 +0000
@@ -2,295 +2,19 @@
#include "zmu9250.h"
#include "math.h"
-ZMU9250 a;
+ZMU9250 axis_Gen;
Serial pc(D1, D0);
-float yaaw_z, piitch_y, rooll_x;
-int ir,ip,iy;
-int l;
-int xlr,yfb,ztd;
-int sata;
-char rt;
-
-int cube1(int x, int y, int z);
-int seccon(float rooll ,float piitch, float yaaw);
-int round(int rt_x, int rt_y, int rt_z);
-
-
-DigitalIn op_rooll(USER_BUTTON);
-
-
+float axis_z, axis_y, axis_x;
int main(){
while(1){
wait(0.1);
- a.Update();
- yaaw_z = a.Yaw();
- piitch_y = a.Pitch();
- rooll_x = a.Roll();
-
- // 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 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(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);
- }
-
-
+ axis_Gen.Update();
+ axis_z = axis_Gen.Yaw();
+ axis_y = axis_Gen.Pitch();
+ axis_x = axis_Gen.Roll();
+ pc.printf(" axis_x %f\t axis_y %f\t axis_z %f\n\n\n\r", axis_x, axis_y, axis_z);
}
- }
-
- 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 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 = rooll;
- ip = piitch;
- iy = yaaw;
- return 1;
- }
- else{
- return 0;
- }
- }
-
-
- int round(int rt_x, int rt_y, int rt_z){
-
- if( (piitch_y > -30) && (piitch_y < 30) ){ // vCC L leg D
-
- if( ((yaaw_z > 36) && (yaaw_z < 113)) && ( ((rooll_x > -40) && (rooll_x < 40)) ) ){
- //R x+
- if(xlr == 1){
- sata = 0;
- rt = 'z';
- }
- else if(yfb == -1){
- sata = 90;
- rt = 'z';
- }
- else if(yfb == 1){
- sata = 270;
- rt = 'z';
- }
- else if(xlr == -1){
- sata = 180;
- rt = 'z';
- }
- else if(ztd == 1){
- sata = 270; //x
- rt = 'x';
- }
- else if(ztd == -1){
- sata = 90; //x
- rt ='x';
- }
- }
- if( ((yaaw_z > -74) && (yaaw_z < 35)) && ( ((rooll_x > -40) && (rooll_x < 40)) ) ){
- //B y+
- if(xlr == 1){
- sata = 270;
- rt = 'z';
- }
- else if(yfb == -1){
- sata = 0;
- rt = 'z';
- }
- else if(yfb == 1){
- sata = 180;
- rt = 'z';
- }
- else if(xlr == -1){
- sata = 90;
- rt = 'z';
- }
- else if(ztd == 1){
- sata = 270; //x
- rt = 'x';
- }
- else if(ztd == -1){
- sata = 90; //x
- rt = 'x';
- }
- }
- if( ((yaaw_z > 110) && (yaaw_z < 179)) &&( ((rooll_x > -40) && (rooll_x < 40)) ) ){
- //F y-
- if(xlr == 1){
- sata = 90;
- rt = 'z';
- }
- else if(yfb == -1){
- sata = 180;
- rt = 'z';
- }
- else if(yfb == 1){
- sata = 0;
- rt = 'z';
- }
- else if(xlr == -1){
- sata = 270;
- rt = 'z';
- }
- else if(ztd == 1){
- sata = 90;
- rt = 'x';
- }
- else if(ztd == -1){
- sata = 270;
- rt = 'x';
- }
- }
- if( ((yaaw_z > -180) && (yaaw_z < -75)) &&( ((rooll_x > -40) && (rooll_x < 40)) ) ){
- //L x-
- if(xlr == 1){
- sata = 180;
- rt = 'z';
- }
- else if(yfb == -1){
- sata = 270;
- rt = 'z';
- }
- else if(yfb == 1){
- sata = 90;
- rt = 'z';
- }
- else if(xlr == -1){
- sata = 0;
- rt = 'z';
- }
- else if(ztd == 1){
- sata = 90;
- rt = 'x';
- }
- else if(ztd == -1){
- sata = 270;
- rt = 'x';
- }
- }
- if((rooll_x > 41) && (rooll_x < 130) ){
- //T z+
- if(xlr == 1){
- sata = 90; //x
- rt = 'x';
- }
- else if(yfb == -1){
- sata = 90; //y
- rt = 'y';
- }
- else if(yfb == 1){
- sata = 270;
- rt = 'y';
- }
- else if(xlr == -1){
- sata = 270;
- rt = 'x';
- }
- else if(ztd == 1){
- sata = 0;
- rt = 'x';
- }
- else if(ztd == -1){
- sata = 180; //x
- rt = 'x';
- }
- }
- if( (rooll_x > -130) && (rooll_x < -41)){
- //D z-
- if(xlr == 1){
- sata = 270;
- rt = 'x';
- }
- else if(yfb == -1){
- sata = 270;
- rt = 'y';
- }
- else if(yfb == 1){
- sata = 90;
- rt = 'y';
- }
- else if(xlr == -1){
- sata = 90;
- rt = 'x';
- }
- else if(ztd == 1){
- sata = 180;
- rt = 'x';
- }
- else if(ztd == -1){
- sata = 0 ; //x
- rt = 'x';
- }
-
- }
- }
- 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'){
- rtt_x = rt_x ; //x
- rtt_y = ((rt_y * k2)- (rt_z * k1 ));
- rtt_z = ((rt_y * k1)+ (rt_z * k2 ));
- }
- if(rt == 'y'){
- rtt_x = ((rt_x * k2) + (rt_z * k1 )); // y
- rtt_y = rt_y ;
- rtt_z = ((-rt_x * k1) + (rt_z * k2 ));
- }
- if(rt == 'z'){
- rtt_x = ((rt_x * k2)- (rt_y * k1 )); //z
- rtt_y = ((rt_x * k1)+ (rt_y * k2 ));
- rtt_z = rt_z;
- }
-
-
- return (rtt_x*100 + rtt_y*10 + rtt_z + 333);
- }
+}
\ No newline at end of file