ジャイロ

Dependents:   00_yotsuba 103_JY901_practice 200_yotsuba_21 200_yotuba_21_uiChange

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::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,&registar,1,true);
00135     read(IICADDR,data,2,false);
00136     return data;
00137 }