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@8:b36be08c44f8, 2016-03-23 (annotated)

Committer:
12104404
Date:
Wed Mar 23 06:55:39 2016 +0000
Revision:
8:b36be08c44f8
Parent:
6:0602a9e8118b
Child:
9:4fc5d70c3bab
frame

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 4:d70375cfa533 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 int LOCALIZE::init (void)
12104404 0:96d6eb224379 7 {
12104404 0:96d6eb224379 8 if(_imu.chip_ready()==0)
12104404 0:96d6eb224379 9 return ERROR_IMU;
12104404 0:96d6eb224379 10 _imu.set_mounting_position(MT_P0);
12104404 6:0602a9e8118b 11 //_imu.change_fusion_mode(MODE_IMU);
12104404 6:0602a9e8118b 12 _imu.change_fusion_mode(MODE_NDOF);
12104404 1:8966a48ce8d5 13 char data[]= {0,0,0};
12104404 1:8966a48ce8d5 14 if(_x_imu_i2c.read(R_P_ADDR<<1, data, 3)!=0)
12104404 1:8966a48ce8d5 15 return ERROR_RX_P;
12104404 2:634c1adf89b2 16 if(_x_imu_i2c.read(R_N_ADDR<<1, data, 3)!=0)
12104404 2:634c1adf89b2 17 return ERROR_RX_N;
12104404 1:8966a48ce8d5 18 if(_y_i2c.read(R_P_ADDR<<1, data, 3)!=0)
12104404 1:8966a48ce8d5 19 return ERROR_RY_P;
12104404 2:634c1adf89b2 20 if(_y_i2c.read(R_N_ADDR<<1, data, 3)!=0)
12104404 2:634c1adf89b2 21 return ERROR_RY_N;
12104404 0:96d6eb224379 22 return 0;
12104404 0:96d6eb224379 23 }
12104404 0:96d6eb224379 24
12104404 0:96d6eb224379 25 void LOCALIZE::get_angle(LOCALIZE_xya *xya)
12104404 0:96d6eb224379 26 {
12104404 1:8966a48ce8d5 27 _imu.get_Euler_Angles(&_euler);
12104404 1:8966a48ce8d5 28 _imu.get_gravity(&_gravity);
12104404 0:96d6eb224379 29 unsigned int quad=0;
12104404 1:8966a48ce8d5 30 if(_gravity.y>=0)
12104404 0:96d6eb224379 31 quad&=~1;
12104404 0:96d6eb224379 32 else
12104404 0:96d6eb224379 33 quad|=1;
12104404 1:8966a48ce8d5 34 if(_gravity.x>=0)
12104404 0:96d6eb224379 35 quad&=~(1<<1);
12104404 0:96d6eb224379 36 else
12104404 0:96d6eb224379 37 quad|=(1<<1);
12104404 0:96d6eb224379 38 quad&=3;
12104404 0:96d6eb224379 39 switch(quad) {
12104404 0:96d6eb224379 40 case 0:
12104404 1:8966a48ce8d5 41 _angle=_euler.p;
12104404 0:96d6eb224379 42 break;
12104404 0:96d6eb224379 43 case 1:
12104404 1:8966a48ce8d5 44 _angle=180-_euler.p;
12104404 0:96d6eb224379 45 break;
12104404 0:96d6eb224379 46 case 2:
12104404 1:8966a48ce8d5 47 _angle=360+_euler.p;
12104404 0:96d6eb224379 48 break;
12104404 0:96d6eb224379 49 case 3:
12104404 1:8966a48ce8d5 50 _angle=180+abs(_euler.p);
12104404 0:96d6eb224379 51 break;
12104404 0:96d6eb224379 52 default:
12104404 1:8966a48ce8d5 53 _angle=-1;
12104404 0:96d6eb224379 54 break;
12104404 0:96d6eb224379 55 }
12104404 8:b36be08c44f8 56 //_xya.a=_angle-FRAME_OFF;
12104404 6:0602a9e8118b 57 //xya->a=_xya.a;
12104404 6:0602a9e8118b 58 xya->a=_euler.h;
12104404 8:b36be08c44f8 59 _xya.a=xya->a;
12104404 0:96d6eb224379 60 }
12104404 1:8966a48ce8d5 61
12104404 1:8966a48ce8d5 62 void LOCALIZE::get_xy(LOCALIZE_xya *xya)
12104404 1:8966a48ce8d5 63 {
12104404 3:c58eb4be51de 64 int _rx, _ry;
12104404 3:c58eb4be51de 65 get_raw_xy();
12104404 1:8966a48ce8d5 66 get_angle(xya);
12104404 1:8966a48ce8d5 67 if(_xya.a<R_ERROR || _xya.a>360-R_ERROR) {
12104404 4:d70375cfa533 68 _rx=_rx_p<_rx_n ? _rx_p : FRAME_W-RX_OFF-_rx_n;
12104404 4:d70375cfa533 69 _ry=_ry_p<_ry_n ? _ry_p : FRAME_H-RY_OFF-_ry_n;
12104404 1:8966a48ce8d5 70 } else if(abs(_xya.a-90)<R_ERROR) {
12104404 4:d70375cfa533 71 _rx=_ry_p<_ry_n ? _ry_p : FRAME_W-RY_OFF-_ry_n;
12104404 4:d70375cfa533 72 _ry=_rx_p<_rx_n ? FRAME_H-RX_OFF-_rx_p : _rx_n;
12104404 1:8966a48ce8d5 73 } else if(abs(_xya.a-180)<R_ERROR) {
12104404 4:d70375cfa533 74 _rx=_rx_p<_rx_n ? FRAME_W-RX_OFF-_rx_p : _rx_n;
12104404 4:d70375cfa533 75 _ry=_ry_p<_ry_n ? FRAME_H-RY_OFF-_ry_p : _ry_n;
12104404 1:8966a48ce8d5 76 } else if(abs(_xya.a-270)<R_ERROR) {
12104404 4:d70375cfa533 77 _rx=_ry_p<_ry_n ? FRAME_W-RY_OFF-_ry_p : _ry_n;
12104404 4:d70375cfa533 78 _ry=_rx_p<_rx_n ? _rx_p : FRAME_H-RX_OFF-_rx_n;
12104404 1:8966a48ce8d5 79 } else {
12104404 4:d70375cfa533 80 //error
12104404 1:8966a48ce8d5 81 _rx=_rx_p;
12104404 1:8966a48ce8d5 82 _ry=_ry_p;
12104404 1:8966a48ce8d5 83 }
12104404 1:8966a48ce8d5 84 _xya.x=_rx;
12104404 1:8966a48ce8d5 85 _xya.y=_ry;
12104404 1:8966a48ce8d5 86 xya->x=_xya.x;
12104404 1:8966a48ce8d5 87 xya->y=_xya.y;
12104404 1:8966a48ce8d5 88 }
12104404 1:8966a48ce8d5 89
12104404 3:c58eb4be51de 90 void LOCALIZE::get_raw_xy(void)
12104404 3:c58eb4be51de 91 {
12104404 3:c58eb4be51de 92 char data[]= {0,0,0};
12104404 3:c58eb4be51de 93 _x_imu_i2c.read(R_P_ADDR<<1, data, 3);
12104404 4:d70375cfa533 94 wait(R_WAIT);
12104404 8:b36be08c44f8 95 _rx_n=data[1];
12104404 3:c58eb4be51de 96 _x_imu_i2c.read(R_N_ADDR<<1, data, 3);
12104404 4:d70375cfa533 97 wait(R_WAIT);
12104404 8:b36be08c44f8 98 _rx_p=data[1];
12104404 3:c58eb4be51de 99 _y_i2c.read(R_P_ADDR<<1, data, 3);
12104404 4:d70375cfa533 100 wait(R_WAIT);
12104404 8:b36be08c44f8 101 _ry_n=data[1];
12104404 3:c58eb4be51de 102 _y_i2c.read(R_N_ADDR<<1, data, 3);
12104404 4:d70375cfa533 103 wait(R_WAIT);
12104404 8:b36be08c44f8 104 _ry_p=data[1];
12104404 5:c89308dc1827 105 //printf("%d %d %d %d\n",_rx_p,_rx_n,_ry_p,_ry_n);
12104404 6:0602a9e8118b 106 }