SHENG-HEN HSIEH
/
Sample_manerine_SPI_LSM9DS0
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Diff: main.cpp
- Revision:
- 4:b9dd320947ff
- Parent:
- 3:502b83f7761c
- Child:
- 5:2f0633d8fc20
diff -r 502b83f7761c -r b9dd320947ff main.cpp
--- a/main.cpp Mon Sep 05 14:43:43 2016 +0000
+++ b/main.cpp Wed Sep 14 05:57:55 2016 +0000
@@ -1,12 +1,15 @@
#include "mbed.h"
#include "LSM9DS0_SH.h"
-#define pi 3.141592f
+
+#define pi 3.141592f
#define d2r 0.01745329f
-#define Rms 5000
-#define dt 0.005f
+
+#define Rms 5000 //TT rate
+#define dt 0.003f
+#define NN 200
+
#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
-
//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓GPIO registor↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
//~~~structure~~~//
DigitalOut led(D13); //detection
@@ -17,17 +20,77 @@
DigitalOut SPI_CSXM(D6,1); //low for ACC/MAG enable
SPI spi(D4, D5, D3); //MOSI MISO SCLK
+//~~~Servo out~~~//
+PwmOut Drive1(D8); //control leg
+PwmOut Drive2(D9);
+PwmOut Drive3(D10);
+PwmOut Drive4(D11);
+PwmOut Drive5(D14);
+PwmOut Drive6(D15);
+
//~~~Serial~~~//
Serial pc(D1, D0); //Serial reg(TX RX)
-//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of GPIO registor↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
+//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of GPIO registor↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
-//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓Varible registor↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
+//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓Varible registor↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
//~~~globle~~~//
Ticker TT; //call a timer
int count = 0; //one second counter for extrenal led blink
+//~~~PWMreference~~~//
+const int pwm_mid = 1450; //+2080 for minimall lenght
+const int PWM_base[1][6] = { //desired six reference command at 0 deg
+ {pwm_mid+70,pwm_mid+10,pwm_mid+40,pwm_mid+60,pwm_mid+10,pwm_mid+30},
+// {pwm_mid,pwm_mid,pwm_mid,pwm_mid,pwm_mid,pwm_mid}, //only for debugging
+};
+int PWM[1][6] = { //desired six reference command by PWM_base + PWM
+ {0,0,0,0,0,0}, //transfer equaliyu 10us to 1 deg
+};
+
+//~~~RR varible~~~//
+float Z0 = 2.45; //as mid point hight
+float Z_dis = -0.087; //move rotation center
+
+//~~~stPF_lenth_uni varible~~~//
+const float alpha = 2.094f; //pair angle
+const float beta = 0.1309f; //couple angle
+const float A[3][6] = { //base cood
+ {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)},
+ {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)},
+ {0,0,0,0,0,0}
+};
+const float B[3][6] = { //top cood(static)
+ {cos(beta),cos(alpha-beta),cos(alpha+beta),cos(2*alpha-beta),cos(2*alpha+beta),cos(3*alpha-beta)},
+ {sin(beta),sin(alpha-beta),sin(alpha+beta),sin(2*alpha-beta),sin(2*alpha+beta),sin(3*alpha-beta)},
+ {Z_dis,Z_dis,Z_dis,Z_dis,Z_dis,Z_dis}
+};
+float C[3][6] = { //top cood(active)
+ {0,0,0,0,0,0},
+ {0,0,0,0,0,0},
+ {0,0,0,0,0,0}
+};
+float VEC[1][6] = { //desired six reference command
+ {0,0,2.52,0,0,0}, //X Y Z VEC[0][3] VEC[0][4] VEC[0][5]
+};
+float L[1][6] = { //desired six reference command
+ {0,0,0,0,0,0},
+};
+float Rtot[3][3] = { //RR'
+ {0,0,0},
+ {0,0,0},
+ {0,0,0}
+};
+
+//~~~stPF_tracle_R varible~~~//
+const float L90 = 2.422; //L under 90° (195 mm)
+const float Larm = 0.4969; //arm lenth (40 mm) b
+const float Llink = 0.8944; //link lenth (72 mm)
+float A_R = 0;
+float B_R = 0;
+float C_R = 0;
+
//~~~IMU_SPI~~~//
short low_byte = 0x00; //buffer
short high_byte = 0x00;
@@ -39,20 +102,35 @@
float Ay = 0.0;
float Az = 0.0;
float gDIR[1][3] = { //g vector's direction , required unitconstrain
- {0,0,-1}, //X Y Z
+ {0.0,0.0,0.0}, //X Y Z
};
-float gUnity = 0;
+float Bet = 0.005; //confidence of Acc data
+float gUnity = 0.0;
+float Gdx = 0.0;
+float Gdy = 0.0;
+float Gdz = 0.0;
+float Ele_phy = 0.0; //estimation of top plate attitide
+float Til_phy = 0.0;
+float Ele_phy_int = 0.0;
+float Til_phy_int = 0.0;
//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of Varible registor↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓Function registor↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
-void init_TIMER(); //set TT_main() rate
-void TT_main(); //timebase function rated by TT
-void init_IO(); //initialize IO state
-void init_IMU(); //initialize IMU
-void read_IMU(); //read IMU data give raw data
-void state_update(); //estimation of new attitude
+void init_TIMER(); //set TT_main() rate
+void TT_main(); //timebase function rated by TT
+void init_IO(); //initialize IO state
+void init_IMU(); //initialize IMU
+void init_Gdrift(); //read Gdrift at start up
+void read_IMU(); //read IMU data give raw data
+void state_update(); //estimation of new attitude
+
+void RR(); //status generator
+void stPF_lenth_uni(); //referenve generator
+void stPF_travle_R(); //lenth to pwm pulse width
+
+float lpf(float input, float output_old, float frequency); //lpf discrete
//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of Function registor↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
@@ -60,13 +138,18 @@
//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓main funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
int main()
{
- init_IO(); //initialized value
- init_IMU(); //initialize IMU
- init_TIMER(); //start TT_main
- pc.baud(115200); //set baud rate
+ pc.baud(115200); //set baud rate
+ wait_ms(1000);
- while(1) { //main() loop
- if(count >= 200) { //check if main working
+ init_IO(); //initialized value
+ init_IMU(); //initialize IMU
+ init_Gdrift(); //read Gdrift at start up
+ wait_ms(1000);
+
+ init_TIMER(); //start TT_main
+
+ while(1) { //main() loop
+ if(count >= NN) { //check if main working
count=0;
led = !led;
}
@@ -90,25 +173,43 @@
read_IMU(); //read IMU data give raw data
state_update(); //estimation of new attitude
- pc.printf("%.2f\t%.2f\t%.2f\n", gDIR[0][0], gDIR[0][1], gDIR[0][2]);
-// pc.printf("%.2f\t%.2f\n", Sele, Srol);
-// pc.printf("%.2f\n", Sx);
+ RR(); //VEC generated
+ stPF_lenth_uni(); //L generated
+ stPF_travle_R(); //PWM generated
+
+ for(int i=0; i<6; i++) { //safty constrain
+ PWM[0][i] = constrain(PWM[0][i],725,2025);
+ }
+
+ Drive1.pulsewidth_us(PWM[0][0]); //drive command
+ Drive2.pulsewidth_us(PWM[0][1]);
+ Drive3.pulsewidth_us(PWM[0][2]);
+ Drive4.pulsewidth_us(PWM[0][3]);
+ Drive5.pulsewidth_us(PWM[0][4]);
+ Drive6.pulsewidth_us(PWM[0][5]);
+
+//for Serial-Oscilloscope
+// pc.printf("%.3f\r", Bet);
+ pc.printf("%.3f,%.3f\r", Ele_phy, Til_phy);
+// pc.printf("%.2f,%.2f\r", VEC[0][4], VEC[0][5]);
+// pc.printf("%.2f,%.2f,%.2f\r", Ax, Ay, Az);
+// pc.printf("%.3f,%.3f,%.3f\r", gDIR[0][0], gDIR[0][1], gDIR[0][2]);
}
-//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of Timebase funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
+//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of Timebase funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
-//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓init_IO funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
+//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓init_IO funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
void init_IO(void) //initialize
{
TT_ext = 0;
led = 1;
}
-//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of init_IO funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
+//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of init_IO funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
-//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓init_IMU funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
+//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓init_IMU funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
void init_IMU(void) //initialize
{
//gloable config
@@ -139,11 +240,33 @@
spi.write(0xC8); //cut off 50 Hz/ Scale +-4g
SPI_CSXM = 1; //end spi talking
}
-//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of init_IMU funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
+//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of init_IMU funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
-//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓read_IMU funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
+//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓init_Gdrift funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
+void init_Gdrift(void) //initialize
+{
+ for(int i=0; i<1000; i++) {
+ read_IMU(); //note Gdrift = 0 at this moment
+ gDIR[0][0] = gDIR[0][0] - Wx;
+ gDIR[0][1] = gDIR[0][1] - Wy;
+ gDIR[0][2] = gDIR[0][2] - Wz;
+ wait_ms(2);
+ }
+ Gdx = gDIR[0][0] /1000.0f;
+ Gdy = gDIR[0][1] /1000.0f;
+ Gdz = gDIR[0][2] /1000.0f;
+// pc.printf("%.4f,%.4f,%.4f\r", Gdx, Gdy, Gdz);
+ gDIR[0][0] = 0;
+ gDIR[0][1] = 0;
+ gDIR[0][2] = -1;
+}
+//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of init_Gdrift funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
+
+
+
+//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓read_IMU funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
void read_IMU(void) //read IMU data give raw data
{
//Wx
@@ -153,7 +276,8 @@
high_byte = spi.write(0);
Buff = high_byte << 8 |low_byte;
SPI_CSG = 1; //end spi talking
- Wx = Buff * 2.663e-4; //1.526e-2 * d2r =
+// Wx = Buff * Gpx + Gdx;
+ Wx = lpf(Buff * Gpx + Gdx, Wx, 18.0f);
//Wy
SPI_CSG = 0; //start spi talking Wx
spi.write(0xEA); //read B11101010 read/multi/address
@@ -161,7 +285,8 @@
high_byte = spi.write(0);
Buff = high_byte << 8 |low_byte;
SPI_CSG = 1; //end spi talking
- Wy = Buff * 2.663e-4; //1.526e-2 * d2r =
+// Wy = Buff * Gpy + Gdy;
+ Wy = lpf(Buff * Gpy + Gdy, Wy, 18.0f);
//Wz
SPI_CSG = 0; //start spi talking Wx
spi.write(0xEC); //read B11101100 read/multi/address
@@ -169,7 +294,8 @@
high_byte = spi.write(0);
Buff = high_byte << 8 |low_byte;
SPI_CSG = 1; //end spi talking
- Wz = Buff * 2.663e-4; //1.526e-2 * d2r =
+// Wz = Buff * Gpz + Gdz;
+ Wz = lpf(Buff * Gpz + Gdz, Wz, 18.0f);
//Ax
SPI_CSXM = 0; //start spi talking Ax
spi.write(0xE8); //read B11101000 read/multi/address
@@ -177,7 +303,7 @@
high_byte = spi.write(0);
Buff = high_byte << 8 |low_byte;
SPI_CSXM = 1; //end spi talking
- Ax = Buff * 1.22e-4;
+ Ax = lpf(Buff*Apx + Adx, Ax, 15.0f);
//Ay
SPI_CSXM = 0; //start spi talking Ax
spi.write(0xEA); //read B11101010 read/multi/address
@@ -185,7 +311,7 @@
high_byte = spi.write(0);
Buff = high_byte << 8 |low_byte;
SPI_CSXM = 1; //end spi talking
- Ay = Buff * 1.22e-4;
+ Ay = lpf(Buff*Apy + Ady, Ay, 15.0f);
//Az
SPI_CSXM = 0; //start spi talking Ax
spi.write(0xEC); //read B11101100 read/multi/address
@@ -193,7 +319,7 @@
high_byte = spi.write(0);
Buff = high_byte << 8 |low_byte;
SPI_CSXM = 1; //end spi talking
- Az = Buff * 1.22e-4;
+ Az = lpf(Buff*Apz + Adz, Az, 15.0f);
}
//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of read_IMU funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
@@ -206,14 +332,110 @@
gDIR[0][0] = gDIR[0][0] - (Wy*gDIR[0][2] - Wz*gDIR[0][1])*dt;
gDIR[0][1] = gDIR[0][1] - (Wz*gDIR[0][0] - Wx*gDIR[0][2])*dt;
gDIR[0][2] = gDIR[0][2] - (Wx*gDIR[0][1] - Wy*gDIR[0][0])*dt;
-
+
//update
-
-
+ gDIR[0][0] = (1-Bet) * gDIR[0][0] + Bet * Ax;
+ gDIR[0][1] = (1-Bet) * gDIR[0][1] + Bet * Ay;
+ gDIR[0][2] = (1-Bet) * gDIR[0][2] + Bet * Az;
+
//nutralizing
- gUnity = gDIR[0][0]*gDIR[0][0] + gDIR[0][1]*gDIR[0][1] + gDIR[0][2]*gDIR[0][2];
+ gUnity = sqrt( gDIR[0][0]*gDIR[0][0] + gDIR[0][1]*gDIR[0][1] + gDIR[0][2]*gDIR[0][2] );
for(int i=0; i<3; i++) {
gDIR[0][i] = gDIR[0][i] / gUnity;
}
+
+ //transfer
+ Ele_phy = asin(gDIR[0][0]);
+ Til_phy = asin(-gDIR[0][1] / cos(Ele_phy));
+// //test
+// gDIR[0][0] = gDIR[0][0] + Wx*dt;
+// gDIR[0][1] = gDIR[0][1] + Wy*dt;
+// gDIR[0][2] = gDIR[0][2] + Wz*dt;
}
-//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of state_update funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
\ No newline at end of file
+//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of state_update funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
+
+
+
+//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓RR funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
+void RR() //status generator
+{
+ Ele_phy_int = Ele_phy_int + Ele_phy*dt;
+ Til_phy_int = Til_phy_int + Til_phy*dt;
+ Ele_phy_int = constrain(Ele_phy_int,-0.01f,0.01f);
+ Til_phy_int = constrain(Til_phy_int,-0.01f,0.01f);
+
+ VEC[0][4] = -9.5f*Ele_phy - 0.17f*Wy - 60.0f*Ele_phy_int;
+ VEC[0][5] = -9.5f*Til_phy - 0.17f*Wx - 60.0f*Til_phy_int;
+
+ VEC[0][4] = constrain(VEC[0][4],-0.30f,0.30f);
+ VEC[0][5] = constrain(VEC[0][5],-0.30f,0.30f);
+}
+//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of RR funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
+
+
+
+//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓stPF_lenth_uni funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
+void stPF_lenth_uni() //referenve generator
+{
+ Rtot[0][0] = cos(VEC[0][4])*cos(VEC[0][3]);
+ Rtot[0][1] = - cos(VEC[0][5])*sin(VEC[0][3]) + cos(VEC[0][3])*sin(VEC[0][4])*sin(VEC[0][5]);
+ Rtot[0][2] = sin(VEC[0][5])*sin(VEC[0][3]) + cos(VEC[0][5])*cos(VEC[0][3])*sin(VEC[0][4]);
+ Rtot[1][0] = cos(VEC[0][4])*sin(VEC[0][3]);
+ Rtot[1][1] = cos(VEC[0][5])*cos(VEC[0][3]) + sin(VEC[0][4])*sin(VEC[0][5])*sin(VEC[0][3]);
+ Rtot[1][2] = - cos(VEC[0][3])*sin(VEC[0][5]) + cos(VEC[0][5])*sin(VEC[0][4])*sin(VEC[0][3]);
+ Rtot[2][0] = - sin(VEC[0][4]);
+ Rtot[2][1] = cos(VEC[0][4])*sin(VEC[0][5]);
+ Rtot[2][2] = cos(VEC[0][4])*cos(VEC[0][5]);
+
+ for(int j=0; j<6; j++) { //reset C
+ for(int i=0; i<3; i++) {
+ C[i][j] = 0;
+ }
+ }
+
+ for(int i=0; i<6; i++) { //(RR.' * B)
+ for(int j=0; j<3; j++) {
+ for(int k=0; k<3; k++) {
+ C[j][i] = Rtot[j][k] * B[k][i] + C[j][i];
+ }
+ }
+ }
+
+ for(int j=0; j<6; j++) { //+ [X,Y,Z]
+ for(int i=0; i<3; i++) {
+ C[i][j] = C[i][j] + VEC[0][i];
+ }
+ }
+ for(int i=0; i<6; i++) {
+ float X = C[0][i]-A[0][i];
+ float Y = C[1][i]-A[1][i];
+ float Z = C[2][i]-A[2][i];
+ L[0][i] = sqrt(X*X+Y*Y+Z*Z);
+ }
+}
+//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of stPF_lenth_uni funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
+
+
+
+//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓stPF_travle_R funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
+void stPF_travle_R() //lenth to pwm pulse width
+{
+ for(int i=0; i<6; i++) {
+ A_R = ( L[0][i] - L90 + sqrt( Llink*Llink - Larm*Larm ) ) /Larm;
+ B_R = Llink/Larm;
+ C_R = ( A_R*A_R - B_R*B_R + 1 ) /(A_R*2);
+ PWM[0][i] = -asin(C_R)*573 + PWM_base[0][i];
+ }
+}
+//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of stPF_travle_R funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
+
+
+
+//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓lpf funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
+float lpf(float input, float output_old, float frequency)
+{
+ float output = 0;
+ output = (output_old + frequency*dt*input) / (1 + frequency*dt);
+ return output;
+}
+//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of lpf funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
\ No newline at end of file
