NagaokaRoboticsClub_mbedTeam / Mbed 2 deprecated 2021_NHK_A

2021 nhk A team

Dependencies:   mbed QEI　led beep softPWM Servo_softpwm IR2302 lpf

jy901/jy901.cpp@31:ce079259f559, 2021-10-31 (annotated)

Committer:
THtakahiro702286
Date:
Sun Oct 31 13:45:53 2021 +0000
Revision:
31:ce079259f559
Parent:
1:ba598e6ff285 
10/31 last parameter

Who changed what in which revision?

UserRevisionLine numberNew contents of line
THtakahiro702286 1:ba598e6ff285 1 #include "jy901.h"
THtakahiro702286 1:ba598e6ff285 2
THtakahiro702286 1:ba598e6ff285 3 JY901::JY901(PinName sda, PinName scl) : I2C(sda, scl)
THtakahiro702286 1:ba598e6ff285 4 {
THtakahiro702286 1:ba598e6ff285 5 }
THtakahiro702286 1:ba598e6ff285 6
THtakahiro702286 1:ba598e6ff285 7 void JY901::calibrateGyroAccel()
THtakahiro702286 1:ba598e6ff285 8 {
THtakahiro702286 1:ba598e6ff285 9 char calibrationRegistar[3]= {CALSW,0x01,0x00};
THtakahiro702286 1:ba598e6ff285 10 write(IICADDR,calibrationRegistar,3,false);
THtakahiro702286 1:ba598e6ff285 11 }
THtakahiro702286 1:ba598e6ff285 12
THtakahiro702286 1:ba598e6ff285 13 void JY901::calibrateMagnetic()
THtakahiro702286 1:ba598e6ff285 14 {
THtakahiro702286 1:ba598e6ff285 15 char calibrationRegistar[3]= {CALSW,0x02,0x00};
THtakahiro702286 1:ba598e6ff285 16 write(IICADDR,calibrationRegistar,3,false);
THtakahiro702286 1:ba598e6ff285 17 }
THtakahiro702286 1:ba598e6ff285 18
THtakahiro702286 1:ba598e6ff285 19 void JY901::calibrateHeight()
THtakahiro702286 1:ba598e6ff285 20 {
THtakahiro702286 1:ba598e6ff285 21 char calibrationRegistar[3]= {CALSW,0x03,0x00};
THtakahiro702286 1:ba598e6ff285 22 write(IICADDR,calibrationRegistar,3,false);
THtakahiro702286 1:ba598e6ff285 23 }
THtakahiro702286 1:ba598e6ff285 24
THtakahiro702286 1:ba598e6ff285 25 void JY901::yawcalibrate()
THtakahiro702286 1:ba598e6ff285 26 {
THtakahiro702286 1:ba598e6ff285 27 char calibrationRegistar[3]= {CALSW,0x04,0x00};
THtakahiro702286 1:ba598e6ff285 28 write(IICADDR,calibrationRegistar,3,false);
THtakahiro702286 1:ba598e6ff285 29 }
THtakahiro702286 1:ba598e6ff285 30
THtakahiro702286 1:ba598e6ff285 31 void JY901::algorithmtrasition()
THtakahiro702286 1:ba598e6ff285 32 {
THtakahiro702286 1:ba598e6ff285 33 char calibrationRegistar[3]= {0x24,0x01,0x00};
THtakahiro702286 1:ba598e6ff285 34 write(IICADDR,calibrationRegistar,3,false);
THtakahiro702286 1:ba598e6ff285 35 }
THtakahiro702286 1:ba598e6ff285 36
THtakahiro702286 1:ba598e6ff285 37 void JY901::endCalibrate()
THtakahiro702286 1:ba598e6ff285 38 {
THtakahiro702286 1:ba598e6ff285 39 char calibrationRegistar[3]= {CALSW,0x00,0x00};
THtakahiro702286 1:ba598e6ff285 40 write(IICADDR,calibrationRegistar,3,false);
THtakahiro702286 1:ba598e6ff285 41 }
THtakahiro702286 1:ba598e6ff285 42
THtakahiro702286 1:ba598e6ff285 43 void JY901::calibrateAll(int time)
THtakahiro702286 1:ba598e6ff285 44 {
THtakahiro702286 1:ba598e6ff285 45 calibrateGyroAccel();
THtakahiro702286 1:ba598e6ff285 46 wait_ms(time);
THtakahiro702286 1:ba598e6ff285 47 calibrateMagnetic();
THtakahiro702286 1:ba598e6ff285 48 wait_ms(time);
THtakahiro702286 1:ba598e6ff285 49 calibrateHeight();
THtakahiro702286 1:ba598e6ff285 50 wait_ms(time);
THtakahiro702286 1:ba598e6ff285 51 endCalibrate();
THtakahiro702286 1:ba598e6ff285 52 wait_ms(time);
THtakahiro702286 1:ba598e6ff285 53 algorithmtrasition();
THtakahiro702286 1:ba598e6ff285 54 wait_ms(time);
THtakahiro702286 1:ba598e6ff285 55 yawcalibrate();
THtakahiro702286 1:ba598e6ff285 56 }
THtakahiro702286 1:ba598e6ff285 57
THtakahiro702286 1:ba598e6ff285 58 float JY901::getXaxisAcceleration()
THtakahiro702286 1:ba598e6ff285 59 {
THtakahiro702286 1:ba598e6ff285 60 char *data = getdata(AX);
THtakahiro702286 1:ba598e6ff285 61 return (float)((*(data+1) << 8) | *data) / 32768 * 16 * g;
THtakahiro702286 1:ba598e6ff285 62 }
THtakahiro702286 1:ba598e6ff285 63
THtakahiro702286 1:ba598e6ff285 64 float JY901::getYaxisAcceleration()
THtakahiro702286 1:ba598e6ff285 65 {
THtakahiro702286 1:ba598e6ff285 66 char *data = getdata(AY);
THtakahiro702286 1:ba598e6ff285 67 return (float)((*(data+1) << 8) | *data) / 32768 * 16 * g;
THtakahiro702286 1:ba598e6ff285 68 }
THtakahiro702286 1:ba598e6ff285 69
THtakahiro702286 1:ba598e6ff285 70 float JY901::getZaxisAcceleration()
THtakahiro702286 1:ba598e6ff285 71 {
THtakahiro702286 1:ba598e6ff285 72 char *data = getdata(AZ);
THtakahiro702286 1:ba598e6ff285 73 return (float)((*(data+1) << 8) | *data) / 32768 * 16 * g;
THtakahiro702286 1:ba598e6ff285 74 }
THtakahiro702286 1:ba598e6ff285 75
THtakahiro702286 1:ba598e6ff285 76 float JY901::getXaxisAngularVelocity()
THtakahiro702286 1:ba598e6ff285 77 {
THtakahiro702286 1:ba598e6ff285 78 char *data = getdata(GX);
THtakahiro702286 1:ba598e6ff285 79 return (float)((*(data+1) << 8 ) | *data) / 32768 * 2000;
THtakahiro702286 1:ba598e6ff285 80 }
THtakahiro702286 1:ba598e6ff285 81
THtakahiro702286 1:ba598e6ff285 82 float JY901::getYaxisAngularVelocity()
THtakahiro702286 1:ba598e6ff285 83 {
THtakahiro702286 1:ba598e6ff285 84 char *data = getdata(GY);
THtakahiro702286 1:ba598e6ff285 85 return (float)((*(data+1) << 8 ) | *data) / 32768 * 2000;
THtakahiro702286 1:ba598e6ff285 86 }
THtakahiro702286 1:ba598e6ff285 87
THtakahiro702286 1:ba598e6ff285 88 float JY901::getZaxisAngularVelocity()
THtakahiro702286 1:ba598e6ff285 89 {
THtakahiro702286 1:ba598e6ff285 90 char *data = getdata(GZ);
THtakahiro702286 1:ba598e6ff285 91 return (float)((*(data+1) << 8 ) | *data) / 32768 * 2000;
THtakahiro702286 1:ba598e6ff285 92 }
THtakahiro702286 1:ba598e6ff285 93
THtakahiro702286 1:ba598e6ff285 94 float JY901::getXaxisMagnetic()
THtakahiro702286 1:ba598e6ff285 95 {
THtakahiro702286 1:ba598e6ff285 96 char *data = getdata(HX);
THtakahiro702286 1:ba598e6ff285 97 return (float)((*(data+1) << 8) | *data);
THtakahiro702286 1:ba598e6ff285 98 }
THtakahiro702286 1:ba598e6ff285 99
THtakahiro702286 1:ba598e6ff285 100 float JY901::getYaxisMagnetic()
THtakahiro702286 1:ba598e6ff285 101 {
THtakahiro702286 1:ba598e6ff285 102 char *data = getdata(HY);
THtakahiro702286 1:ba598e6ff285 103 return (float)((*(data+1) << 8) | *data);
THtakahiro702286 1:ba598e6ff285 104 }
THtakahiro702286 1:ba598e6ff285 105
THtakahiro702286 1:ba598e6ff285 106 float JY901::getZaxisMagnetic()
THtakahiro702286 1:ba598e6ff285 107 {
THtakahiro702286 1:ba598e6ff285 108 char *data = getdata(HZ);
THtakahiro702286 1:ba598e6ff285 109 return (float)((*(data+1) << 8) | *data);
THtakahiro702286 1:ba598e6ff285 110 }
THtakahiro702286 1:ba598e6ff285 111
THtakahiro702286 1:ba598e6ff285 112 float JY901::getXaxisAngle()
THtakahiro702286 1:ba598e6ff285 113 {
THtakahiro702286 1:ba598e6ff285 114 char *data = getdata(Roll);
THtakahiro702286 1:ba598e6ff285 115 return (float)((*(data+1) << 8) | *data) / 32768 * 180;
THtakahiro702286 1:ba598e6ff285 116 }
THtakahiro702286 1:ba598e6ff285 117
THtakahiro702286 1:ba598e6ff285 118 float JY901::getYaxisAngle()
THtakahiro702286 1:ba598e6ff285 119 {
THtakahiro702286 1:ba598e6ff285 120 char *data = getdata(Pitch);
THtakahiro702286 1:ba598e6ff285 121 return (float)((*(data+1) << 8) | *data) / 32768 * 180;
THtakahiro702286 1:ba598e6ff285 122 }
THtakahiro702286 1:ba598e6ff285 123
THtakahiro702286 1:ba598e6ff285 124 float JY901::getZaxisAngle()
THtakahiro702286 1:ba598e6ff285 125 {
THtakahiro702286 1:ba598e6ff285 126 char *data = getdata(Yaw);
THtakahiro702286 1:ba598e6ff285 127 return (float)((*(data+1) << 8) | *data) / 32768 * 180;
THtakahiro702286 1:ba598e6ff285 128 }
THtakahiro702286 1:ba598e6ff285 129
THtakahiro702286 1:ba598e6ff285 130
THtakahiro702286 1:ba598e6ff285 131
THtakahiro702286 1:ba598e6ff285 132 char *JY901::getdata(char registar)
THtakahiro702286 1:ba598e6ff285 133 {
THtakahiro702286 1:ba598e6ff285 134 char data[2] = {};
THtakahiro702286 1:ba598e6ff285 135 write(IICADDR,&registar,1,true);
THtakahiro702286 1:ba598e6ff285 136 read(IICADDR,data,2,false);
THtakahiro702286 1:ba598e6ff285 137 return data;
THtakahiro702286 1:ba598e6ff285 138 }