DERPS
Dependencies: BMP280 BNO055_fusion PowerControl mbed
Fork of STRAIGHT_DRIVE_NO_SEP by
LOCALIZE.cpp@38:16208e003dc9, 2016-04-27 (annotated)
- Committer:
- 12104404
- Date:
- Wed Apr 27 04:24:11 2016 +0000
- Revision:
- 38:16208e003dc9
- Parent:
- 32:37dc4d5bac98
good good;
Who changed what in which revision?
User | Revision | Line number | New 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 | } |