Zhilin He / Mbed 2 deprecated mbed_JY61

The finished code for JY61---MPU6050

Dependencies:   mbed

gyro.cpp@0:68d5307f4eba, 2018-06-02 (annotated)

Committer:
Charlie_He
Date:
Sat Jun 02 08:18:12 2018 +0000
Revision:
0:68d5307f4eba
The Code for JY61---MPU6050

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Charlie_He 0:68d5307f4eba 1 #include "gyro.h"
Charlie_He 0:68d5307f4eba 2
Charlie_He 0:68d5307f4eba 3 #define IMU_TX_PINMAP p28
Charlie_He 0:68d5307f4eba 4 #define IMU_RX_PINMAP p27
Charlie_He 0:68d5307f4eba 5 Serial device(IMU_TX_PINMAP, IMU_RX_PINMAP); // tx, rx
Charlie_He 0:68d5307f4eba 6 Serial pc(USBTX,USBRX);
Charlie_He 0:68d5307f4eba 7 unsigned char Re_buf[11],counter=0;
Charlie_He 0:68d5307f4eba 8 unsigned char sign=0;
Charlie_He 0:68d5307f4eba 9 float a[3],w[3],angle[3],T;
Charlie_He 0:68d5307f4eba 10 float yaw = 0;
Charlie_He 0:68d5307f4eba 11 void output();
Charlie_He 0:68d5307f4eba 12 void deviceEvent() {
Charlie_He 0:68d5307f4eba 13 Re_buf[counter]=(unsigned char)device.getc();
Charlie_He 0:68d5307f4eba 14 if(counter==0&&Re_buf[0]!=0x55) return; //第0号数据不是帧头
Charlie_He 0:68d5307f4eba 15 counter++;
Charlie_He 0:68d5307f4eba 16 if(counter==11) //接收到11个数据
Charlie_He 0:68d5307f4eba 17 {
Charlie_He 0:68d5307f4eba 18 counter=0; //重新赋值，准备下一帧数据的接收
Charlie_He 0:68d5307f4eba 19 sign=1;
Charlie_He 0:68d5307f4eba 20 }
Charlie_He 0:68d5307f4eba 21 output();
Charlie_He 0:68d5307f4eba 22 }
Charlie_He 0:68d5307f4eba 23
Charlie_He 0:68d5307f4eba 24 void output()
Charlie_He 0:68d5307f4eba 25 {
Charlie_He 0:68d5307f4eba 26 if(sign)
Charlie_He 0:68d5307f4eba 27 {
Charlie_He 0:68d5307f4eba 28 sign=0;
Charlie_He 0:68d5307f4eba 29 if(Re_buf[0]==0x55) //检查帧头
Charlie_He 0:68d5307f4eba 30 {
Charlie_He 0:68d5307f4eba 31 for (int i=0;i<10;i++)
Charlie_He 0:68d5307f4eba 32 /*
Charlie_He 0:68d5307f4eba 33 {pc.printf("%d",Re_buf[i]);}
Charlie_He 0:68d5307f4eba 34 pc.printf("\n"}*/
Charlie_He 0:68d5307f4eba 35 switch(Re_buf [1])
Charlie_He 0:68d5307f4eba 36 {
Charlie_He 0:68d5307f4eba 37 static float lastYaw = 0;
Charlie_He 0:68d5307f4eba 38 static float turnNum = 0;
Charlie_He 0:68d5307f4eba 39 case 0x51:
Charlie_He 0:68d5307f4eba 40 a[0] = (short(Re_buf [3]<<8| Re_buf [2]))/32768.0*16;
Charlie_He 0:68d5307f4eba 41 a[1] = (short(Re_buf [5]<<8| Re_buf [4]))/32768.0*16;
Charlie_He 0:68d5307f4eba 42 a[2] = (short(Re_buf [7]<<8| Re_buf [6]))/32768.0*16;
Charlie_He 0:68d5307f4eba 43 T = (short(Re_buf [9]<<8| Re_buf [8]))/340.0+36.25;
Charlie_He 0:68d5307f4eba 44 break;
Charlie_He 0:68d5307f4eba 45 case 0x52:
Charlie_He 0:68d5307f4eba 46 w[0] = (short(Re_buf [3]<<8| Re_buf [2]))/32768.0*2000;
Charlie_He 0:68d5307f4eba 47 w[1] = (short(Re_buf [5]<<8| Re_buf [4]))/32768.0*2000;
Charlie_He 0:68d5307f4eba 48 w[2] = (short(Re_buf [7]<<8| Re_buf [6]))/32768.0*2000;
Charlie_He 0:68d5307f4eba 49 T = (short(Re_buf [9]<<8| Re_buf [8]))/340.0+36.25;
Charlie_He 0:68d5307f4eba 50 break;
Charlie_He 0:68d5307f4eba 51 case 0x53:
Charlie_He 0:68d5307f4eba 52 double yaw_;
Charlie_He 0:68d5307f4eba 53 yaw_ = Re_buf[7] << 8 | Re_buf[6];
Charlie_He 0:68d5307f4eba 54 yaw_ = yaw_ / 32768.0 * 180;
Charlie_He 0:68d5307f4eba 55 if(lastYaw < 90 && yaw_ > 270){turnNum -= 1;}
Charlie_He 0:68d5307f4eba 56 else if(lastYaw > 270 && yaw_ < 90){turnNum += 1;}
Charlie_He 0:68d5307f4eba 57 lastYaw = yaw_;
Charlie_He 0:68d5307f4eba 58 yaw = yaw_ + 360*turnNum;
Charlie_He 0:68d5307f4eba 59 pc.printf(" %8.2f\n", yaw_+360*turnNum);
Charlie_He 0:68d5307f4eba 60 angle[0] = (short(Re_buf [3]<<8| Re_buf [2]))/32768.0*180;
Charlie_He 0:68d5307f4eba 61 angle[1] = (short(Re_buf [5]<<8| Re_buf [4]))/32768.0*180;
Charlie_He 0:68d5307f4eba 62 angle[2] = (short(Re_buf [7]<<8| Re_buf [6]))/32768.0*180;
Charlie_He 0:68d5307f4eba 63 T = (short(Re_buf [9]<<8| Re_buf [8]))/340.0+36.25;
Charlie_He 0:68d5307f4eba 64 /* pc.printf("a:");
Charlie_He 0:68d5307f4eba 65 pc.printf("%d",a[0]);pc.printf(" ");
Charlie_He 0:68d5307f4eba 66 pc.printf("%d",a[1]);pc.printf(" ");
Charlie_He 0:68d5307f4eba 67 pc.printf("%d\n",a[2]);pc.printf(" ");
Charlie_He 0:68d5307f4eba 68 pc.printf("w:");
Charlie_He 0:68d5307f4eba 69 pc.printf("%d",w[0]);pc.printf(" ");
Charlie_He 0:68d5307f4eba 70 pc.printf("%d",w[1]);pc.printf(" ");
Charlie_He 0:68d5307f4eba 71 pc.printf("%d\n",w[2]);pc.printf(" ");
Charlie_He 0:68d5307f4eba 72 pc.printf("angle:");
Charlie_He 0:68d5307f4eba 73 pc.printf("%d",angle[0]);pc.printf(" ");
Charlie_He 0:68d5307f4eba 74 pc.printf("%d",angle[1]);pc.printf(" ");
Charlie_He 0:68d5307f4eba 75 pc.printf("%d\n",angle[2]);pc.printf(" ");
Charlie_He 0:68d5307f4eba 76 pc.printf("T:");*/
Charlie_He 0:68d5307f4eba 77 break;
Charlie_He 0:68d5307f4eba 78 }
Charlie_He 0:68d5307f4eba 79 }
Charlie_He 0:68d5307f4eba 80 }
Charlie_He 0:68d5307f4eba 81 }
Charlie_He 0:68d5307f4eba 82
Charlie_He 0:68d5307f4eba 83 void init_gyro(){
Charlie_He 0:68d5307f4eba 84 device.baud(115200);
Charlie_He 0:68d5307f4eba 85 device.attach(&deviceEvent, device.RxIrq);
Charlie_He 0:68d5307f4eba 86 }