Chawisorn samrit
kor bork wa cop koy ma
Fork of
testIMU2_copy2
by 
OX
Diff: main.cpp
- Revision:
- 0:77a7d1a1c6db
- Child:
- 1:cc3c774c218a
- Child:
- 2:6bc2c5d68446
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sat Dec 03 18:21:47 2016 +0000
@@ -0,0 +1,305 @@
+#include "mbed.h"
+#include "zmu9250.h"
+#include "math.h"
+
+ZMU9250 a;
+Serial pc(D1, D0);
+
+float yaw_z, pitch_y, roll_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 roll ,float pitch, float yaw);
+int round(int rt_x, int rt_y, int rt_z);
+
+
+DigitalIn op_roll(USER_BUTTON);
+
+
+int main(){
+
+ while(1){
+ wait(0.1);
+ a.Update();
+ yaw_z = a.Yaw();
+ pitch_y = a.Pitch();
+ roll_x = a.Roll();
+
+// pc.printf("Yaw_z %f\t Pitch_y %f\t Roll_x %f\n", yaw_z, pitch_y, roll_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\n" ,seccon(roll_x, pitch_y, yaw_z));
+// pc.printf("========\n"); //it
+ if(op_roll == 0){
+
+ l = cube1(3,3,3);
+ }
+
+
+ }
+ }
+
+ 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;
+
+ }
+
+ }
+
+
+
+ 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
+ ){
+ ir = roll;
+ ip = pitch;
+ iy = yaw;
+ return 1;
+ }
+ else{
+ return 0;
+ }
+ }
+
+
+ int round(int rt_x, int rt_y, int rt_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)) ) ){
+ //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( ((yaw_z > -74) && (yaw_z < 35)) && ( ((roll_x > -40) && (roll_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( ((yaw_z > 110) && (yaw_z < 179)) &&( ((roll_x > -40) && (roll_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( ((yaw_z > -180) && (yaw_z < -75)) &&( ((roll_x > -40) && (roll_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((roll_x > 41) && (roll_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( (roll_x > -130) && (roll_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';
+ }
+
+ }
+ }
+
+ 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;
+ }
+ 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;
+ }
+ 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;
+ }
+ 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);
+ }
+
\ No newline at end of file