SHENG-HEN HSIEH / Mbed 2 deprecated LSM9DS0_STM32compatible

works fine on STM

Dependencies:   mbed

Fork of Sample_manerine_SPI_LSM9DS0 by SHENG-HEN HSIEH

main.cpp@6:c2efb0a3a543, 2016-09-25 (annotated)

Committer:
open4416
Date:
Sun Sep 25 06:04:57 2016 +0000
Revision:
6:c2efb0a3a543
Parent:
5:2f0633d8fc20
Child:
7:f53b52e23818
stewart_platform_selfbalance

Who changed what in which revision?

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