Edwin Cho / Mbed 2 deprecated FLOW_DERP

DERPS

Dependencies:   BMP280 BNO055_fusion PowerControl mbed

Fork of STRAIGHT_DRIVE_NO_SEP by Antoine Laurens

LOCALIZE.cpp@40:9a97c4403c0a, 2016-05-03 (annotated)

Committer:
12104404
Date:
Tue May 03 19:17:04 2016 +0000
Revision:
40:9a97c4403c0a
Parent:
38:16208e003dc9 
WORKING;

Who changed what in which revision?

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