SHENG-HEN HSIEH
/
LSM9DS0_STM32compatible
works fine on STM
Fork of Sample_manerine_SPI_LSM9DS0 by
main.cpp@5:2f0633d8fc20, 2016-09-17 (annotated)
- Committer:
- open4416
- Date:
- Sat Sep 17 13:24:17 2016 +0000
- Revision:
- 5:2f0633d8fc20
- Parent:
- 4:b9dd320947ff
- Child:
- 6:c2efb0a3a543
2016/9/17
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 | 5:2f0633d8fc20 | 8 | #define dt 0.005f |
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 | 5:2f0633d8fc20 | 53 | float Z0 = 2.40; //as mid point hight |
open4416 | 5:2f0633d8fc20 | 54 | float Z_dis = -0.1; //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 | 5:2f0633d8fc20 | 107 | float Bet = 0.002; //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 | 5:2f0633d8fc20 | 116 | float Ele_control = 0.0; |
open4416 | 5:2f0633d8fc20 | 117 | float Til_control = 0.0; |
open4416 | 1:b42c3522a50a | 118 | //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of Varible registor↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// |
open4416 | 1:b42c3522a50a | 119 | |
open4416 | 0:d68e088dfbcd | 120 | |
open4416 | 0:d68e088dfbcd | 121 | |
open4416 | 1:b42c3522a50a | 122 | //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓Function registor↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// |
open4416 | 4:b9dd320947ff | 123 | void init_TIMER(); //set TT_main() rate |
open4416 | 4:b9dd320947ff | 124 | void TT_main(); //timebase function rated by TT |
open4416 | 4:b9dd320947ff | 125 | void init_IO(); //initialize IO state |
open4416 | 4:b9dd320947ff | 126 | void init_IMU(); //initialize IMU |
open4416 | 4:b9dd320947ff | 127 | void init_Gdrift(); //read Gdrift at start up |
open4416 | 4:b9dd320947ff | 128 | void read_IMU(); //read IMU data give raw data |
open4416 | 4:b9dd320947ff | 129 | void state_update(); //estimation of new attitude |
open4416 | 4:b9dd320947ff | 130 | |
open4416 | 4:b9dd320947ff | 131 | void RR(); //status generator |
open4416 | 4:b9dd320947ff | 132 | void stPF_lenth_uni(); //referenve generator |
open4416 | 4:b9dd320947ff | 133 | void stPF_travle_R(); //lenth to pwm pulse width |
open4416 | 4:b9dd320947ff | 134 | |
open4416 | 4:b9dd320947ff | 135 | float lpf(float input, float output_old, float frequency); //lpf discrete |
open4416 | 1:b42c3522a50a | 136 | //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of Function registor↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// |
open4416 | 1:b42c3522a50a | 137 | |
open4416 | 1:b42c3522a50a | 138 | |
open4416 | 1:b42c3522a50a | 139 | |
open4416 | 1:b42c3522a50a | 140 | //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓main funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// |
open4416 | 0:d68e088dfbcd | 141 | int main() |
open4416 | 0:d68e088dfbcd | 142 | { |
open4416 | 4:b9dd320947ff | 143 | pc.baud(115200); //set baud rate |
open4416 | 4:b9dd320947ff | 144 | wait_ms(1000); |
open4416 | 0:d68e088dfbcd | 145 | |
open4416 | 4:b9dd320947ff | 146 | init_IO(); //initialized value |
open4416 | 4:b9dd320947ff | 147 | init_IMU(); //initialize IMU |
open4416 | 4:b9dd320947ff | 148 | init_Gdrift(); //read Gdrift at start up |
open4416 | 4:b9dd320947ff | 149 | wait_ms(1000); |
open4416 | 4:b9dd320947ff | 150 | |
open4416 | 4:b9dd320947ff | 151 | init_TIMER(); //start TT_main |
open4416 | 4:b9dd320947ff | 152 | |
open4416 | 4:b9dd320947ff | 153 | while(1) { //main() loop |
open4416 | 4:b9dd320947ff | 154 | if(count >= NN) { //check if main working |
open4416 | 0:d68e088dfbcd | 155 | count=0; |
open4416 | 0:d68e088dfbcd | 156 | led = !led; |
open4416 | 0:d68e088dfbcd | 157 | } |
open4416 | 1:b42c3522a50a | 158 | } |
open4416 | 0:d68e088dfbcd | 159 | |
open4416 | 0:d68e088dfbcd | 160 | } |
open4416 | 1:b42c3522a50a | 161 | //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of main funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// |
open4416 | 0:d68e088dfbcd | 162 | |
open4416 | 0:d68e088dfbcd | 163 | |
open4416 | 0:d68e088dfbcd | 164 | |
open4416 | 3:502b83f7761c | 165 | //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓Timebase funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// |
open4416 | 3:502b83f7761c | 166 | void init_TIMER() //set TT_main{} rate |
open4416 | 3:502b83f7761c | 167 | { |
open4416 | 3:502b83f7761c | 168 | TT.attach_us(&TT_main, Rms); |
open4416 | 3:502b83f7761c | 169 | } |
open4416 | 3:502b83f7761c | 170 | void TT_main() //interrupt function by TT |
open4416 | 3:502b83f7761c | 171 | { |
open4416 | 3:502b83f7761c | 172 | TT_ext = !TT_ext; //indicate TT_main() function working |
open4416 | 3:502b83f7761c | 173 | count = count+1; //one second counter |
open4416 | 3:502b83f7761c | 174 | |
open4416 | 3:502b83f7761c | 175 | read_IMU(); //read IMU data give raw data |
open4416 | 3:502b83f7761c | 176 | state_update(); //estimation of new attitude |
open4416 | 3:502b83f7761c | 177 | |
open4416 | 4:b9dd320947ff | 178 | RR(); //VEC generated |
open4416 | 4:b9dd320947ff | 179 | stPF_lenth_uni(); //L generated |
open4416 | 4:b9dd320947ff | 180 | stPF_travle_R(); //PWM generated |
open4416 | 4:b9dd320947ff | 181 | |
open4416 | 4:b9dd320947ff | 182 | for(int i=0; i<6; i++) { //safty constrain |
open4416 | 5:2f0633d8fc20 | 183 | PWM[0][i] = constrain(PWM[0][i],700,2100); |
open4416 | 4:b9dd320947ff | 184 | } |
open4416 | 4:b9dd320947ff | 185 | |
open4416 | 4:b9dd320947ff | 186 | Drive1.pulsewidth_us(PWM[0][0]); //drive command |
open4416 | 4:b9dd320947ff | 187 | Drive2.pulsewidth_us(PWM[0][1]); |
open4416 | 4:b9dd320947ff | 188 | Drive3.pulsewidth_us(PWM[0][2]); |
open4416 | 4:b9dd320947ff | 189 | Drive4.pulsewidth_us(PWM[0][3]); |
open4416 | 4:b9dd320947ff | 190 | Drive5.pulsewidth_us(PWM[0][4]); |
open4416 | 4:b9dd320947ff | 191 | Drive6.pulsewidth_us(PWM[0][5]); |
open4416 | 4:b9dd320947ff | 192 | |
open4416 | 4:b9dd320947ff | 193 | //for Serial-Oscilloscope |
open4416 | 4:b9dd320947ff | 194 | // pc.printf("%.3f\r", Bet); |
open4416 | 5:2f0633d8fc20 | 195 | // pc.printf("%.3f,%.3f\r", Ele_phy, Til_phy); |
open4416 | 4:b9dd320947ff | 196 | // pc.printf("%.2f,%.2f\r", VEC[0][4], VEC[0][5]); |
open4416 | 5:2f0633d8fc20 | 197 | pc.printf("%.2f,%.2f,%.2f\r", Ax, Ay, Az); |
open4416 | 4:b9dd320947ff | 198 | // pc.printf("%.3f,%.3f,%.3f\r", gDIR[0][0], gDIR[0][1], gDIR[0][2]); |
open4416 | 3:502b83f7761c | 199 | } |
open4416 | 4:b9dd320947ff | 200 | //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of Timebase funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// |
open4416 | 3:502b83f7761c | 201 | |
open4416 | 3:502b83f7761c | 202 | |
open4416 | 3:502b83f7761c | 203 | |
open4416 | 4:b9dd320947ff | 204 | //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓init_IO funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// |
open4416 | 1:b42c3522a50a | 205 | void init_IO(void) //initialize |
open4416 | 1:b42c3522a50a | 206 | { |
open4416 | 1:b42c3522a50a | 207 | TT_ext = 0; |
open4416 | 1:b42c3522a50a | 208 | led = 1; |
open4416 | 1:b42c3522a50a | 209 | } |
open4416 | 4:b9dd320947ff | 210 | //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of init_IO funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// |
open4416 | 0:d68e088dfbcd | 211 | |
open4416 | 0:d68e088dfbcd | 212 | |
open4416 | 0:d68e088dfbcd | 213 | |
open4416 | 4:b9dd320947ff | 214 | //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓init_IMU funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// |
open4416 | 3:502b83f7761c | 215 | void init_IMU(void) //initialize |
open4416 | 0:d68e088dfbcd | 216 | { |
open4416 | 3:502b83f7761c | 217 | //gloable config |
open4416 | 1:b42c3522a50a | 218 | SPI_CSXM = 1; //high as init for disable SPI |
open4416 | 1:b42c3522a50a | 219 | SPI_CSG = 1; |
open4416 | 1:b42c3522a50a | 220 | spi.format(8, 0); //byte width, spi mode |
open4416 | 1:b42c3522a50a | 221 | spi.frequency(4000000); //8MHz |
open4416 | 1:b42c3522a50a | 222 | |
open4416 | 3:502b83f7761c | 223 | //for GYRO config |
open4416 | 1:b42c3522a50a | 224 | SPI_CSG = 0; //start spi talking |
open4416 | 1:b42c3522a50a | 225 | spi.write(CTRL_REG1_G); |
open4416 | 1:b42c3522a50a | 226 | spi.write(0x9F); //data rate 380 Hz/ cut off 25 Hz |
open4416 | 1:b42c3522a50a | 227 | SPI_CSG = 1; //end spi talking |
open4416 | 1:b42c3522a50a | 228 | |
open4416 | 1:b42c3522a50a | 229 | SPI_CSG = 0; //start spi talking |
open4416 | 1:b42c3522a50a | 230 | spi.write(CTRL_REG4_G); |
open4416 | 1:b42c3522a50a | 231 | spi.write(0x10); //Scle 500dps |
open4416 | 1:b42c3522a50a | 232 | SPI_CSG = 1; //end spi talking |
open4416 | 1:b42c3522a50a | 233 | |
open4416 | 3:502b83f7761c | 234 | //for ACC config |
open4416 | 1:b42c3522a50a | 235 | SPI_CSXM = 0; //start spi talking |
open4416 | 1:b42c3522a50a | 236 | spi.write(CTRL_REG1_XM); |
open4416 | 1:b42c3522a50a | 237 | spi.write(0x87); //data rate 400 Hz/ Enable |
open4416 | 1:b42c3522a50a | 238 | SPI_CSXM = 1; //end spi talking |
open4416 | 1:b42c3522a50a | 239 | |
open4416 | 1:b42c3522a50a | 240 | SPI_CSXM = 0; //start spi talking |
open4416 | 1:b42c3522a50a | 241 | spi.write(CTRL_REG2_XM); |
open4416 | 3:502b83f7761c | 242 | spi.write(0xC8); //cut off 50 Hz/ Scale +-4g |
open4416 | 1:b42c3522a50a | 243 | SPI_CSXM = 1; //end spi talking |
open4416 | 0:d68e088dfbcd | 244 | } |
open4416 | 4:b9dd320947ff | 245 | //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of init_IMU funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// |
open4416 | 0:d68e088dfbcd | 246 | |
open4416 | 0:d68e088dfbcd | 247 | |
open4416 | 0:d68e088dfbcd | 248 | |
open4416 | 4:b9dd320947ff | 249 | //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓init_Gdrift funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// |
open4416 | 4:b9dd320947ff | 250 | void init_Gdrift(void) //initialize |
open4416 | 4:b9dd320947ff | 251 | { |
open4416 | 4:b9dd320947ff | 252 | for(int i=0; i<1000; i++) { |
open4416 | 4:b9dd320947ff | 253 | read_IMU(); //note Gdrift = 0 at this moment |
open4416 | 4:b9dd320947ff | 254 | gDIR[0][0] = gDIR[0][0] - Wx; |
open4416 | 4:b9dd320947ff | 255 | gDIR[0][1] = gDIR[0][1] - Wy; |
open4416 | 4:b9dd320947ff | 256 | gDIR[0][2] = gDIR[0][2] - Wz; |
open4416 | 4:b9dd320947ff | 257 | wait_ms(2); |
open4416 | 4:b9dd320947ff | 258 | } |
open4416 | 4:b9dd320947ff | 259 | Gdx = gDIR[0][0] /1000.0f; |
open4416 | 4:b9dd320947ff | 260 | Gdy = gDIR[0][1] /1000.0f; |
open4416 | 4:b9dd320947ff | 261 | Gdz = gDIR[0][2] /1000.0f; |
open4416 | 4:b9dd320947ff | 262 | // pc.printf("%.4f,%.4f,%.4f\r", Gdx, Gdy, Gdz); |
open4416 | 4:b9dd320947ff | 263 | gDIR[0][0] = 0; |
open4416 | 4:b9dd320947ff | 264 | gDIR[0][1] = 0; |
open4416 | 4:b9dd320947ff | 265 | gDIR[0][2] = -1; |
open4416 | 4:b9dd320947ff | 266 | } |
open4416 | 4:b9dd320947ff | 267 | //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of init_Gdrift funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// |
open4416 | 4:b9dd320947ff | 268 | |
open4416 | 4:b9dd320947ff | 269 | |
open4416 | 4:b9dd320947ff | 270 | |
open4416 | 4:b9dd320947ff | 271 | //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓read_IMU funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// |
open4416 | 3:502b83f7761c | 272 | void read_IMU(void) //read IMU data give raw data |
open4416 | 0:d68e088dfbcd | 273 | { |
open4416 | 3:502b83f7761c | 274 | //Wx |
open4416 | 3:502b83f7761c | 275 | SPI_CSG = 0; //start spi talking Wx |
open4416 | 3:502b83f7761c | 276 | spi.write(0xE8); //read B11101000 read/multi/address |
open4416 | 3:502b83f7761c | 277 | low_byte = spi.write(0); |
open4416 | 3:502b83f7761c | 278 | high_byte = spi.write(0); |
open4416 | 3:502b83f7761c | 279 | Buff = high_byte << 8 |low_byte; |
open4416 | 3:502b83f7761c | 280 | SPI_CSG = 1; //end spi talking |
open4416 | 4:b9dd320947ff | 281 | // Wx = Buff * Gpx + Gdx; |
open4416 | 5:2f0633d8fc20 | 282 | Wx = lpf(Buff * Gpx + Gdx, Wx, 25.0f); |
open4416 | 3:502b83f7761c | 283 | //Wy |
open4416 | 3:502b83f7761c | 284 | SPI_CSG = 0; //start spi talking Wx |
open4416 | 3:502b83f7761c | 285 | spi.write(0xEA); //read B11101010 read/multi/address |
open4416 | 3:502b83f7761c | 286 | low_byte = spi.write(0); |
open4416 | 3:502b83f7761c | 287 | high_byte = spi.write(0); |
open4416 | 3:502b83f7761c | 288 | Buff = high_byte << 8 |low_byte; |
open4416 | 3:502b83f7761c | 289 | SPI_CSG = 1; //end spi talking |
open4416 | 4:b9dd320947ff | 290 | // Wy = Buff * Gpy + Gdy; |
open4416 | 5:2f0633d8fc20 | 291 | Wy = lpf(Buff * Gpy + Gdy, Wy, 25.0f); |
open4416 | 3:502b83f7761c | 292 | //Wz |
open4416 | 3:502b83f7761c | 293 | SPI_CSG = 0; //start spi talking Wx |
open4416 | 3:502b83f7761c | 294 | spi.write(0xEC); //read B11101100 read/multi/address |
open4416 | 3:502b83f7761c | 295 | low_byte = spi.write(0); |
open4416 | 3:502b83f7761c | 296 | high_byte = spi.write(0); |
open4416 | 3:502b83f7761c | 297 | Buff = high_byte << 8 |low_byte; |
open4416 | 3:502b83f7761c | 298 | SPI_CSG = 1; //end spi talking |
open4416 | 4:b9dd320947ff | 299 | // Wz = Buff * Gpz + Gdz; |
open4416 | 5:2f0633d8fc20 | 300 | Wz = lpf(Buff * Gpz + Gdz, Wz, 25.0f); |
open4416 | 3:502b83f7761c | 301 | //Ax |
open4416 | 3:502b83f7761c | 302 | SPI_CSXM = 0; //start spi talking Ax |
open4416 | 2:0d90c0436797 | 303 | spi.write(0xE8); //read B11101000 read/multi/address |
open4416 | 2:0d90c0436797 | 304 | low_byte = spi.write(0); |
open4416 | 2:0d90c0436797 | 305 | high_byte = spi.write(0); |
open4416 | 3:502b83f7761c | 306 | Buff = high_byte << 8 |low_byte; |
open4416 | 3:502b83f7761c | 307 | SPI_CSXM = 1; //end spi talking |
open4416 | 4:b9dd320947ff | 308 | Ax = lpf(Buff*Apx + Adx, Ax, 15.0f); |
open4416 | 3:502b83f7761c | 309 | //Ay |
open4416 | 3:502b83f7761c | 310 | SPI_CSXM = 0; //start spi talking Ax |
open4416 | 3:502b83f7761c | 311 | spi.write(0xEA); //read B11101010 read/multi/address |
open4416 | 3:502b83f7761c | 312 | low_byte = spi.write(0); |
open4416 | 3:502b83f7761c | 313 | high_byte = spi.write(0); |
open4416 | 3:502b83f7761c | 314 | Buff = high_byte << 8 |low_byte; |
open4416 | 3:502b83f7761c | 315 | SPI_CSXM = 1; //end spi talking |
open4416 | 4:b9dd320947ff | 316 | Ay = lpf(Buff*Apy + Ady, Ay, 15.0f); |
open4416 | 3:502b83f7761c | 317 | //Az |
open4416 | 3:502b83f7761c | 318 | SPI_CSXM = 0; //start spi talking Ax |
open4416 | 3:502b83f7761c | 319 | spi.write(0xEC); //read B11101100 read/multi/address |
open4416 | 3:502b83f7761c | 320 | low_byte = spi.write(0); |
open4416 | 3:502b83f7761c | 321 | high_byte = spi.write(0); |
open4416 | 3:502b83f7761c | 322 | Buff = high_byte << 8 |low_byte; |
open4416 | 3:502b83f7761c | 323 | SPI_CSXM = 1; //end spi talking |
open4416 | 4:b9dd320947ff | 324 | Az = lpf(Buff*Apz + Adz, Az, 15.0f); |
open4416 | 3:502b83f7761c | 325 | } |
open4416 | 3:502b83f7761c | 326 | //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of read_IMU funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// |
open4416 | 1:b42c3522a50a | 327 | |
open4416 | 1:b42c3522a50a | 328 | |
open4416 | 1:b42c3522a50a | 329 | |
open4416 | 3:502b83f7761c | 330 | //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓state_update funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// |
open4416 | 3:502b83f7761c | 331 | void state_update(void) //estimation of new attitude |
open4416 | 3:502b83f7761c | 332 | { |
open4416 | 3:502b83f7761c | 333 | //pridict |
open4416 | 3:502b83f7761c | 334 | gDIR[0][0] = gDIR[0][0] - (Wy*gDIR[0][2] - Wz*gDIR[0][1])*dt; |
open4416 | 3:502b83f7761c | 335 | gDIR[0][1] = gDIR[0][1] - (Wz*gDIR[0][0] - Wx*gDIR[0][2])*dt; |
open4416 | 3:502b83f7761c | 336 | gDIR[0][2] = gDIR[0][2] - (Wx*gDIR[0][1] - Wy*gDIR[0][0])*dt; |
open4416 | 4:b9dd320947ff | 337 | |
open4416 | 3:502b83f7761c | 338 | //update |
open4416 | 4:b9dd320947ff | 339 | gDIR[0][0] = (1-Bet) * gDIR[0][0] + Bet * Ax; |
open4416 | 4:b9dd320947ff | 340 | gDIR[0][1] = (1-Bet) * gDIR[0][1] + Bet * Ay; |
open4416 | 4:b9dd320947ff | 341 | gDIR[0][2] = (1-Bet) * gDIR[0][2] + Bet * Az; |
open4416 | 4:b9dd320947ff | 342 | |
open4416 | 3:502b83f7761c | 343 | //nutralizing |
open4416 | 4:b9dd320947ff | 344 | gUnity = sqrt( gDIR[0][0]*gDIR[0][0] + gDIR[0][1]*gDIR[0][1] + gDIR[0][2]*gDIR[0][2] ); |
open4416 | 3:502b83f7761c | 345 | for(int i=0; i<3; i++) { |
open4416 | 3:502b83f7761c | 346 | gDIR[0][i] = gDIR[0][i] / gUnity; |
open4416 | 3:502b83f7761c | 347 | } |
open4416 | 4:b9dd320947ff | 348 | |
open4416 | 4:b9dd320947ff | 349 | //transfer |
open4416 | 4:b9dd320947ff | 350 | Ele_phy = asin(gDIR[0][0]); |
open4416 | 4:b9dd320947ff | 351 | Til_phy = asin(-gDIR[0][1] / cos(Ele_phy)); |
open4416 | 4:b9dd320947ff | 352 | // //test |
open4416 | 4:b9dd320947ff | 353 | // gDIR[0][0] = gDIR[0][0] + Wx*dt; |
open4416 | 4:b9dd320947ff | 354 | // gDIR[0][1] = gDIR[0][1] + Wy*dt; |
open4416 | 4:b9dd320947ff | 355 | // gDIR[0][2] = gDIR[0][2] + Wz*dt; |
open4416 | 0:d68e088dfbcd | 356 | } |
open4416 | 4:b9dd320947ff | 357 | //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of state_update funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// |
open4416 | 4:b9dd320947ff | 358 | |
open4416 | 4:b9dd320947ff | 359 | |
open4416 | 4:b9dd320947ff | 360 | |
open4416 | 4:b9dd320947ff | 361 | //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓RR funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// |
open4416 | 4:b9dd320947ff | 362 | void RR() //status generator |
open4416 | 4:b9dd320947ff | 363 | { |
open4416 | 4:b9dd320947ff | 364 | Ele_phy_int = Ele_phy_int + Ele_phy*dt; |
open4416 | 4:b9dd320947ff | 365 | Til_phy_int = Til_phy_int + Til_phy*dt; |
open4416 | 5:2f0633d8fc20 | 366 | Ele_phy_int = constrain(Ele_phy_int,-0.006f,0.006f); |
open4416 | 5:2f0633d8fc20 | 367 | Til_phy_int = constrain(Til_phy_int,-0.006f,0.006f); |
open4416 | 5:2f0633d8fc20 | 368 | |
open4416 | 5:2f0633d8fc20 | 369 | Ele_control = -3.5f*Ele_phy - 0.10f*Wy - 20.0f*Ele_phy_int; |
open4416 | 5:2f0633d8fc20 | 370 | Til_control = -3.5f*Til_phy - 0.10f*Wx - 20.0f*Til_phy_int; |
open4416 | 4:b9dd320947ff | 371 | |
open4416 | 5:2f0633d8fc20 | 372 | Ele_control = constrain(Ele_control,-0.35f,0.35f); |
open4416 | 5:2f0633d8fc20 | 373 | Til_control = constrain(Til_control,-0.35f,0.35f); |
open4416 | 4:b9dd320947ff | 374 | |
open4416 | 5:2f0633d8fc20 | 375 | VEC[0][4] = lpf(Ele_control, VEC[0][4], 20.0f); |
open4416 | 5:2f0633d8fc20 | 376 | VEC[0][5] = lpf(Til_control, VEC[0][5], 20.0f); |
open4416 | 5:2f0633d8fc20 | 377 | VEC[0][2] = Z0 -Z_dis; |
open4416 | 4:b9dd320947ff | 378 | } |
open4416 | 4:b9dd320947ff | 379 | //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of RR funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// |
open4416 | 4:b9dd320947ff | 380 | |
open4416 | 4:b9dd320947ff | 381 | |
open4416 | 4:b9dd320947ff | 382 | |
open4416 | 4:b9dd320947ff | 383 | //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓stPF_lenth_uni funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// |
open4416 | 4:b9dd320947ff | 384 | void stPF_lenth_uni() //referenve generator |
open4416 | 4:b9dd320947ff | 385 | { |
open4416 | 4:b9dd320947ff | 386 | Rtot[0][0] = cos(VEC[0][4])*cos(VEC[0][3]); |
open4416 | 4:b9dd320947ff | 387 | 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 | 388 | 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 | 389 | Rtot[1][0] = cos(VEC[0][4])*sin(VEC[0][3]); |
open4416 | 4:b9dd320947ff | 390 | 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 | 391 | 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 | 392 | Rtot[2][0] = - sin(VEC[0][4]); |
open4416 | 4:b9dd320947ff | 393 | Rtot[2][1] = cos(VEC[0][4])*sin(VEC[0][5]); |
open4416 | 4:b9dd320947ff | 394 | Rtot[2][2] = cos(VEC[0][4])*cos(VEC[0][5]); |
open4416 | 4:b9dd320947ff | 395 | |
open4416 | 4:b9dd320947ff | 396 | for(int j=0; j<6; j++) { //reset C |
open4416 | 4:b9dd320947ff | 397 | for(int i=0; i<3; i++) { |
open4416 | 4:b9dd320947ff | 398 | C[i][j] = 0; |
open4416 | 4:b9dd320947ff | 399 | } |
open4416 | 4:b9dd320947ff | 400 | } |
open4416 | 4:b9dd320947ff | 401 | |
open4416 | 4:b9dd320947ff | 402 | for(int i=0; i<6; i++) { //(RR.' * B) |
open4416 | 4:b9dd320947ff | 403 | for(int j=0; j<3; j++) { |
open4416 | 4:b9dd320947ff | 404 | for(int k=0; k<3; k++) { |
open4416 | 4:b9dd320947ff | 405 | C[j][i] = Rtot[j][k] * B[k][i] + C[j][i]; |
open4416 | 4:b9dd320947ff | 406 | } |
open4416 | 4:b9dd320947ff | 407 | } |
open4416 | 4:b9dd320947ff | 408 | } |
open4416 | 4:b9dd320947ff | 409 | |
open4416 | 4:b9dd320947ff | 410 | for(int j=0; j<6; j++) { //+ [X,Y,Z] |
open4416 | 4:b9dd320947ff | 411 | for(int i=0; i<3; i++) { |
open4416 | 4:b9dd320947ff | 412 | C[i][j] = C[i][j] + VEC[0][i]; |
open4416 | 4:b9dd320947ff | 413 | } |
open4416 | 4:b9dd320947ff | 414 | } |
open4416 | 4:b9dd320947ff | 415 | for(int i=0; i<6; i++) { |
open4416 | 4:b9dd320947ff | 416 | float X = C[0][i]-A[0][i]; |
open4416 | 4:b9dd320947ff | 417 | float Y = C[1][i]-A[1][i]; |
open4416 | 4:b9dd320947ff | 418 | float Z = C[2][i]-A[2][i]; |
open4416 | 4:b9dd320947ff | 419 | L[0][i] = sqrt(X*X+Y*Y+Z*Z); |
open4416 | 4:b9dd320947ff | 420 | } |
open4416 | 4:b9dd320947ff | 421 | } |
open4416 | 4:b9dd320947ff | 422 | //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of stPF_lenth_uni funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// |
open4416 | 4:b9dd320947ff | 423 | |
open4416 | 4:b9dd320947ff | 424 | |
open4416 | 4:b9dd320947ff | 425 | |
open4416 | 4:b9dd320947ff | 426 | //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓stPF_travle_R funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// |
open4416 | 4:b9dd320947ff | 427 | void stPF_travle_R() //lenth to pwm pulse width |
open4416 | 4:b9dd320947ff | 428 | { |
open4416 | 4:b9dd320947ff | 429 | for(int i=0; i<6; i++) { |
open4416 | 4:b9dd320947ff | 430 | A_R = ( L[0][i] - L90 + sqrt( Llink*Llink - Larm*Larm ) ) /Larm; |
open4416 | 4:b9dd320947ff | 431 | B_R = Llink/Larm; |
open4416 | 4:b9dd320947ff | 432 | C_R = ( A_R*A_R - B_R*B_R + 1 ) /(A_R*2); |
open4416 | 4:b9dd320947ff | 433 | PWM[0][i] = -asin(C_R)*573 + PWM_base[0][i]; |
open4416 | 4:b9dd320947ff | 434 | } |
open4416 | 4:b9dd320947ff | 435 | } |
open4416 | 4:b9dd320947ff | 436 | //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of stPF_travle_R funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// |
open4416 | 4:b9dd320947ff | 437 | |
open4416 | 4:b9dd320947ff | 438 | |
open4416 | 4:b9dd320947ff | 439 | |
open4416 | 4:b9dd320947ff | 440 | //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓lpf funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// |
open4416 | 4:b9dd320947ff | 441 | float lpf(float input, float output_old, float frequency) |
open4416 | 4:b9dd320947ff | 442 | { |
open4416 | 4:b9dd320947ff | 443 | float output = 0; |
open4416 | 4:b9dd320947ff | 444 | output = (output_old + frequency*dt*input) / (1 + frequency*dt); |
open4416 | 4:b9dd320947ff | 445 | return output; |
open4416 | 4:b9dd320947ff | 446 | } |
open4416 | 4:b9dd320947ff | 447 | //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of lpf funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// |