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@13:c62f975dfcfe, 2016-03-23 (annotated)

Committer:
12104404
Date:
Wed Mar 23 18:12:47 2016 +0000
Revision:
13:c62f975dfcfe
Parent:
10:cf44f4387bc7
Child:
15:7729da55873a
WORKING buttons

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