Quadrifoglio / jy901

ジャイロ

Dependents:   00_yotsuba 103_JY901_practice 200_yotsuba_21 200_yotuba_21_uiChange

jy901.cpp@6:79d045ea3542, 2021-01-30 (annotated)

Committer:
piroro4560
Date:
Sat Jan 30 10:50:05 2021 +0000
Revision:
6:79d045ea3542
Parent:
5:2397765e5b08 
1/30/2021

Who changed what in which revision?

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