2021 nhk A team
Dependencies: mbed QEI led beep softPWM Servo_softpwm IR2302 lpf
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,®istar,1,true); 00136 read(IICADDR,data,2,false); 00137 return data; 00138 }
Generated on Thu Jul 21 2022 18:57:01 by
1.7.2