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@19:2dd81b864e14, 2016-04-03 (annotated)

Committer:
12104404
Date:
Sun Apr 03 05:48:37 2016 +0000
Revision:
19:2dd81b864e14
Parent:
15:7729da55873a
Child:
20:10a305298e27
Cleaning up code;

Who changed what in which revision?

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