2021 nhk A team

Dependencies:   mbed QEI led beep softPWM Servo_softpwm IR2302 lpf

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers jy901.cpp Source File

jy901.cpp

00001 #include "jy901.h"
00002 
00003 JY901::JY901 (PinName sda, PinName scl) : I2C(sda, scl)
00004 {
00005 }
00006 
00007 void JY901::calibrateGyroAccel()
00008 {
00009     char calibrationRegistar[3]= {CALSW,0x01,0x00};
00010     write(IICADDR,calibrationRegistar,3,false);
00011 }
00012 
00013 void JY901::calibrateMagnetic()
00014 {
00015     char calibrationRegistar[3]= {CALSW,0x02,0x00};
00016     write(IICADDR,calibrationRegistar,3,false);
00017 }
00018 
00019 void JY901::calibrateHeight()
00020 {
00021     char calibrationRegistar[3]= {CALSW,0x03,0x00};
00022     write(IICADDR,calibrationRegistar,3,false);
00023 }
00024 
00025 void JY901::yawcalibrate()
00026 {
00027     char calibrationRegistar[3]= {CALSW,0x04,0x00};
00028     write(IICADDR,calibrationRegistar,3,false);
00029 }
00030 
00031 void JY901::algorithmtrasition()
00032 {
00033     char calibrationRegistar[3]= {0x24,0x01,0x00};
00034     write(IICADDR,calibrationRegistar,3,false);
00035 }
00036 
00037 void JY901::endCalibrate()
00038 {
00039     char calibrationRegistar[3]= {CALSW,0x00,0x00};
00040     write(IICADDR,calibrationRegistar,3,false);
00041 }
00042 
00043 void JY901::calibrateAll(int time)
00044 {
00045     calibrateGyroAccel();
00046     wait_ms(time);
00047     calibrateMagnetic();
00048     wait_ms(time);
00049     calibrateHeight();
00050     wait_ms(time);
00051     endCalibrate();
00052     wait_ms(time);
00053     algorithmtrasition();
00054     wait_ms(time);
00055     yawcalibrate();
00056 }
00057 
00058 float JY901::getXaxisAcceleration()
00059 {
00060     char *data = getdata(AX);
00061     return (float)((*(data+1) << 8) | *data) / 32768 * 16 * g;
00062 }
00063 
00064 float JY901::getYaxisAcceleration()
00065 {
00066     char *data = getdata(AY);
00067     return (float)((*(data+1) << 8) | *data) / 32768 * 16 * g;
00068 }
00069 
00070 float JY901::getZaxisAcceleration()
00071 {
00072     char *data = getdata(AZ);
00073     return (float)((*(data+1) << 8) | *data) / 32768 * 16 * g;
00074 }
00075 
00076 float JY901::getXaxisAngularVelocity()
00077 {
00078     char *data = getdata(GX);
00079     return (float)((*(data+1) << 8 ) | *data) / 32768 * 2000;
00080 }
00081 
00082 float JY901::getYaxisAngularVelocity()
00083 {
00084     char *data = getdata(GY);
00085     return (float)((*(data+1) << 8 ) | *data) / 32768 * 2000;
00086 }
00087 
00088 float JY901::getZaxisAngularVelocity()
00089 {
00090     char *data = getdata(GZ);
00091     return (float)((*(data+1) << 8 ) | *data) / 32768 * 2000;
00092 }
00093 
00094 float JY901::getXaxisMagnetic()
00095 {
00096     char *data = getdata(HX);
00097     return (float)((*(data+1) << 8) | *data);
00098 }
00099 
00100 float JY901::getYaxisMagnetic()
00101 {
00102     char *data = getdata(HY);
00103     return (float)((*(data+1) << 8) | *data);
00104 }
00105 
00106 float JY901::getZaxisMagnetic()
00107 {
00108     char *data = getdata(HZ);
00109     return (float)((*(data+1) << 8) | *data);
00110 }
00111 
00112 float JY901::getXaxisAngle()
00113 {
00114     char *data = getdata(Roll);
00115     return (float)((*(data+1) << 8) | *data) / 32768 * 180;
00116 }
00117 
00118 float JY901::getYaxisAngle()
00119 {
00120     char *data = getdata(Pitch);
00121     return (float)((*(data+1) << 8) | *data) / 32768 * 180;
00122 }
00123 
00124 float JY901::getZaxisAngle()
00125 {
00126     char *data = getdata(Yaw);
00127     return (float)((*(data+1) << 8) | *data) / 32768 * 180;
00128 }
00129 
00130 
00131 
00132 char *JY901::getdata(char registar)
00133 {
00134     char data[2] = {};
00135     write(IICADDR,&registar,1,true);
00136     read(IICADDR,data,2,false);
00137     return data;
00138 }