Zhilin He / Mbed 2 deprecated TDPS-COM1

UNFINISHED

Dependencies:   HCSR04 SRF05 mbed

gyro.cpp@1:60c79e942c98, 2018-06-08 (annotated)

Committer:
Charlie_He
Date:
Fri Jun 08 22:22:10 2018 +0000
Revision:
1:60c79e942c98
Parent:
0:4ca3e247b86a 
TDPS; ; ;

Who changed what in which revision?

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