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@23:455f7da3dd7a, 2016-04-03 (annotated)

Committer:
12104404
Date:
Sun Apr 03 07:08:18 2016 +0000
Revision:
23:455f7da3dd7a
Parent:
20:10a305298e27
Child:
24:fb1f083ebd62
Brown Out and LED calling fixed

Who changed what in which revision?

UserRevisionLine numberNew contents of line
12104404 0:96d6eb224379 1 #include "LOCALIZE.h"
12104404 0:96d6eb224379 2
12104404 23:455f7da3dd7a 3 LOCALIZE::LOCALIZE (I2C& y_i2c, I2C& x_imu_i2c, PinName imu_reset, PinName sw1, PinName sw2, PinName sw3, PinName sw4, DigitalOut& led1, DigitalOut& led2, DigitalOut& led3, DigitalOut& led4):
12104404 23:455f7da3dd7a 4 _y_i2c(y_i2c), _x_imu_i2c(x_imu_i2c), _imu(x_imu_i2c, imu_reset), _sw1(sw1), _sw2(sw2), _sw3(sw3), _sw4(sw4), _led1(led1), _led2(led2), _led3(led3), _led4(led4)
12104404 13:c62f975dfcfe 5 {
12104404 15:7729da55873a 6 _sw1.mode(PullUp);
12104404 15:7729da55873a 7 _sw2.mode(PullUp);
12104404 15:7729da55873a 8 _sw3.mode(PullUp);
12104404 15:7729da55873a 9 _sw4.mode(PullUp);
12104404 13:c62f975dfcfe 10 }
12104404 0:96d6eb224379 11
12104404 0:96d6eb224379 12 int LOCALIZE::init (void)
12104404 0:96d6eb224379 13 {
12104404 0:96d6eb224379 14 if(_imu.chip_ready()==0)
12104404 0:96d6eb224379 15 return ERROR_IMU;
12104404 0:96d6eb224379 16 _imu.set_mounting_position(MT_P0);
12104404 19:2dd81b864e14 17 #if defined (IMU_GROUND)
12104404 6:0602a9e8118b 18 _imu.change_fusion_mode(MODE_NDOF);
12104404 19:2dd81b864e14 19 #else
12104404 19:2dd81b864e14 20 _imu.change_fusion_mode(MODE_IMU);
12104404 19:2dd81b864e14 21 #endif
12104404 19:2dd81b864e14 22
12104404 1:8966a48ce8d5 23 char data[]= {0,0,0};
12104404 1:8966a48ce8d5 24 if(_x_imu_i2c.read(R_P_ADDR<<1, data, 3)!=0)
12104404 1:8966a48ce8d5 25 return ERROR_RX_P;
12104404 2:634c1adf89b2 26 if(_x_imu_i2c.read(R_N_ADDR<<1, data, 3)!=0)
12104404 2:634c1adf89b2 27 return ERROR_RX_N;
12104404 1:8966a48ce8d5 28 if(_y_i2c.read(R_P_ADDR<<1, data, 3)!=0)
12104404 1:8966a48ce8d5 29 return ERROR_RY_P;
12104404 2:634c1adf89b2 30 if(_y_i2c.read(R_N_ADDR<<1, data, 3)!=0)
12104404 2:634c1adf89b2 31 return ERROR_RY_N;
12104404 0:96d6eb224379 32 return 0;
12104404 0:96d6eb224379 33 }
12104404 0:96d6eb224379 34
12104404 0:96d6eb224379 35 void LOCALIZE::get_angle(LOCALIZE_xya *xya)
12104404 0:96d6eb224379 36 {
12104404 1:8966a48ce8d5 37 _imu.get_Euler_Angles(&_euler);
12104404 1:8966a48ce8d5 38 _imu.get_gravity(&_gravity);
12104404 20:10a305298e27 39 volatile unsigned int quad=0;
12104404 1:8966a48ce8d5 40 if(_gravity.y>=0)
12104404 0:96d6eb224379 41 quad&=~1;
12104404 0:96d6eb224379 42 else
12104404 0:96d6eb224379 43 quad|=1;
12104404 1:8966a48ce8d5 44 if(_gravity.x>=0)
12104404 0:96d6eb224379 45 quad&=~(1<<1);
12104404 0:96d6eb224379 46 else
12104404 0:96d6eb224379 47 quad|=(1<<1);
12104404 0:96d6eb224379 48 quad&=3;
12104404 0:96d6eb224379 49 switch(quad) {
12104404 0:96d6eb224379 50 case 0:
12104404 1:8966a48ce8d5 51 _angle=_euler.p;
12104404 0:96d6eb224379 52 break;
12104404 0:96d6eb224379 53 case 1:
12104404 1:8966a48ce8d5 54 _angle=180-_euler.p;
12104404 0:96d6eb224379 55 break;
12104404 0:96d6eb224379 56 case 2:
12104404 1:8966a48ce8d5 57 _angle=360+_euler.p;
12104404 0:96d6eb224379 58 break;
12104404 0:96d6eb224379 59 case 3:
12104404 1:8966a48ce8d5 60 _angle=180+abs(_euler.p);
12104404 0:96d6eb224379 61 break;
12104404 0:96d6eb224379 62 default:
12104404 1:8966a48ce8d5 63 _angle=-1;
12104404 0:96d6eb224379 64 break;
12104404 0:96d6eb224379 65 }
12104404 19:2dd81b864e14 66 #if defined (IMU_GROUND)
12104404 6:0602a9e8118b 67 xya->a=_euler.h;
12104404 19:2dd81b864e14 68 #else
12104404 19:2dd81b864e14 69 xya->a=_angle;
12104404 19:2dd81b864e14 70 #endif
12104404 8:b36be08c44f8 71 _xya.a=xya->a;
12104404 0:96d6eb224379 72 }
12104404 1:8966a48ce8d5 73
12104404 1:8966a48ce8d5 74 void LOCALIZE::get_xy(LOCALIZE_xya *xya)
12104404 1:8966a48ce8d5 75 {
12104404 3:c58eb4be51de 76 int _rx, _ry;
12104404 3:c58eb4be51de 77 get_raw_xy();
12104404 1:8966a48ce8d5 78 get_angle(xya);
12104404 13:c62f975dfcfe 79 ledSw();
12104404 1:8966a48ce8d5 80 if(_xya.a<R_ERROR || _xya.a>360-R_ERROR) {
12104404 19:2dd81b864e14 81 _rx=_rx_p<_rx_n && _rx_p>UR_DEADZONE ? _rx_p : FRAME_W-RX_OFF-_rx_n;
12104404 19:2dd81b864e14 82 _ry=_ry_p<_ry_n && _ry_p>UR_DEADZONE ? _ry_p : FRAME_H-RY_OFF-_ry_n;
12104404 15:7729da55873a 83 if(!_sw1 && !_sw2)
12104404 13:c62f975dfcfe 84 _rx=RX_OFF;
12104404 15:7729da55873a 85 else if(!_sw3 && !_sw4)
12104404 13:c62f975dfcfe 86 _rx=FRAME_W-RX_OFF;
12104404 9:4fc5d70c3bab 87 } else if(abs(_xya.a-270)<R_ERROR) {
12104404 19:2dd81b864e14 88 _rx=_ry_p<_ry_n && _ry_p>UR_DEADZONE ? _ry_p : FRAME_W-RY_OFF-_ry_n;
12104404 19:2dd81b864e14 89 _ry=_rx_p<_rx_n && _rx_p>UR_DEADZONE ? FRAME_H-RX_OFF-_rx_p : _rx_n;
12104404 15:7729da55873a 90 if(!_sw1 && !_sw2)
12104404 13:c62f975dfcfe 91 _ry=FRAME_H-RY_OFF;
12104404 15:7729da55873a 92 else if(!_sw3 && !_sw4)
12104404 13:c62f975dfcfe 93 _ry=RY_OFF;
12104404 1:8966a48ce8d5 94 } else if(abs(_xya.a-180)<R_ERROR) {
12104404 19:2dd81b864e14 95 _rx=_rx_p<_rx_n && _rx_p>UR_DEADZONE ? FRAME_W-RX_OFF-_rx_p : _rx_n;
12104404 19:2dd81b864e14 96 _ry=_ry_p<_ry_n && _ry_p>UR_DEADZONE ? FRAME_H-RY_OFF-_ry_p : _ry_n;
12104404 15:7729da55873a 97 if(!_sw1 && !_sw2)
12104404 13:c62f975dfcfe 98 _rx=FRAME_W-RX_OFF;
12104404 15:7729da55873a 99 else if(!_sw3 && !_sw4)
12104404 13:c62f975dfcfe 100 _rx=RX_OFF;
12104404 9:4fc5d70c3bab 101 } else if(abs(_xya.a-90)<R_ERROR) {
12104404 19:2dd81b864e14 102 _rx=_ry_p<_ry_n && _ry_p>UR_DEADZONE ? FRAME_W-RY_OFF-_ry_p : _ry_n;
12104404 19:2dd81b864e14 103 _ry=_rx_p<_rx_n && _rx_p>UR_DEADZONE ? _rx_p : FRAME_H-RX_OFF-_rx_n;
12104404 15:7729da55873a 104 if(!_sw1 && !_sw2)
12104404 13:c62f975dfcfe 105 _ry=RY_OFF;
12104404 15:7729da55873a 106 else if(!_sw3 && !_sw4)
12104404 13:c62f975dfcfe 107 _ry=FRAME_H-RY_OFF;
12104404 1:8966a48ce8d5 108 } else {
12104404 19:2dd81b864e14 109 //error last value
12104404 13:c62f975dfcfe 110 _rx=_xya.x;
12104404 13:c62f975dfcfe 111 _ry=_xya.x;
12104404 1:8966a48ce8d5 112 }
12104404 20:10a305298e27 113 if(abs(_rx-_xya.x)>UR_JUMP) {
12104404 20:10a305298e27 114 _rx=_xya.x;
12104404 20:10a305298e27 115 }
12104404 20:10a305298e27 116 if(abs(_ry-_xya.y)>UR_JUMP) {
12104404 20:10a305298e27 117 _ry=_xya.y;
12104404 20:10a305298e27 118 }
12104404 1:8966a48ce8d5 119 _xya.x=_rx;
12104404 1:8966a48ce8d5 120 _xya.y=_ry;
12104404 1:8966a48ce8d5 121 xya->x=_xya.x;
12104404 1:8966a48ce8d5 122 xya->y=_xya.y;
12104404 1:8966a48ce8d5 123 }
12104404 1:8966a48ce8d5 124
12104404 3:c58eb4be51de 125 void LOCALIZE::get_raw_xy(void)
12104404 3:c58eb4be51de 126 {
12104404 3:c58eb4be51de 127 char data[]= {0,0,0};
12104404 3:c58eb4be51de 128 _x_imu_i2c.read(R_P_ADDR<<1, data, 3);
12104404 4:d70375cfa533 129 wait(R_WAIT);
12104404 8:b36be08c44f8 130 _rx_n=data[1];
12104404 3:c58eb4be51de 131 _x_imu_i2c.read(R_N_ADDR<<1, data, 3);
12104404 4:d70375cfa533 132 wait(R_WAIT);
12104404 8:b36be08c44f8 133 _rx_p=data[1];
12104404 3:c58eb4be51de 134 _y_i2c.read(R_P_ADDR<<1, data, 3);
12104404 4:d70375cfa533 135 wait(R_WAIT);
12104404 8:b36be08c44f8 136 _ry_n=data[1];
12104404 3:c58eb4be51de 137 _y_i2c.read(R_N_ADDR<<1, data, 3);
12104404 4:d70375cfa533 138 wait(R_WAIT);
12104404 8:b36be08c44f8 139 _ry_p=data[1];
12104404 13:c62f975dfcfe 140 }
12104404 13:c62f975dfcfe 141
12104404 20:10a305298e27 142 inline void LOCALIZE::ledSw(void)
12104404 13:c62f975dfcfe 143 {
12104404 19:2dd81b864e14 144 _led1=!_sw1;
12104404 19:2dd81b864e14 145 _led2=!_sw2;
12104404 19:2dd81b864e14 146 _led3=!_sw3;
12104404 19:2dd81b864e14 147 _led4=!_sw4;
12104404 10:cf44f4387bc7 148 }