ジャイロ
Dependents: 00_yotsuba 103_JY901_practice 200_yotsuba_21 200_yotuba_21_uiChange
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::endCalibrate() 00026 { 00027 char calibrationRegistar[3]= {CALSW,0x00,0x00}; 00028 write(IICADDR,calibrationRegistar,3,false); 00029 } 00030 00031 void JY901::yawcalibrate() 00032 { 00033 char calibrationRegistar[3]= {CALSW,0x04,0x00}; 00034 write(IICADDR,calibrationRegistar,3,false); 00035 } 00036 00037 void JY901::algorithmtrasition() 00038 { 00039 char calibrationRegistar[3]= {0x24,0x01,0x00}; 00040 write(IICADDR,calibrationRegistar,3,false); 00041 } 00042 00043 void JY901::calibrateAll(int time_ms) 00044 { 00045 calibrateGyroAccel(); 00046 thread_sleep_for(time_ms); 00047 calibrateMagnetic(); 00048 thread_sleep_for(time_ms); 00049 calibrateHeight(); 00050 thread_sleep_for(time_ms); 00051 endCalibrate(); 00052 thread_sleep_for(time_ms); 00053 algorithmtrasition(); 00054 thread_sleep_for(time_ms); 00055 yawcalibrate(); 00056 00057 } 00058 00059 float JY901::getXaxisAcceleration() 00060 { 00061 char *data = getdata(AX); 00062 return (float)((*(data+1) << 8) | *data) / 32768 * 16 * g; 00063 } 00064 00065 float JY901::getYaxisAcceleration() 00066 { 00067 char *data = getdata(AY); 00068 return (float)((*(data+1) << 8) | *data) / 32768 * 16 * g; 00069 } 00070 00071 float JY901::getZaxisAcceleration() 00072 { 00073 char *data = getdata(AZ); 00074 return (float)((*(data+1) << 8) | *data) / 32768 * 16 * g; 00075 } 00076 00077 float JY901::getXaxisAngularVelocity() 00078 { 00079 char *data = getdata(GX); 00080 return (float)((*(data+1) << 8 ) | *data) / 32768 * 2000; 00081 } 00082 00083 float JY901::getYaxisAngularVelocity() 00084 { 00085 char *data = getdata(GY); 00086 return (float)((*(data+1) << 8 ) | *data) / 32768 * 2000; 00087 } 00088 00089 float JY901::getZaxisAngularVelocity() 00090 { 00091 char *data = getdata(GZ); 00092 return (float)((*(data+1) << 8 ) | *data) / 32768 * 2000; 00093 } 00094 00095 float JY901::getXaxisMagnetic() 00096 { 00097 char *data = getdata(HX); 00098 return (float)((*(data+1) << 8) | *data); 00099 } 00100 00101 float JY901::getYaxisMagnetic() 00102 { 00103 char *data = getdata(HY); 00104 return (float)((*(data+1) << 8) | *data); 00105 } 00106 00107 float JY901::getZaxisMagnetic() 00108 { 00109 char *data = getdata(HZ); 00110 return (float)((*(data+1) << 8) | *data); 00111 } 00112 00113 float JY901::getXaxisAngle() 00114 { 00115 char *data = getdata(Roll); 00116 return (float)((*(data+1) << 8) | *data) / 32768 * 180; 00117 } 00118 00119 float JY901::getYaxisAngle() 00120 { 00121 char *data = getdata(Pitch); 00122 return (float)((*(data+1) << 8) | *data) / 32768 * 180; 00123 } 00124 00125 float JY901::getZaxisAngle() 00126 { 00127 char *data = getdata(Yaw); 00128 return (float)((*(data+1) << 8) | *data) / 32768 * 180; 00129 } 00130 00131 char *JY901::getdata(char registar) 00132 { 00133 char data[2] = {}; 00134 write(IICADDR,®istar,1,true); 00135 read(IICADDR,data,2,false); 00136 return data; 00137 }
Generated on Mon Aug 1 2022 23:18:09 by
1.7.2