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@25:f3bf8351bbd4, 2016-04-05 (annotated)

Committer:
12104404
Date:
Tue Apr 05 02:30:40 2016 +0000
Revision:
25:f3bf8351bbd4
Parent:
24:fb1f083ebd62
Child:
26:0ea6a550a99d
goood but overshoot bottom missing tol state

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