miki sumito / Mbed 2 deprecated four_wheel_omni

四輪オムニの足回りを動かすプログラム

Dependencies:   mbed

BNO055/BNO055.cpp@1:cb4a6d5c77c8, 22 months ago (annotated)

Committer:
m_smt
Date:
Sat Oct 08 11:14:39 2022 +0000
Revision:
1:cb4a6d5c77c8
Parent:
0:39c2bb18192b 
move four wheel omni

Who changed what in which revision?

UserRevisionLine numberNew contents of line
m_smt 0:39c2bb18192b 1 #include "BNO055.hpp"
m_smt 0:39c2bb18192b 2 #include "mbed.h"
m_smt 0:39c2bb18192b 3
m_smt 0:39c2bb18192b 4 BNO055::BNO055(PinName SDA, PinName SCL) : _i2c(SDA,SCL){
m_smt 0:39c2bb18192b 5 //Set I2C fast and bring reset line high
m_smt 0:39c2bb18192b 6 _i2c.frequency(50000);
m_smt 0:39c2bb18192b 7 address = BNOAddress;
m_smt 0:39c2bb18192b 8 accel_scale = 0.001f;
m_smt 0:39c2bb18192b 9 rate_scale = 1.0f/16.0f;
m_smt 0:39c2bb18192b 10 angle_scale = 1.0f/16.0f;
m_smt 0:39c2bb18192b 11 temp_scale = 1;
m_smt 0:39c2bb18192b 12 }
m_smt 0:39c2bb18192b 13
m_smt 0:39c2bb18192b 14 void BNO055::reset(){
m_smt 0:39c2bb18192b 15 //Perform a power-on-reset
m_smt 0:39c2bb18192b 16 readchar(BNO055_SYS_TRIGGER_ADDR);
m_smt 0:39c2bb18192b 17 rx = rx | 0x20;
m_smt 0:39c2bb18192b 18 writechar(BNO055_SYS_TRIGGER_ADDR,rx);
m_smt 0:39c2bb18192b 19 //Wait for the system to come back up again (datasheet says 650ms)
m_smt 0:39c2bb18192b 20 wait_ms(675);
m_smt 0:39c2bb18192b 21 }
m_smt 0:39c2bb18192b 22
m_smt 0:39c2bb18192b 23 bool BNO055::check(){
m_smt 0:39c2bb18192b 24 //Check we have communication link with the chip
m_smt 0:39c2bb18192b 25 readchar(BNO055_CHIP_ID_ADDR);
m_smt 0:39c2bb18192b 26 if (rx != 0xA0) return false;
m_smt 0:39c2bb18192b 27 //Grab the chip ID and software versions
m_smt 0:39c2bb18192b 28 tx[0] = BNO055_CHIP_ID_ADDR;
m_smt 0:39c2bb18192b 29 _i2c.write(address,tx,1,true);
m_smt 0:39c2bb18192b 30 _i2c.read(address+1,rawdata,7,false);
m_smt 0:39c2bb18192b 31 ID.id = rawdata[0];
m_smt 0:39c2bb18192b 32 ID.accel = rawdata[1];
m_smt 0:39c2bb18192b 33 ID.mag = rawdata[2];
m_smt 0:39c2bb18192b 34 ID.gyro = rawdata[3];
m_smt 0:39c2bb18192b 35 ID.sw[0] = rawdata[4];
m_smt 0:39c2bb18192b 36 ID.sw[1] = rawdata[5];
m_smt 0:39c2bb18192b 37 ID.bootload = rawdata[6];
m_smt 0:39c2bb18192b 38 setpage(1);
m_smt 0:39c2bb18192b 39 tx[0] = BNO055_UNIQUE_ID_ADDR;
m_smt 0:39c2bb18192b 40 _i2c.write(address,tx,1,true);
m_smt 0:39c2bb18192b 41 _i2c.read(address+1,ID.serial,16,false);
m_smt 0:39c2bb18192b 42 setpage(0);
m_smt 0:39c2bb18192b 43 return true;
m_smt 0:39c2bb18192b 44 }
m_smt 0:39c2bb18192b 45
m_smt 0:39c2bb18192b 46 void BNO055::SetExternalCrystal(bool yn){
m_smt 0:39c2bb18192b 47 // Read the current status from the device
m_smt 0:39c2bb18192b 48 readchar(BNO055_SYS_TRIGGER_ADDR);
m_smt 0:39c2bb18192b 49 if (yn) rx = rx | 0x80;
m_smt 0:39c2bb18192b 50 else rx = rx & 0x7F;
m_smt 0:39c2bb18192b 51 writechar(BNO055_SYS_TRIGGER_ADDR,rx);
m_smt 0:39c2bb18192b 52 }
m_smt 0:39c2bb18192b 53
m_smt 0:39c2bb18192b 54 void BNO055::set_accel_units(char units){
m_smt 0:39c2bb18192b 55 readchar(BNO055_UNIT_SEL_ADDR);
m_smt 0:39c2bb18192b 56 if(units == MPERSPERS){
m_smt 0:39c2bb18192b 57 rx = rx & 0xFE;
m_smt 0:39c2bb18192b 58 accel_scale = 0.01f;
m_smt 0:39c2bb18192b 59 }
m_smt 0:39c2bb18192b 60 else {
m_smt 0:39c2bb18192b 61 rx = rx | units;
m_smt 0:39c2bb18192b 62 accel_scale = 0.001f;
m_smt 0:39c2bb18192b 63 }
m_smt 0:39c2bb18192b 64 writechar(BNO055_UNIT_SEL_ADDR,rx);
m_smt 0:39c2bb18192b 65 }
m_smt 0:39c2bb18192b 66
m_smt 0:39c2bb18192b 67 void BNO055::set_anglerate_units(char units){
m_smt 0:39c2bb18192b 68 readchar(BNO055_UNIT_SEL_ADDR);
m_smt 0:39c2bb18192b 69 if (units == DEG_PER_SEC){
m_smt 0:39c2bb18192b 70 rx = rx & 0xFD;
m_smt 0:39c2bb18192b 71 rate_scale = 1.0f/16.0f;
m_smt 0:39c2bb18192b 72 }
m_smt 0:39c2bb18192b 73 else {
m_smt 0:39c2bb18192b 74 rx = rx | units;
m_smt 0:39c2bb18192b 75 rate_scale = 1.0f/900.0f;
m_smt 0:39c2bb18192b 76 }
m_smt 0:39c2bb18192b 77 writechar(BNO055_UNIT_SEL_ADDR,rx);
m_smt 0:39c2bb18192b 78 }
m_smt 0:39c2bb18192b 79
m_smt 0:39c2bb18192b 80 void BNO055::set_angle_units(char units){
m_smt 0:39c2bb18192b 81 readchar(BNO055_UNIT_SEL_ADDR);
m_smt 0:39c2bb18192b 82 if (units == DEGREES){
m_smt 0:39c2bb18192b 83 rx = rx & 0xFB;
m_smt 0:39c2bb18192b 84 angle_scale = 1.0f/16.0f;
m_smt 0:39c2bb18192b 85 }
m_smt 0:39c2bb18192b 86 else {
m_smt 0:39c2bb18192b 87 rx = rx | units;
m_smt 0:39c2bb18192b 88 rate_scale = 1.0f/900.0f;
m_smt 0:39c2bb18192b 89 }
m_smt 0:39c2bb18192b 90 writechar(BNO055_UNIT_SEL_ADDR,rx);
m_smt 0:39c2bb18192b 91 }
m_smt 0:39c2bb18192b 92
m_smt 0:39c2bb18192b 93 void BNO055::set_temp_units(char units){
m_smt 0:39c2bb18192b 94 readchar(BNO055_UNIT_SEL_ADDR);
m_smt 0:39c2bb18192b 95 if (units == CENTIGRADE){
m_smt 0:39c2bb18192b 96 rx = rx & 0xEF;
m_smt 0:39c2bb18192b 97 temp_scale = 1;
m_smt 0:39c2bb18192b 98 }
m_smt 0:39c2bb18192b 99 else {
m_smt 0:39c2bb18192b 100 rx = rx | units;
m_smt 0:39c2bb18192b 101 temp_scale = 2;
m_smt 0:39c2bb18192b 102 }
m_smt 0:39c2bb18192b 103 writechar(BNO055_UNIT_SEL_ADDR,rx);
m_smt 0:39c2bb18192b 104 }
m_smt 0:39c2bb18192b 105
m_smt 0:39c2bb18192b 106 void BNO055::set_orientation(char units){
m_smt 0:39c2bb18192b 107 readchar(BNO055_UNIT_SEL_ADDR);
m_smt 0:39c2bb18192b 108 if (units == WINDOWS) rx = rx &0x7F;
m_smt 0:39c2bb18192b 109 else rx = rx | units;
m_smt 0:39c2bb18192b 110 writechar(BNO055_UNIT_SEL_ADDR,rx);
m_smt 0:39c2bb18192b 111 }
m_smt 0:39c2bb18192b 112
m_smt 0:39c2bb18192b 113 void BNO055::setmode(char omode){
m_smt 0:39c2bb18192b 114 writechar(BNO055_OPR_MODE_ADDR,omode);
m_smt 0:39c2bb18192b 115 op_mode = omode;
m_smt 0:39c2bb18192b 116 }
m_smt 0:39c2bb18192b 117
m_smt 0:39c2bb18192b 118 void BNO055::setpowermode(char pmode){
m_smt 0:39c2bb18192b 119 writechar(BNO055_PWR_MODE_ADDR,pmode);
m_smt 0:39c2bb18192b 120 pwr_mode = pmode;
m_smt 0:39c2bb18192b 121 }
m_smt 0:39c2bb18192b 122
m_smt 0:39c2bb18192b 123 void BNO055::get_accel(void){
m_smt 0:39c2bb18192b 124 tx[0] = BNO055_ACCEL_DATA_X_LSB_ADDR;
m_smt 0:39c2bb18192b 125 _i2c.write(address,tx,1,true);
m_smt 0:39c2bb18192b 126 _i2c.read(address+1,rawdata,6,0);
m_smt 0:39c2bb18192b 127 accel.rawx = (rawdata[1] << 8 | rawdata[0]);
m_smt 0:39c2bb18192b 128 accel.rawy = (rawdata[3] << 8 | rawdata[2]);
m_smt 0:39c2bb18192b 129 accel.rawz = (rawdata[5] << 8 | rawdata[4]);
m_smt 0:39c2bb18192b 130 accel.x = float(accel.rawx)*accel_scale;
m_smt 0:39c2bb18192b 131 accel.y = float(accel.rawy)*accel_scale;
m_smt 0:39c2bb18192b 132 accel.z = float(accel.rawz)*accel_scale;
m_smt 0:39c2bb18192b 133 }
m_smt 0:39c2bb18192b 134
m_smt 0:39c2bb18192b 135 void BNO055::get_gyro(void){
m_smt 0:39c2bb18192b 136 tx[0] = BNO055_GYRO_DATA_X_LSB_ADDR;
m_smt 0:39c2bb18192b 137 _i2c.write(address,tx,1,true);
m_smt 0:39c2bb18192b 138 _i2c.read(address+1,rawdata,6,0);
m_smt 0:39c2bb18192b 139 gyro.rawx = (rawdata[1] << 8 | rawdata[0]);
m_smt 0:39c2bb18192b 140 gyro.rawy = (rawdata[3] << 8 | rawdata[2]);
m_smt 0:39c2bb18192b 141 gyro.rawz = (rawdata[5] << 8 | rawdata[4]);
m_smt 0:39c2bb18192b 142 gyro.x = float(gyro.rawx)*rate_scale;
m_smt 0:39c2bb18192b 143 gyro.y = float(gyro.rawy)*rate_scale;
m_smt 0:39c2bb18192b 144 gyro.z = float(gyro.rawz)*rate_scale;
m_smt 0:39c2bb18192b 145 }
m_smt 0:39c2bb18192b 146
m_smt 0:39c2bb18192b 147 void BNO055::get_mag(void){
m_smt 0:39c2bb18192b 148 tx[0] = BNO055_MAG_DATA_X_LSB_ADDR;
m_smt 0:39c2bb18192b 149 _i2c.write(address,tx,1,true);
m_smt 0:39c2bb18192b 150 _i2c.read(address+1,rawdata,6,0);
m_smt 0:39c2bb18192b 151 mag.rawx = (rawdata[1] << 8 | rawdata[0]);
m_smt 0:39c2bb18192b 152 mag.rawy = (rawdata[3] << 8 | rawdata[2]);
m_smt 0:39c2bb18192b 153 mag.rawz = (rawdata[5] << 8 | rawdata[4]);
m_smt 0:39c2bb18192b 154 mag.x = float(mag.rawx);
m_smt 0:39c2bb18192b 155 mag.y = float(mag.rawy);
m_smt 0:39c2bb18192b 156 mag.z = float(mag.rawz);
m_smt 0:39c2bb18192b 157 }
m_smt 0:39c2bb18192b 158
m_smt 0:39c2bb18192b 159 void BNO055::get_lia(void){
m_smt 0:39c2bb18192b 160 tx[0] = BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR;
m_smt 0:39c2bb18192b 161 _i2c.write(address,tx,1,true);
m_smt 0:39c2bb18192b 162 _i2c.read(address+1,rawdata,6,0);
m_smt 0:39c2bb18192b 163 lia.rawx = (rawdata[1] << 8 | rawdata[0]);
m_smt 0:39c2bb18192b 164 lia.rawy = (rawdata[3] << 8 | rawdata[2]);
m_smt 0:39c2bb18192b 165 lia.rawz = (rawdata[5] << 8 | rawdata[4]);
m_smt 0:39c2bb18192b 166 lia.x = float(lia.rawx)*accel_scale;
m_smt 0:39c2bb18192b 167 lia.y = float(lia.rawy)*accel_scale;
m_smt 0:39c2bb18192b 168 lia.z = float(lia.rawz)*accel_scale;
m_smt 0:39c2bb18192b 169 }
m_smt 0:39c2bb18192b 170
m_smt 0:39c2bb18192b 171 void BNO055::get_grv(void){
m_smt 0:39c2bb18192b 172 tx[0] = BNO055_GRAVITY_DATA_X_LSB_ADDR;
m_smt 0:39c2bb18192b 173 _i2c.write(address,tx,1,true);
m_smt 0:39c2bb18192b 174 _i2c.read(address+1,rawdata,6,0);
m_smt 0:39c2bb18192b 175 gravity.rawx = (rawdata[1] << 8 | rawdata[0]);
m_smt 0:39c2bb18192b 176 gravity.rawy = (rawdata[3] << 8 | rawdata[2]);
m_smt 0:39c2bb18192b 177 gravity.rawz = (rawdata[5] << 8 | rawdata[4]);
m_smt 0:39c2bb18192b 178 gravity.x = float(gravity.rawx)*accel_scale;
m_smt 0:39c2bb18192b 179 gravity.y = float(gravity.rawy)*accel_scale;
m_smt 0:39c2bb18192b 180 gravity.z = float(gravity.rawz)*accel_scale;
m_smt 0:39c2bb18192b 181 }
m_smt 0:39c2bb18192b 182
m_smt 0:39c2bb18192b 183 void BNO055::get_quat(void){
m_smt 0:39c2bb18192b 184 tx[0] = BNO055_QUATERNION_DATA_W_LSB_ADDR;
m_smt 0:39c2bb18192b 185 _i2c.write(address,tx,1,true);
m_smt 0:39c2bb18192b 186 _i2c.read(address+1,rawdata,8,0);
m_smt 0:39c2bb18192b 187 quat.raww = (rawdata[1] << 8 | rawdata[0]);
m_smt 0:39c2bb18192b 188 quat.rawx = (rawdata[3] << 8 | rawdata[2]);
m_smt 0:39c2bb18192b 189 quat.rawy = (rawdata[5] << 8 | rawdata[4]);
m_smt 0:39c2bb18192b 190 quat.rawz = (rawdata[7] << 8 | rawdata[6]);
m_smt 0:39c2bb18192b 191 quat.w = float(quat.raww)/16384.0f;
m_smt 0:39c2bb18192b 192 quat.x = float(quat.rawx)/16384.0f;
m_smt 0:39c2bb18192b 193 quat.y = float(quat.rawy)/16384.0f;
m_smt 0:39c2bb18192b 194 quat.z = float(quat.rawz)/16384.0f;
m_smt 0:39c2bb18192b 195 }
m_smt 0:39c2bb18192b 196
m_smt 0:39c2bb18192b 197 void BNO055::get_angles(void){
m_smt 0:39c2bb18192b 198 tx[0] = BNO055_EULER_H_LSB_ADDR;
m_smt 0:39c2bb18192b 199 _i2c.write(address,tx,1,true);
m_smt 0:39c2bb18192b 200 _i2c.read(address+1,rawdata,6,0);
m_smt 0:39c2bb18192b 201 euler.rawyaw = (rawdata[1] << 8 | rawdata[0]);
m_smt 0:39c2bb18192b 202 euler.rawroll = (rawdata[3] << 8 | rawdata[2]);
m_smt 0:39c2bb18192b 203 euler.rawpitch = (rawdata[5] << 8 | rawdata[4]);
m_smt 0:39c2bb18192b 204 euler.yaw = float(euler.rawyaw)*angle_scale;
m_smt 0:39c2bb18192b 205 euler.roll = float(euler.rawroll)*angle_scale;
m_smt 0:39c2bb18192b 206 euler.pitch = float(euler.rawpitch)*angle_scale;
m_smt 0:39c2bb18192b 207 }
m_smt 0:39c2bb18192b 208
m_smt 0:39c2bb18192b 209
m_smt 0:39c2bb18192b 210 void BNO055::get_temp(void){
m_smt 0:39c2bb18192b 211 readchar(BNO055_TEMP_ADDR);
m_smt 0:39c2bb18192b 212 temperature = rx / temp_scale;
m_smt 0:39c2bb18192b 213 }
m_smt 0:39c2bb18192b 214
m_smt 0:39c2bb18192b 215 void BNO055::get_calib(void){
m_smt 0:39c2bb18192b 216 readchar(BNO055_CALIB_STAT_ADDR);
m_smt 0:39c2bb18192b 217 calib = rx;
m_smt 0:39c2bb18192b 218 }
m_smt 0:39c2bb18192b 219
m_smt 0:39c2bb18192b 220 void BNO055::read_calibration_data(void){
m_smt 0:39c2bb18192b 221 char tempmode = op_mode;
m_smt 0:39c2bb18192b 222 setmode(OPERATION_MODE_CONFIG);
m_smt 0:39c2bb18192b 223 wait_ms(20);
m_smt 0:39c2bb18192b 224 tx[0] = ACCEL_OFFSET_X_LSB_ADDR;
m_smt 0:39c2bb18192b 225 _i2c.write(address,tx,1,true);
m_smt 0:39c2bb18192b 226 _i2c.read(address,calibration,22,false);
m_smt 0:39c2bb18192b 227 setmode(tempmode);
m_smt 0:39c2bb18192b 228 wait_ms(10);
m_smt 0:39c2bb18192b 229 }
m_smt 0:39c2bb18192b 230
m_smt 0:39c2bb18192b 231 void BNO055::write_calibration_data(void){
m_smt 0:39c2bb18192b 232 char tempmode = op_mode;
m_smt 0:39c2bb18192b 233 setmode(OPERATION_MODE_CONFIG);
m_smt 0:39c2bb18192b 234 wait_ms(20);
m_smt 0:39c2bb18192b 235 tx[0] = ACCEL_OFFSET_X_LSB_ADDR;
m_smt 0:39c2bb18192b 236 _i2c.write(address,tx,1,true);
m_smt 0:39c2bb18192b 237 _i2c.write(address,calibration,22,false);
m_smt 0:39c2bb18192b 238 setmode(tempmode);
m_smt 0:39c2bb18192b 239 wait_ms(10);
m_smt 0:39c2bb18192b 240 }
m_smt 0:39c2bb18192b 241
m_smt 0:39c2bb18192b 242 void BNO055::set_mapping(char orient){
m_smt 0:39c2bb18192b 243 switch (orient){
m_smt 0:39c2bb18192b 244 case 0:
m_smt 0:39c2bb18192b 245 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x21);
m_smt 0:39c2bb18192b 246 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x04);
m_smt 0:39c2bb18192b 247 break;
m_smt 0:39c2bb18192b 248 case 1:
m_smt 0:39c2bb18192b 249 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24);
m_smt 0:39c2bb18192b 250 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x00);
m_smt 0:39c2bb18192b 251 break;
m_smt 0:39c2bb18192b 252 case 2:
m_smt 0:39c2bb18192b 253 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24);
m_smt 0:39c2bb18192b 254 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x00);
m_smt 0:39c2bb18192b 255 break;
m_smt 0:39c2bb18192b 256 case 3:
m_smt 0:39c2bb18192b 257 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x21);
m_smt 0:39c2bb18192b 258 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x02);
m_smt 0:39c2bb18192b 259 break;
m_smt 0:39c2bb18192b 260 case 4:
m_smt 0:39c2bb18192b 261 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24);
m_smt 0:39c2bb18192b 262 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x03);
m_smt 0:39c2bb18192b 263 break;
m_smt 0:39c2bb18192b 264 case 5:
m_smt 0:39c2bb18192b 265 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x21);
m_smt 0:39c2bb18192b 266 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x01);
m_smt 0:39c2bb18192b 267 break;
m_smt 0:39c2bb18192b 268 case 6:
m_smt 0:39c2bb18192b 269 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x21);
m_smt 0:39c2bb18192b 270 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x07);
m_smt 0:39c2bb18192b 271 break;
m_smt 0:39c2bb18192b 272 case 7:
m_smt 0:39c2bb18192b 273 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24);
m_smt 0:39c2bb18192b 274 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x05);
m_smt 0:39c2bb18192b 275 break;
m_smt 0:39c2bb18192b 276 default:
m_smt 0:39c2bb18192b 277 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24);
m_smt 0:39c2bb18192b 278 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x00);
m_smt 0:39c2bb18192b 279 }
m_smt 0:39c2bb18192b 280 }