Antoine Laurens / Mbed 2 deprecated STRAIGHT_DRIVE_NO_SEP

drive down

Dependencies:   BMP280 BNO055_fusion PowerControl mbed

Fork of TEAM_G_FLOW_RIDA by Edwin Cho

LOCALIZE.cpp@0:96d6eb224379, 2016-03-01 (annotated)

Committer:
12104404
Date:
Tue Mar 01 18:20:45 2016 +0000
Revision:
0:96d6eb224379
Child:
1:8966a48ce8d5
ANGLE;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
12104404 0:96d6eb224379 1 #include "LOCALIZE.h"
12104404 0:96d6eb224379 2
12104404 0:96d6eb224379 3 LOCALIZE::LOCALIZE (I2C& y_i2c, I2C& x_imu_i2c, PinName imu_reset):
12104404 0:96d6eb224379 4 _y_i2c(y_i2c), _x_imu_i2c(x_imu_i2c), _imu_reset(imu_reset), _imu(x_imu_i2c, imu_reset)
12104404 0:96d6eb224379 5 {
12104404 0:96d6eb224379 6
12104404 0:96d6eb224379 7 }
12104404 0:96d6eb224379 8
12104404 0:96d6eb224379 9 int LOCALIZE::init (void)
12104404 0:96d6eb224379 10 {
12104404 0:96d6eb224379 11 if(_imu.chip_ready()==0)
12104404 0:96d6eb224379 12 return ERROR_IMU;
12104404 0:96d6eb224379 13 _imu.set_mounting_position(MT_P0);
12104404 0:96d6eb224379 14 _imu.change_fusion_mode(MODE_IMU);
12104404 0:96d6eb224379 15 return 0;
12104404 0:96d6eb224379 16 }
12104404 0:96d6eb224379 17
12104404 0:96d6eb224379 18 void LOCALIZE::get_angle(LOCALIZE_xya *xya)
12104404 0:96d6eb224379 19 {
12104404 0:96d6eb224379 20 _imu.get_Euler_Angles(&euler);
12104404 0:96d6eb224379 21 _imu.get_gravity(&gravity);
12104404 0:96d6eb224379 22 int angle=0;
12104404 0:96d6eb224379 23 unsigned int quad=0;
12104404 0:96d6eb224379 24 if(gravity.y>=0)
12104404 0:96d6eb224379 25 quad&=~1;
12104404 0:96d6eb224379 26 else
12104404 0:96d6eb224379 27 quad|=1;
12104404 0:96d6eb224379 28 if(gravity.x>=0)
12104404 0:96d6eb224379 29 quad&=~(1<<1);
12104404 0:96d6eb224379 30 else
12104404 0:96d6eb224379 31 quad|=(1<<1);
12104404 0:96d6eb224379 32 quad&=3;
12104404 0:96d6eb224379 33 switch(quad) {
12104404 0:96d6eb224379 34 case 0:
12104404 0:96d6eb224379 35 angle=euler.p;
12104404 0:96d6eb224379 36 break;
12104404 0:96d6eb224379 37 case 1:
12104404 0:96d6eb224379 38 angle=180-euler.p;
12104404 0:96d6eb224379 39 break;
12104404 0:96d6eb224379 40 case 2:
12104404 0:96d6eb224379 41 angle=360+euler.p;
12104404 0:96d6eb224379 42 break;
12104404 0:96d6eb224379 43 case 3:
12104404 0:96d6eb224379 44 angle=180+abs(euler.p);
12104404 0:96d6eb224379 45 break;
12104404 0:96d6eb224379 46 default:
12104404 0:96d6eb224379 47 angle=-1;
12104404 0:96d6eb224379 48 break;
12104404 0:96d6eb224379 49 }
12104404 0:96d6eb224379 50 xya->a=angle;
12104404 0:96d6eb224379 51 }