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@15:7729da55873a, 2016-03-24 (annotated)

Committer:
12104404
Date:
Thu Mar 24 07:20:51 2016 +0000
Revision:
15:7729da55873a
Parent:
13:c62f975dfcfe
Child:
19:2dd81b864e14
how to combine x y control

Who changed what in which revision?

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