SHENG-HEN HSIEH
/
LSM9DS0_STM32compatible
works fine on STM
Fork of Sample_manerine_SPI_LSM9DS0 by
main.cpp@4:b9dd320947ff, 2016-09-14 (annotated)
- Committer:
- open4416
- Date:
- Wed Sep 14 05:57:55 2016 +0000
- Revision:
- 4:b9dd320947ff
- Parent:
- 3:502b83f7761c
- Child:
- 5:2f0633d8fc20
2016/9/14 stable
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
open4416 | 0:d68e088dfbcd | 1 | #include "mbed.h" |
open4416 | 1:b42c3522a50a | 2 | #include "LSM9DS0_SH.h" |
open4416 | 4:b9dd320947ff | 3 | |
open4416 | 4:b9dd320947ff | 4 | #define pi 3.141592f |
open4416 | 3:502b83f7761c | 5 | #define d2r 0.01745329f |
open4416 | 4:b9dd320947ff | 6 | |
open4416 | 4:b9dd320947ff | 7 | #define Rms 5000 //TT rate |
open4416 | 4:b9dd320947ff | 8 | #define dt 0.003f |
open4416 | 4:b9dd320947ff | 9 | #define NN 200 |
open4416 | 4:b9dd320947ff | 10 | |
open4416 | 1:b42c3522a50a | 11 | #define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) |
open4416 | 0:d68e088dfbcd | 12 | |
open4416 | 1:b42c3522a50a | 13 | //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓GPIO registor↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// |
open4416 | 1:b42c3522a50a | 14 | //~~~structure~~~// |
open4416 | 1:b42c3522a50a | 15 | DigitalOut led(D13); //detection |
open4416 | 1:b42c3522a50a | 16 | DigitalOut TT_ext(D12); |
open4416 | 1:b42c3522a50a | 17 | |
open4416 | 2:0d90c0436797 | 18 | //~~~IMU_SPI~~~// |
open4416 | 1:b42c3522a50a | 19 | DigitalOut SPI_CSG(D7,1); //low for GYRO enable |
open4416 | 1:b42c3522a50a | 20 | DigitalOut SPI_CSXM(D6,1); //low for ACC/MAG enable |
open4416 | 1:b42c3522a50a | 21 | SPI spi(D4, D5, D3); //MOSI MISO SCLK |
open4416 | 1:b42c3522a50a | 22 | |
open4416 | 4:b9dd320947ff | 23 | //~~~Servo out~~~// |
open4416 | 4:b9dd320947ff | 24 | PwmOut Drive1(D8); //control leg |
open4416 | 4:b9dd320947ff | 25 | PwmOut Drive2(D9); |
open4416 | 4:b9dd320947ff | 26 | PwmOut Drive3(D10); |
open4416 | 4:b9dd320947ff | 27 | PwmOut Drive4(D11); |
open4416 | 4:b9dd320947ff | 28 | PwmOut Drive5(D14); |
open4416 | 4:b9dd320947ff | 29 | PwmOut Drive6(D15); |
open4416 | 4:b9dd320947ff | 30 | |
open4416 | 1:b42c3522a50a | 31 | //~~~Serial~~~// |
open4416 | 1:b42c3522a50a | 32 | Serial pc(D1, D0); //Serial reg(TX RX) |
open4416 | 4:b9dd320947ff | 33 | //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of GPIO registor↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// |
open4416 | 1:b42c3522a50a | 34 | |
open4416 | 0:d68e088dfbcd | 35 | |
open4416 | 0:d68e088dfbcd | 36 | |
open4416 | 4:b9dd320947ff | 37 | //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓Varible registor↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// |
open4416 | 1:b42c3522a50a | 38 | //~~~globle~~~// |
open4416 | 1:b42c3522a50a | 39 | Ticker TT; //call a timer |
open4416 | 1:b42c3522a50a | 40 | int count = 0; //one second counter for extrenal led blink |
open4416 | 2:0d90c0436797 | 41 | |
open4416 | 4:b9dd320947ff | 42 | //~~~PWMreference~~~// |
open4416 | 4:b9dd320947ff | 43 | const int pwm_mid = 1450; //+2080 for minimall lenght |
open4416 | 4:b9dd320947ff | 44 | const int PWM_base[1][6] = { //desired six reference command at 0 deg |
open4416 | 4:b9dd320947ff | 45 | {pwm_mid+70,pwm_mid+10,pwm_mid+40,pwm_mid+60,pwm_mid+10,pwm_mid+30}, |
open4416 | 4:b9dd320947ff | 46 | // {pwm_mid,pwm_mid,pwm_mid,pwm_mid,pwm_mid,pwm_mid}, //only for debugging |
open4416 | 4:b9dd320947ff | 47 | }; |
open4416 | 4:b9dd320947ff | 48 | int PWM[1][6] = { //desired six reference command by PWM_base + PWM |
open4416 | 4:b9dd320947ff | 49 | {0,0,0,0,0,0}, //transfer equaliyu 10us to 1 deg |
open4416 | 4:b9dd320947ff | 50 | }; |
open4416 | 4:b9dd320947ff | 51 | |
open4416 | 4:b9dd320947ff | 52 | //~~~RR varible~~~// |
open4416 | 4:b9dd320947ff | 53 | float Z0 = 2.45; //as mid point hight |
open4416 | 4:b9dd320947ff | 54 | float Z_dis = -0.087; //move rotation center |
open4416 | 4:b9dd320947ff | 55 | |
open4416 | 4:b9dd320947ff | 56 | //~~~stPF_lenth_uni varible~~~// |
open4416 | 4:b9dd320947ff | 57 | const float alpha = 2.094f; //pair angle |
open4416 | 4:b9dd320947ff | 58 | const float beta = 0.1309f; //couple angle |
open4416 | 4:b9dd320947ff | 59 | const float A[3][6] = { //base cood |
open4416 | 4:b9dd320947ff | 60 | {cos(0.5f*alpha-beta),cos(0.5f*alpha+beta),cos(1.5f*alpha-beta),cos(1.5f*alpha+beta),cos(2.5f*alpha-beta),cos(2.5f*alpha+beta)}, |
open4416 | 4:b9dd320947ff | 61 | {sin(0.5f*alpha-beta),sin(0.5f*alpha+beta),sin(1.5f*alpha-beta),sin(1.5f*alpha+beta),sin(2.5f*alpha-beta),sin(2.5f*alpha+beta)}, |
open4416 | 4:b9dd320947ff | 62 | {0,0,0,0,0,0} |
open4416 | 4:b9dd320947ff | 63 | }; |
open4416 | 4:b9dd320947ff | 64 | const float B[3][6] = { //top cood(static) |
open4416 | 4:b9dd320947ff | 65 | {cos(beta),cos(alpha-beta),cos(alpha+beta),cos(2*alpha-beta),cos(2*alpha+beta),cos(3*alpha-beta)}, |
open4416 | 4:b9dd320947ff | 66 | {sin(beta),sin(alpha-beta),sin(alpha+beta),sin(2*alpha-beta),sin(2*alpha+beta),sin(3*alpha-beta)}, |
open4416 | 4:b9dd320947ff | 67 | {Z_dis,Z_dis,Z_dis,Z_dis,Z_dis,Z_dis} |
open4416 | 4:b9dd320947ff | 68 | }; |
open4416 | 4:b9dd320947ff | 69 | float C[3][6] = { //top cood(active) |
open4416 | 4:b9dd320947ff | 70 | {0,0,0,0,0,0}, |
open4416 | 4:b9dd320947ff | 71 | {0,0,0,0,0,0}, |
open4416 | 4:b9dd320947ff | 72 | {0,0,0,0,0,0} |
open4416 | 4:b9dd320947ff | 73 | }; |
open4416 | 4:b9dd320947ff | 74 | float VEC[1][6] = { //desired six reference command |
open4416 | 4:b9dd320947ff | 75 | {0,0,2.52,0,0,0}, //X Y Z VEC[0][3] VEC[0][4] VEC[0][5] |
open4416 | 4:b9dd320947ff | 76 | }; |
open4416 | 4:b9dd320947ff | 77 | float L[1][6] = { //desired six reference command |
open4416 | 4:b9dd320947ff | 78 | {0,0,0,0,0,0}, |
open4416 | 4:b9dd320947ff | 79 | }; |
open4416 | 4:b9dd320947ff | 80 | float Rtot[3][3] = { //RR' |
open4416 | 4:b9dd320947ff | 81 | {0,0,0}, |
open4416 | 4:b9dd320947ff | 82 | {0,0,0}, |
open4416 | 4:b9dd320947ff | 83 | {0,0,0} |
open4416 | 4:b9dd320947ff | 84 | }; |
open4416 | 4:b9dd320947ff | 85 | |
open4416 | 4:b9dd320947ff | 86 | //~~~stPF_tracle_R varible~~~// |
open4416 | 4:b9dd320947ff | 87 | const float L90 = 2.422; //L under 90° (195 mm) |
open4416 | 4:b9dd320947ff | 88 | const float Larm = 0.4969; //arm lenth (40 mm) b |
open4416 | 4:b9dd320947ff | 89 | const float Llink = 0.8944; //link lenth (72 mm) |
open4416 | 4:b9dd320947ff | 90 | float A_R = 0; |
open4416 | 4:b9dd320947ff | 91 | float B_R = 0; |
open4416 | 4:b9dd320947ff | 92 | float C_R = 0; |
open4416 | 4:b9dd320947ff | 93 | |
open4416 | 2:0d90c0436797 | 94 | //~~~IMU_SPI~~~// |
open4416 | 3:502b83f7761c | 95 | short low_byte = 0x00; //buffer |
open4416 | 2:0d90c0436797 | 96 | short high_byte = 0x00; |
open4416 | 3:502b83f7761c | 97 | short Buff = 0x00; |
open4416 | 3:502b83f7761c | 98 | float Wx = 0.0; |
open4416 | 3:502b83f7761c | 99 | float Wy = 0.0; |
open4416 | 3:502b83f7761c | 100 | float Wz = 0.0; |
open4416 | 3:502b83f7761c | 101 | float Ax = 0.0; |
open4416 | 3:502b83f7761c | 102 | float Ay = 0.0; |
open4416 | 3:502b83f7761c | 103 | float Az = 0.0; |
open4416 | 3:502b83f7761c | 104 | float gDIR[1][3] = { //g vector's direction , required unitconstrain |
open4416 | 4:b9dd320947ff | 105 | {0.0,0.0,0.0}, //X Y Z |
open4416 | 3:502b83f7761c | 106 | }; |
open4416 | 4:b9dd320947ff | 107 | float Bet = 0.005; //confidence of Acc data |
open4416 | 4:b9dd320947ff | 108 | float gUnity = 0.0; |
open4416 | 4:b9dd320947ff | 109 | float Gdx = 0.0; |
open4416 | 4:b9dd320947ff | 110 | float Gdy = 0.0; |
open4416 | 4:b9dd320947ff | 111 | float Gdz = 0.0; |
open4416 | 4:b9dd320947ff | 112 | float Ele_phy = 0.0; //estimation of top plate attitide |
open4416 | 4:b9dd320947ff | 113 | float Til_phy = 0.0; |
open4416 | 4:b9dd320947ff | 114 | float Ele_phy_int = 0.0; |
open4416 | 4:b9dd320947ff | 115 | float Til_phy_int = 0.0; |
open4416 | 1:b42c3522a50a | 116 | //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of Varible registor↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// |
open4416 | 1:b42c3522a50a | 117 | |
open4416 | 0:d68e088dfbcd | 118 | |
open4416 | 0:d68e088dfbcd | 119 | |
open4416 | 1:b42c3522a50a | 120 | //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓Function registor↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// |
open4416 | 4:b9dd320947ff | 121 | void init_TIMER(); //set TT_main() rate |
open4416 | 4:b9dd320947ff | 122 | void TT_main(); //timebase function rated by TT |
open4416 | 4:b9dd320947ff | 123 | void init_IO(); //initialize IO state |
open4416 | 4:b9dd320947ff | 124 | void init_IMU(); //initialize IMU |
open4416 | 4:b9dd320947ff | 125 | void init_Gdrift(); //read Gdrift at start up |
open4416 | 4:b9dd320947ff | 126 | void read_IMU(); //read IMU data give raw data |
open4416 | 4:b9dd320947ff | 127 | void state_update(); //estimation of new attitude |
open4416 | 4:b9dd320947ff | 128 | |
open4416 | 4:b9dd320947ff | 129 | void RR(); //status generator |
open4416 | 4:b9dd320947ff | 130 | void stPF_lenth_uni(); //referenve generator |
open4416 | 4:b9dd320947ff | 131 | void stPF_travle_R(); //lenth to pwm pulse width |
open4416 | 4:b9dd320947ff | 132 | |
open4416 | 4:b9dd320947ff | 133 | float lpf(float input, float output_old, float frequency); //lpf discrete |
open4416 | 1:b42c3522a50a | 134 | //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of Function registor↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// |
open4416 | 1:b42c3522a50a | 135 | |
open4416 | 1:b42c3522a50a | 136 | |
open4416 | 1:b42c3522a50a | 137 | |
open4416 | 1:b42c3522a50a | 138 | //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓main funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// |
open4416 | 0:d68e088dfbcd | 139 | int main() |
open4416 | 0:d68e088dfbcd | 140 | { |
open4416 | 4:b9dd320947ff | 141 | pc.baud(115200); //set baud rate |
open4416 | 4:b9dd320947ff | 142 | wait_ms(1000); |
open4416 | 0:d68e088dfbcd | 143 | |
open4416 | 4:b9dd320947ff | 144 | init_IO(); //initialized value |
open4416 | 4:b9dd320947ff | 145 | init_IMU(); //initialize IMU |
open4416 | 4:b9dd320947ff | 146 | init_Gdrift(); //read Gdrift at start up |
open4416 | 4:b9dd320947ff | 147 | wait_ms(1000); |
open4416 | 4:b9dd320947ff | 148 | |
open4416 | 4:b9dd320947ff | 149 | init_TIMER(); //start TT_main |
open4416 | 4:b9dd320947ff | 150 | |
open4416 | 4:b9dd320947ff | 151 | while(1) { //main() loop |
open4416 | 4:b9dd320947ff | 152 | if(count >= NN) { //check if main working |
open4416 | 0:d68e088dfbcd | 153 | count=0; |
open4416 | 0:d68e088dfbcd | 154 | led = !led; |
open4416 | 0:d68e088dfbcd | 155 | } |
open4416 | 1:b42c3522a50a | 156 | } |
open4416 | 0:d68e088dfbcd | 157 | |
open4416 | 0:d68e088dfbcd | 158 | } |
open4416 | 1:b42c3522a50a | 159 | //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of main funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// |
open4416 | 0:d68e088dfbcd | 160 | |
open4416 | 0:d68e088dfbcd | 161 | |
open4416 | 0:d68e088dfbcd | 162 | |
open4416 | 3:502b83f7761c | 163 | //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓Timebase funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// |
open4416 | 3:502b83f7761c | 164 | void init_TIMER() //set TT_main{} rate |
open4416 | 3:502b83f7761c | 165 | { |
open4416 | 3:502b83f7761c | 166 | TT.attach_us(&TT_main, Rms); |
open4416 | 3:502b83f7761c | 167 | } |
open4416 | 3:502b83f7761c | 168 | void TT_main() //interrupt function by TT |
open4416 | 3:502b83f7761c | 169 | { |
open4416 | 3:502b83f7761c | 170 | TT_ext = !TT_ext; //indicate TT_main() function working |
open4416 | 3:502b83f7761c | 171 | count = count+1; //one second counter |
open4416 | 3:502b83f7761c | 172 | |
open4416 | 3:502b83f7761c | 173 | read_IMU(); //read IMU data give raw data |
open4416 | 3:502b83f7761c | 174 | state_update(); //estimation of new attitude |
open4416 | 3:502b83f7761c | 175 | |
open4416 | 4:b9dd320947ff | 176 | RR(); //VEC generated |
open4416 | 4:b9dd320947ff | 177 | stPF_lenth_uni(); //L generated |
open4416 | 4:b9dd320947ff | 178 | stPF_travle_R(); //PWM generated |
open4416 | 4:b9dd320947ff | 179 | |
open4416 | 4:b9dd320947ff | 180 | for(int i=0; i<6; i++) { //safty constrain |
open4416 | 4:b9dd320947ff | 181 | PWM[0][i] = constrain(PWM[0][i],725,2025); |
open4416 | 4:b9dd320947ff | 182 | } |
open4416 | 4:b9dd320947ff | 183 | |
open4416 | 4:b9dd320947ff | 184 | Drive1.pulsewidth_us(PWM[0][0]); //drive command |
open4416 | 4:b9dd320947ff | 185 | Drive2.pulsewidth_us(PWM[0][1]); |
open4416 | 4:b9dd320947ff | 186 | Drive3.pulsewidth_us(PWM[0][2]); |
open4416 | 4:b9dd320947ff | 187 | Drive4.pulsewidth_us(PWM[0][3]); |
open4416 | 4:b9dd320947ff | 188 | Drive5.pulsewidth_us(PWM[0][4]); |
open4416 | 4:b9dd320947ff | 189 | Drive6.pulsewidth_us(PWM[0][5]); |
open4416 | 4:b9dd320947ff | 190 | |
open4416 | 4:b9dd320947ff | 191 | //for Serial-Oscilloscope |
open4416 | 4:b9dd320947ff | 192 | // pc.printf("%.3f\r", Bet); |
open4416 | 4:b9dd320947ff | 193 | pc.printf("%.3f,%.3f\r", Ele_phy, Til_phy); |
open4416 | 4:b9dd320947ff | 194 | // pc.printf("%.2f,%.2f\r", VEC[0][4], VEC[0][5]); |
open4416 | 4:b9dd320947ff | 195 | // pc.printf("%.2f,%.2f,%.2f\r", Ax, Ay, Az); |
open4416 | 4:b9dd320947ff | 196 | // pc.printf("%.3f,%.3f,%.3f\r", gDIR[0][0], gDIR[0][1], gDIR[0][2]); |
open4416 | 3:502b83f7761c | 197 | } |
open4416 | 4:b9dd320947ff | 198 | //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of Timebase funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// |
open4416 | 3:502b83f7761c | 199 | |
open4416 | 3:502b83f7761c | 200 | |
open4416 | 3:502b83f7761c | 201 | |
open4416 | 4:b9dd320947ff | 202 | //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓init_IO funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// |
open4416 | 1:b42c3522a50a | 203 | void init_IO(void) //initialize |
open4416 | 1:b42c3522a50a | 204 | { |
open4416 | 1:b42c3522a50a | 205 | TT_ext = 0; |
open4416 | 1:b42c3522a50a | 206 | led = 1; |
open4416 | 1:b42c3522a50a | 207 | } |
open4416 | 4:b9dd320947ff | 208 | //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of init_IO funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// |
open4416 | 0:d68e088dfbcd | 209 | |
open4416 | 0:d68e088dfbcd | 210 | |
open4416 | 0:d68e088dfbcd | 211 | |
open4416 | 4:b9dd320947ff | 212 | //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓init_IMU funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// |
open4416 | 3:502b83f7761c | 213 | void init_IMU(void) //initialize |
open4416 | 0:d68e088dfbcd | 214 | { |
open4416 | 3:502b83f7761c | 215 | //gloable config |
open4416 | 1:b42c3522a50a | 216 | SPI_CSXM = 1; //high as init for disable SPI |
open4416 | 1:b42c3522a50a | 217 | SPI_CSG = 1; |
open4416 | 1:b42c3522a50a | 218 | spi.format(8, 0); //byte width, spi mode |
open4416 | 1:b42c3522a50a | 219 | spi.frequency(4000000); //8MHz |
open4416 | 1:b42c3522a50a | 220 | |
open4416 | 3:502b83f7761c | 221 | //for GYRO config |
open4416 | 1:b42c3522a50a | 222 | SPI_CSG = 0; //start spi talking |
open4416 | 1:b42c3522a50a | 223 | spi.write(CTRL_REG1_G); |
open4416 | 1:b42c3522a50a | 224 | spi.write(0x9F); //data rate 380 Hz/ cut off 25 Hz |
open4416 | 1:b42c3522a50a | 225 | SPI_CSG = 1; //end spi talking |
open4416 | 1:b42c3522a50a | 226 | |
open4416 | 1:b42c3522a50a | 227 | SPI_CSG = 0; //start spi talking |
open4416 | 1:b42c3522a50a | 228 | spi.write(CTRL_REG4_G); |
open4416 | 1:b42c3522a50a | 229 | spi.write(0x10); //Scle 500dps |
open4416 | 1:b42c3522a50a | 230 | SPI_CSG = 1; //end spi talking |
open4416 | 1:b42c3522a50a | 231 | |
open4416 | 3:502b83f7761c | 232 | //for ACC config |
open4416 | 1:b42c3522a50a | 233 | SPI_CSXM = 0; //start spi talking |
open4416 | 1:b42c3522a50a | 234 | spi.write(CTRL_REG1_XM); |
open4416 | 1:b42c3522a50a | 235 | spi.write(0x87); //data rate 400 Hz/ Enable |
open4416 | 1:b42c3522a50a | 236 | SPI_CSXM = 1; //end spi talking |
open4416 | 1:b42c3522a50a | 237 | |
open4416 | 1:b42c3522a50a | 238 | SPI_CSXM = 0; //start spi talking |
open4416 | 1:b42c3522a50a | 239 | spi.write(CTRL_REG2_XM); |
open4416 | 3:502b83f7761c | 240 | spi.write(0xC8); //cut off 50 Hz/ Scale +-4g |
open4416 | 1:b42c3522a50a | 241 | SPI_CSXM = 1; //end spi talking |
open4416 | 0:d68e088dfbcd | 242 | } |
open4416 | 4:b9dd320947ff | 243 | //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of init_IMU funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// |
open4416 | 0:d68e088dfbcd | 244 | |
open4416 | 0:d68e088dfbcd | 245 | |
open4416 | 0:d68e088dfbcd | 246 | |
open4416 | 4:b9dd320947ff | 247 | //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓init_Gdrift funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// |
open4416 | 4:b9dd320947ff | 248 | void init_Gdrift(void) //initialize |
open4416 | 4:b9dd320947ff | 249 | { |
open4416 | 4:b9dd320947ff | 250 | for(int i=0; i<1000; i++) { |
open4416 | 4:b9dd320947ff | 251 | read_IMU(); //note Gdrift = 0 at this moment |
open4416 | 4:b9dd320947ff | 252 | gDIR[0][0] = gDIR[0][0] - Wx; |
open4416 | 4:b9dd320947ff | 253 | gDIR[0][1] = gDIR[0][1] - Wy; |
open4416 | 4:b9dd320947ff | 254 | gDIR[0][2] = gDIR[0][2] - Wz; |
open4416 | 4:b9dd320947ff | 255 | wait_ms(2); |
open4416 | 4:b9dd320947ff | 256 | } |
open4416 | 4:b9dd320947ff | 257 | Gdx = gDIR[0][0] /1000.0f; |
open4416 | 4:b9dd320947ff | 258 | Gdy = gDIR[0][1] /1000.0f; |
open4416 | 4:b9dd320947ff | 259 | Gdz = gDIR[0][2] /1000.0f; |
open4416 | 4:b9dd320947ff | 260 | // pc.printf("%.4f,%.4f,%.4f\r", Gdx, Gdy, Gdz); |
open4416 | 4:b9dd320947ff | 261 | gDIR[0][0] = 0; |
open4416 | 4:b9dd320947ff | 262 | gDIR[0][1] = 0; |
open4416 | 4:b9dd320947ff | 263 | gDIR[0][2] = -1; |
open4416 | 4:b9dd320947ff | 264 | } |
open4416 | 4:b9dd320947ff | 265 | //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of init_Gdrift funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// |
open4416 | 4:b9dd320947ff | 266 | |
open4416 | 4:b9dd320947ff | 267 | |
open4416 | 4:b9dd320947ff | 268 | |
open4416 | 4:b9dd320947ff | 269 | //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓read_IMU funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// |
open4416 | 3:502b83f7761c | 270 | void read_IMU(void) //read IMU data give raw data |
open4416 | 0:d68e088dfbcd | 271 | { |
open4416 | 3:502b83f7761c | 272 | //Wx |
open4416 | 3:502b83f7761c | 273 | SPI_CSG = 0; //start spi talking Wx |
open4416 | 3:502b83f7761c | 274 | spi.write(0xE8); //read B11101000 read/multi/address |
open4416 | 3:502b83f7761c | 275 | low_byte = spi.write(0); |
open4416 | 3:502b83f7761c | 276 | high_byte = spi.write(0); |
open4416 | 3:502b83f7761c | 277 | Buff = high_byte << 8 |low_byte; |
open4416 | 3:502b83f7761c | 278 | SPI_CSG = 1; //end spi talking |
open4416 | 4:b9dd320947ff | 279 | // Wx = Buff * Gpx + Gdx; |
open4416 | 4:b9dd320947ff | 280 | Wx = lpf(Buff * Gpx + Gdx, Wx, 18.0f); |
open4416 | 3:502b83f7761c | 281 | //Wy |
open4416 | 3:502b83f7761c | 282 | SPI_CSG = 0; //start spi talking Wx |
open4416 | 3:502b83f7761c | 283 | spi.write(0xEA); //read B11101010 read/multi/address |
open4416 | 3:502b83f7761c | 284 | low_byte = spi.write(0); |
open4416 | 3:502b83f7761c | 285 | high_byte = spi.write(0); |
open4416 | 3:502b83f7761c | 286 | Buff = high_byte << 8 |low_byte; |
open4416 | 3:502b83f7761c | 287 | SPI_CSG = 1; //end spi talking |
open4416 | 4:b9dd320947ff | 288 | // Wy = Buff * Gpy + Gdy; |
open4416 | 4:b9dd320947ff | 289 | Wy = lpf(Buff * Gpy + Gdy, Wy, 18.0f); |
open4416 | 3:502b83f7761c | 290 | //Wz |
open4416 | 3:502b83f7761c | 291 | SPI_CSG = 0; //start spi talking Wx |
open4416 | 3:502b83f7761c | 292 | spi.write(0xEC); //read B11101100 read/multi/address |
open4416 | 3:502b83f7761c | 293 | low_byte = spi.write(0); |
open4416 | 3:502b83f7761c | 294 | high_byte = spi.write(0); |
open4416 | 3:502b83f7761c | 295 | Buff = high_byte << 8 |low_byte; |
open4416 | 3:502b83f7761c | 296 | SPI_CSG = 1; //end spi talking |
open4416 | 4:b9dd320947ff | 297 | // Wz = Buff * Gpz + Gdz; |
open4416 | 4:b9dd320947ff | 298 | Wz = lpf(Buff * Gpz + Gdz, Wz, 18.0f); |
open4416 | 3:502b83f7761c | 299 | //Ax |
open4416 | 3:502b83f7761c | 300 | SPI_CSXM = 0; //start spi talking Ax |
open4416 | 2:0d90c0436797 | 301 | spi.write(0xE8); //read B11101000 read/multi/address |
open4416 | 2:0d90c0436797 | 302 | low_byte = spi.write(0); |
open4416 | 2:0d90c0436797 | 303 | high_byte = spi.write(0); |
open4416 | 3:502b83f7761c | 304 | Buff = high_byte << 8 |low_byte; |
open4416 | 3:502b83f7761c | 305 | SPI_CSXM = 1; //end spi talking |
open4416 | 4:b9dd320947ff | 306 | Ax = lpf(Buff*Apx + Adx, Ax, 15.0f); |
open4416 | 3:502b83f7761c | 307 | //Ay |
open4416 | 3:502b83f7761c | 308 | SPI_CSXM = 0; //start spi talking Ax |
open4416 | 3:502b83f7761c | 309 | spi.write(0xEA); //read B11101010 read/multi/address |
open4416 | 3:502b83f7761c | 310 | low_byte = spi.write(0); |
open4416 | 3:502b83f7761c | 311 | high_byte = spi.write(0); |
open4416 | 3:502b83f7761c | 312 | Buff = high_byte << 8 |low_byte; |
open4416 | 3:502b83f7761c | 313 | SPI_CSXM = 1; //end spi talking |
open4416 | 4:b9dd320947ff | 314 | Ay = lpf(Buff*Apy + Ady, Ay, 15.0f); |
open4416 | 3:502b83f7761c | 315 | //Az |
open4416 | 3:502b83f7761c | 316 | SPI_CSXM = 0; //start spi talking Ax |
open4416 | 3:502b83f7761c | 317 | spi.write(0xEC); //read B11101100 read/multi/address |
open4416 | 3:502b83f7761c | 318 | low_byte = spi.write(0); |
open4416 | 3:502b83f7761c | 319 | high_byte = spi.write(0); |
open4416 | 3:502b83f7761c | 320 | Buff = high_byte << 8 |low_byte; |
open4416 | 3:502b83f7761c | 321 | SPI_CSXM = 1; //end spi talking |
open4416 | 4:b9dd320947ff | 322 | Az = lpf(Buff*Apz + Adz, Az, 15.0f); |
open4416 | 3:502b83f7761c | 323 | } |
open4416 | 3:502b83f7761c | 324 | //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of read_IMU funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// |
open4416 | 1:b42c3522a50a | 325 | |
open4416 | 1:b42c3522a50a | 326 | |
open4416 | 1:b42c3522a50a | 327 | |
open4416 | 3:502b83f7761c | 328 | //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓state_update funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// |
open4416 | 3:502b83f7761c | 329 | void state_update(void) //estimation of new attitude |
open4416 | 3:502b83f7761c | 330 | { |
open4416 | 3:502b83f7761c | 331 | //pridict |
open4416 | 3:502b83f7761c | 332 | gDIR[0][0] = gDIR[0][0] - (Wy*gDIR[0][2] - Wz*gDIR[0][1])*dt; |
open4416 | 3:502b83f7761c | 333 | gDIR[0][1] = gDIR[0][1] - (Wz*gDIR[0][0] - Wx*gDIR[0][2])*dt; |
open4416 | 3:502b83f7761c | 334 | gDIR[0][2] = gDIR[0][2] - (Wx*gDIR[0][1] - Wy*gDIR[0][0])*dt; |
open4416 | 4:b9dd320947ff | 335 | |
open4416 | 3:502b83f7761c | 336 | //update |
open4416 | 4:b9dd320947ff | 337 | gDIR[0][0] = (1-Bet) * gDIR[0][0] + Bet * Ax; |
open4416 | 4:b9dd320947ff | 338 | gDIR[0][1] = (1-Bet) * gDIR[0][1] + Bet * Ay; |
open4416 | 4:b9dd320947ff | 339 | gDIR[0][2] = (1-Bet) * gDIR[0][2] + Bet * Az; |
open4416 | 4:b9dd320947ff | 340 | |
open4416 | 3:502b83f7761c | 341 | //nutralizing |
open4416 | 4:b9dd320947ff | 342 | gUnity = sqrt( gDIR[0][0]*gDIR[0][0] + gDIR[0][1]*gDIR[0][1] + gDIR[0][2]*gDIR[0][2] ); |
open4416 | 3:502b83f7761c | 343 | for(int i=0; i<3; i++) { |
open4416 | 3:502b83f7761c | 344 | gDIR[0][i] = gDIR[0][i] / gUnity; |
open4416 | 3:502b83f7761c | 345 | } |
open4416 | 4:b9dd320947ff | 346 | |
open4416 | 4:b9dd320947ff | 347 | //transfer |
open4416 | 4:b9dd320947ff | 348 | Ele_phy = asin(gDIR[0][0]); |
open4416 | 4:b9dd320947ff | 349 | Til_phy = asin(-gDIR[0][1] / cos(Ele_phy)); |
open4416 | 4:b9dd320947ff | 350 | // //test |
open4416 | 4:b9dd320947ff | 351 | // gDIR[0][0] = gDIR[0][0] + Wx*dt; |
open4416 | 4:b9dd320947ff | 352 | // gDIR[0][1] = gDIR[0][1] + Wy*dt; |
open4416 | 4:b9dd320947ff | 353 | // gDIR[0][2] = gDIR[0][2] + Wz*dt; |
open4416 | 0:d68e088dfbcd | 354 | } |
open4416 | 4:b9dd320947ff | 355 | //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of state_update funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// |
open4416 | 4:b9dd320947ff | 356 | |
open4416 | 4:b9dd320947ff | 357 | |
open4416 | 4:b9dd320947ff | 358 | |
open4416 | 4:b9dd320947ff | 359 | //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓RR funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// |
open4416 | 4:b9dd320947ff | 360 | void RR() //status generator |
open4416 | 4:b9dd320947ff | 361 | { |
open4416 | 4:b9dd320947ff | 362 | Ele_phy_int = Ele_phy_int + Ele_phy*dt; |
open4416 | 4:b9dd320947ff | 363 | Til_phy_int = Til_phy_int + Til_phy*dt; |
open4416 | 4:b9dd320947ff | 364 | Ele_phy_int = constrain(Ele_phy_int,-0.01f,0.01f); |
open4416 | 4:b9dd320947ff | 365 | Til_phy_int = constrain(Til_phy_int,-0.01f,0.01f); |
open4416 | 4:b9dd320947ff | 366 | |
open4416 | 4:b9dd320947ff | 367 | VEC[0][4] = -9.5f*Ele_phy - 0.17f*Wy - 60.0f*Ele_phy_int; |
open4416 | 4:b9dd320947ff | 368 | VEC[0][5] = -9.5f*Til_phy - 0.17f*Wx - 60.0f*Til_phy_int; |
open4416 | 4:b9dd320947ff | 369 | |
open4416 | 4:b9dd320947ff | 370 | VEC[0][4] = constrain(VEC[0][4],-0.30f,0.30f); |
open4416 | 4:b9dd320947ff | 371 | VEC[0][5] = constrain(VEC[0][5],-0.30f,0.30f); |
open4416 | 4:b9dd320947ff | 372 | } |
open4416 | 4:b9dd320947ff | 373 | //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of RR funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// |
open4416 | 4:b9dd320947ff | 374 | |
open4416 | 4:b9dd320947ff | 375 | |
open4416 | 4:b9dd320947ff | 376 | |
open4416 | 4:b9dd320947ff | 377 | //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓stPF_lenth_uni funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// |
open4416 | 4:b9dd320947ff | 378 | void stPF_lenth_uni() //referenve generator |
open4416 | 4:b9dd320947ff | 379 | { |
open4416 | 4:b9dd320947ff | 380 | Rtot[0][0] = cos(VEC[0][4])*cos(VEC[0][3]); |
open4416 | 4:b9dd320947ff | 381 | Rtot[0][1] = - cos(VEC[0][5])*sin(VEC[0][3]) + cos(VEC[0][3])*sin(VEC[0][4])*sin(VEC[0][5]); |
open4416 | 4:b9dd320947ff | 382 | Rtot[0][2] = sin(VEC[0][5])*sin(VEC[0][3]) + cos(VEC[0][5])*cos(VEC[0][3])*sin(VEC[0][4]); |
open4416 | 4:b9dd320947ff | 383 | Rtot[1][0] = cos(VEC[0][4])*sin(VEC[0][3]); |
open4416 | 4:b9dd320947ff | 384 | Rtot[1][1] = cos(VEC[0][5])*cos(VEC[0][3]) + sin(VEC[0][4])*sin(VEC[0][5])*sin(VEC[0][3]); |
open4416 | 4:b9dd320947ff | 385 | Rtot[1][2] = - cos(VEC[0][3])*sin(VEC[0][5]) + cos(VEC[0][5])*sin(VEC[0][4])*sin(VEC[0][3]); |
open4416 | 4:b9dd320947ff | 386 | Rtot[2][0] = - sin(VEC[0][4]); |
open4416 | 4:b9dd320947ff | 387 | Rtot[2][1] = cos(VEC[0][4])*sin(VEC[0][5]); |
open4416 | 4:b9dd320947ff | 388 | Rtot[2][2] = cos(VEC[0][4])*cos(VEC[0][5]); |
open4416 | 4:b9dd320947ff | 389 | |
open4416 | 4:b9dd320947ff | 390 | for(int j=0; j<6; j++) { //reset C |
open4416 | 4:b9dd320947ff | 391 | for(int i=0; i<3; i++) { |
open4416 | 4:b9dd320947ff | 392 | C[i][j] = 0; |
open4416 | 4:b9dd320947ff | 393 | } |
open4416 | 4:b9dd320947ff | 394 | } |
open4416 | 4:b9dd320947ff | 395 | |
open4416 | 4:b9dd320947ff | 396 | for(int i=0; i<6; i++) { //(RR.' * B) |
open4416 | 4:b9dd320947ff | 397 | for(int j=0; j<3; j++) { |
open4416 | 4:b9dd320947ff | 398 | for(int k=0; k<3; k++) { |
open4416 | 4:b9dd320947ff | 399 | C[j][i] = Rtot[j][k] * B[k][i] + C[j][i]; |
open4416 | 4:b9dd320947ff | 400 | } |
open4416 | 4:b9dd320947ff | 401 | } |
open4416 | 4:b9dd320947ff | 402 | } |
open4416 | 4:b9dd320947ff | 403 | |
open4416 | 4:b9dd320947ff | 404 | for(int j=0; j<6; j++) { //+ [X,Y,Z] |
open4416 | 4:b9dd320947ff | 405 | for(int i=0; i<3; i++) { |
open4416 | 4:b9dd320947ff | 406 | C[i][j] = C[i][j] + VEC[0][i]; |
open4416 | 4:b9dd320947ff | 407 | } |
open4416 | 4:b9dd320947ff | 408 | } |
open4416 | 4:b9dd320947ff | 409 | for(int i=0; i<6; i++) { |
open4416 | 4:b9dd320947ff | 410 | float X = C[0][i]-A[0][i]; |
open4416 | 4:b9dd320947ff | 411 | float Y = C[1][i]-A[1][i]; |
open4416 | 4:b9dd320947ff | 412 | float Z = C[2][i]-A[2][i]; |
open4416 | 4:b9dd320947ff | 413 | L[0][i] = sqrt(X*X+Y*Y+Z*Z); |
open4416 | 4:b9dd320947ff | 414 | } |
open4416 | 4:b9dd320947ff | 415 | } |
open4416 | 4:b9dd320947ff | 416 | //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of stPF_lenth_uni funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// |
open4416 | 4:b9dd320947ff | 417 | |
open4416 | 4:b9dd320947ff | 418 | |
open4416 | 4:b9dd320947ff | 419 | |
open4416 | 4:b9dd320947ff | 420 | //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓stPF_travle_R funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// |
open4416 | 4:b9dd320947ff | 421 | void stPF_travle_R() //lenth to pwm pulse width |
open4416 | 4:b9dd320947ff | 422 | { |
open4416 | 4:b9dd320947ff | 423 | for(int i=0; i<6; i++) { |
open4416 | 4:b9dd320947ff | 424 | A_R = ( L[0][i] - L90 + sqrt( Llink*Llink - Larm*Larm ) ) /Larm; |
open4416 | 4:b9dd320947ff | 425 | B_R = Llink/Larm; |
open4416 | 4:b9dd320947ff | 426 | C_R = ( A_R*A_R - B_R*B_R + 1 ) /(A_R*2); |
open4416 | 4:b9dd320947ff | 427 | PWM[0][i] = -asin(C_R)*573 + PWM_base[0][i]; |
open4416 | 4:b9dd320947ff | 428 | } |
open4416 | 4:b9dd320947ff | 429 | } |
open4416 | 4:b9dd320947ff | 430 | //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of stPF_travle_R funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// |
open4416 | 4:b9dd320947ff | 431 | |
open4416 | 4:b9dd320947ff | 432 | |
open4416 | 4:b9dd320947ff | 433 | |
open4416 | 4:b9dd320947ff | 434 | //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓lpf funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// |
open4416 | 4:b9dd320947ff | 435 | float lpf(float input, float output_old, float frequency) |
open4416 | 4:b9dd320947ff | 436 | { |
open4416 | 4:b9dd320947ff | 437 | float output = 0; |
open4416 | 4:b9dd320947ff | 438 | output = (output_old + frequency*dt*input) / (1 + frequency*dt); |
open4416 | 4:b9dd320947ff | 439 | return output; |
open4416 | 4:b9dd320947ff | 440 | } |
open4416 | 4:b9dd320947ff | 441 | //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of lpf funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// |