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@2:634c1adf89b2, 2016-03-02 (annotated)

Committer:
12104404
Date:
Wed Mar 02 07:21:05 2016 +0000
Revision:
2:634c1adf89b2
Parent:
1:8966a48ce8d5
Child:
3:c58eb4be51de
ADDED ADDRESS TRANSLATOR;

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 1:8966a48ce8d5 4 _y_i2c(y_i2c), _x_imu_i2c(x_imu_i2c), _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 1:8966a48ce8d5 15 char data[]= {0,0,0};
12104404 1:8966a48ce8d5 16 if(_x_imu_i2c.read(R_P_ADDR<<1, data, 3)!=0)
12104404 1:8966a48ce8d5 17 return ERROR_RX_P;
12104404 2:634c1adf89b2 18 if(_x_imu_i2c.read(R_N_ADDR<<1, data, 3)!=0)
12104404 2:634c1adf89b2 19 return ERROR_RX_N;
12104404 1:8966a48ce8d5 20 if(_y_i2c.read(R_P_ADDR<<1, data, 3)!=0)
12104404 1:8966a48ce8d5 21 return ERROR_RY_P;
12104404 2:634c1adf89b2 22 if(_y_i2c.read(R_N_ADDR<<1, data, 3)!=0)
12104404 2:634c1adf89b2 23 return ERROR_RY_N;
12104404 0:96d6eb224379 24 return 0;
12104404 0:96d6eb224379 25 }
12104404 0:96d6eb224379 26
12104404 0:96d6eb224379 27 void LOCALIZE::get_angle(LOCALIZE_xya *xya)
12104404 0:96d6eb224379 28 {
12104404 1:8966a48ce8d5 29 _imu.get_Euler_Angles(&_euler);
12104404 1:8966a48ce8d5 30 _imu.get_gravity(&_gravity);
12104404 1:8966a48ce8d5 31 int _angle=-1;
12104404 0:96d6eb224379 32 unsigned int quad=0;
12104404 1:8966a48ce8d5 33 if(_gravity.y>=0)
12104404 0:96d6eb224379 34 quad&=~1;
12104404 0:96d6eb224379 35 else
12104404 0:96d6eb224379 36 quad|=1;
12104404 1:8966a48ce8d5 37 if(_gravity.x>=0)
12104404 0:96d6eb224379 38 quad&=~(1<<1);
12104404 0:96d6eb224379 39 else
12104404 0:96d6eb224379 40 quad|=(1<<1);
12104404 0:96d6eb224379 41 quad&=3;
12104404 0:96d6eb224379 42 switch(quad) {
12104404 0:96d6eb224379 43 case 0:
12104404 1:8966a48ce8d5 44 _angle=_euler.p;
12104404 0:96d6eb224379 45 break;
12104404 0:96d6eb224379 46 case 1:
12104404 1:8966a48ce8d5 47 _angle=180-_euler.p;
12104404 0:96d6eb224379 48 break;
12104404 0:96d6eb224379 49 case 2:
12104404 1:8966a48ce8d5 50 _angle=360+_euler.p;
12104404 0:96d6eb224379 51 break;
12104404 0:96d6eb224379 52 case 3:
12104404 1:8966a48ce8d5 53 _angle=180+abs(_euler.p);
12104404 0:96d6eb224379 54 break;
12104404 0:96d6eb224379 55 default:
12104404 1:8966a48ce8d5 56 _angle=-1;
12104404 0:96d6eb224379 57 break;
12104404 0:96d6eb224379 58 }
12104404 1:8966a48ce8d5 59 _xya.a=_angle;
12104404 1:8966a48ce8d5 60 xya->a=_xya.a;
12104404 0:96d6eb224379 61 }
12104404 1:8966a48ce8d5 62
12104404 1:8966a48ce8d5 63 void LOCALIZE::get_xy(LOCALIZE_xya *xya)
12104404 1:8966a48ce8d5 64 {
12104404 1:8966a48ce8d5 65 char data[]= {0,0,0};
12104404 2:634c1adf89b2 66 int _rx_p, _ry_p, _rx_n, _ry_n, _rx, _ry;
12104404 1:8966a48ce8d5 67 _x_imu_i2c.read(R_P_ADDR<<1, data, 3);
12104404 1:8966a48ce8d5 68 //longer wait for longer distance
12104404 1:8966a48ce8d5 69 wait(0.1);
12104404 1:8966a48ce8d5 70 _rx_p=data[1];
12104404 2:634c1adf89b2 71 _x_imu_i2c.read(R_N_ADDR<<1, data, 3);
12104404 2:634c1adf89b2 72 wait(0.1);
12104404 2:634c1adf89b2 73 _rx_n=data[1];
12104404 1:8966a48ce8d5 74 _y_i2c.read(R_P_ADDR<<1, data, 3);
12104404 1:8966a48ce8d5 75 wait(0.1);
12104404 1:8966a48ce8d5 76 _ry_p=data[1];
12104404 2:634c1adf89b2 77 // _y_i2c.read(R_N_ADDR<<1, data, 3);
12104404 2:634c1adf89b2 78 // wait(0.1);
12104404 2:634c1adf89b2 79 // _ry_n=data[1];
12104404 1:8966a48ce8d5 80 //check angle again
12104404 1:8966a48ce8d5 81 get_angle(xya);
12104404 1:8966a48ce8d5 82 if(_xya.a<R_ERROR || _xya.a>360-R_ERROR) {
12104404 2:634c1adf89b2 83 _rx=_rx_n;
12104404 1:8966a48ce8d5 84 _ry=_ry_p;
12104404 1:8966a48ce8d5 85 } else if(abs(_xya.a-90)<R_ERROR) {
12104404 1:8966a48ce8d5 86 _rx=_ry_p;
12104404 1:8966a48ce8d5 87 _ry=FRAME_H-RX_OFF-_rx_p;
12104404 1:8966a48ce8d5 88 } else if(abs(_xya.a-180)<R_ERROR) {
12104404 1:8966a48ce8d5 89 _rx=FRAME_W-RX_OFF-_rx_p;
12104404 1:8966a48ce8d5 90 _ry=FRAME_H-RY_OFF-_ry_p;
12104404 1:8966a48ce8d5 91 } else if(abs(_xya.a-270)<R_ERROR) {
12104404 1:8966a48ce8d5 92 _rx=FRAME_W-RY_OFF-_ry_p;
12104404 1:8966a48ce8d5 93 _ry=_rx_p;
12104404 1:8966a48ce8d5 94 } else {
12104404 1:8966a48ce8d5 95 _rx=_rx_p;
12104404 1:8966a48ce8d5 96 _ry=_ry_p;
12104404 1:8966a48ce8d5 97 }
12104404 1:8966a48ce8d5 98 _xya.x=_rx;
12104404 1:8966a48ce8d5 99 _xya.y=_ry;
12104404 1:8966a48ce8d5 100 xya->x=_xya.x;
12104404 1:8966a48ce8d5 101 xya->y=_xya.y;
12104404 1:8966a48ce8d5 102 }
12104404 1:8966a48ce8d5 103