Oscar Liao
SEA_DEMO
main.cpp
- Committer:
- OscarLiao
- Date:
- 2018-12-27
- Revision:
- 2:e8c86014468c
- Parent:
- 1:c6c07cb81c1a
File content as of revision 2:e8c86014468c:
/*
This is a program for localized controller of one leg
Endpoint space Impedence Control and Force/Torque feedforward
Two input was acceptable
q_0_E[3] Neutral point for Impedence reference
T_0[3] Feedforward of Force/Torque
---------o------↺ q_act_est[0]
/|
L1 / | ⤸ q_act_est[1]
o |
L2 ／/
／ / ⤸ q_act_est[2]
o /
MCU target: F303K8
ADC config: 3.3 Vadc common source
*/
//**************** Wiring Map ********************//
// |---------------| | |---------------|
// Srl| PA_9 | D1 |Cmd Serail | | VIN | VIN |
// Srl| PA_10 | D0 |Cmd Serial | | GND | GND |
// | NRST | NRST | | | NRST | NRST |
// | GND | GND | | | 5V | 5V |
// | PA_12 | D2 | |Srl| A7 | PA_2 |PC Serial
// PWM| PB_0 | D3 |Shing Drv |Dou| A6 | PA_7 |CSG
// Dou| PB_7 | D4 |Task1 |Din| A5 | PA_6 |CS2 Stretch Cmd
// Dou| PB_6 | D5 |Task2 |Din| A4 | PA_5 |CS1 Serial Sync
// PWM| PB_1 | D6 |Tigh Drv |Ain| A3 | PA_4 |Shing sense
// | PF_0 | D7 | |Ain| A2 | PA_3 |Tigh sense
// | PF_1 | D8 | |Ain| A1 | PA_1 |Hip sense
// PWM| PA_8 | D9 |Hip Drv | | A0 | PA_0 |
// | PA_11 | D10 | | | AREF | AREF |VR source
// | PB_5 | D11 | | | 3V3 | 3V3 |
// | PB_4 | D12 | |Dou| D13 | PB_3 |led / Serial error
// |---------------| | |---------------|
#include "mbed.h"
//#include "LSM9DS0_SH.h"
//#define DEBUG 1
#define DEBUG 0
#define pi 3.141592f
#define d2r 0.01745329f
#define Rms 5000 //TT rate
#define dt 0.015f
#define Task_1_NN 2
#define Task_2_NN 199
//Torque-Controller gain
#define G1_rad_dc 0.00157f
#define Kqff 636.7f //ffoward gain of q_act_est = 1 / G1_rad_dc
#define KTff 60.408f //ffoward gain of T_ref_WD = 1 / (K_q * G1_rad_dc)
#define Kpt 22.9f
#define Ki 151.9f
//ADC reference constant
#define Kadc 5.817f //adc value to q_spring
#define K_spring 13180f //sping constant of single spring
#define K_q 10.544f //2*K_spring*R^2
#define R 0.02f //wheely's radius
//Endpoint Impedence Contorller coefficcient
#define Kpi_Hip 0
#define Kpi_Thigh 2000
#define Kpi_Shin 8
#define Kd_Hip 0
#define Kd_Thigh 0
#define Kd_Shin 0
#define KN 20
#define L1 0.120f //Tigh length
#define L2 0.1480f //Shing lenth
#define LA (L1*L1+L2*L2) //L1^2 + L2^2
#define LB (L1*L2) //L1*L2
#define LC (L1/L2) //L1/L2
#define L0 0.2f //Length of the leg
//Structure
#define PWM_Hip 1460 //900~1550~2200 1800
#define PWM_Thigh 1330 //900~1550~2200 1300
#define PWM_Shin 1780 //900~1550~2200 1450 1740
#define Buff_size 16 //Serial Buffer
#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
//≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡GPIO registor≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡//
//╔═════════════════╗
//║ Structure ║
//╚═════════════════╝
DigitalOut led(D13); //detection
DigitalOut led2(D12); //detection
DigitalOut Task_1(D4); //Task indicate
DigitalOut Task_2(D5);
DigitalOut CSG(A6); //Stretch interrupt pull down
AnalogIn adc1(A1); //adc
AnalogIn adc2(A2); //adc
AnalogIn adc3(A3); //adc
InterruptIn CS1(D2); //Serial synchronizor
DigitalIn CS2(A5); //Stretch interrupt
//╔═════════════════╗
//║ Servo out ║
//╚═════════════════╝
PwmOut Drive1(D9); //PWM 1/1
PwmOut Drive2(D6); //PWM 1/2N
PwmOut Drive3(D3); //PWM 1/3N
//╔═════════════════╗
//║ Serial ║
//╚═════════════════╝
Serial Debug(PA_2, PA_15); //Serial reg(TX RX) USB port
Serial Cmd(D1, D0); //Serial reg(TX RX)
//■■■■■■■■■■■■■■■■■■■■■■■■■■■end of GPIO registor■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//
//≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Varible registor≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡//
//╔═════════════════╗
//║ Structure ║
//╚═════════════════╝
Ticker TT; //call a timer
uint8_t Task_1_count = 0; //1st prior task count
uint8_t Task_2_count = 0; //2nd prior task count
uint8_t Flag_1 = 0; //1st prior task flag
uint8_t Flag_2 = 0; //2nd prior task flag
//╔═════════════════╗
//║ ADC ║
//╚═════════════════╝
//╔═════════════════╗
//║ PWM ║
//╚═════════════════╝
const int16_t PWM_base[3] = { //reference command at 0, -45, 45 deg
PWM_Hip,PWM_Thigh,PWM_Shin
};
int16_t PWM[3] = { //command out PWM_base + q_ref
0, 0, 0 //transfer: 10us to 1 deg
};
//╔═════════════════╗
//║ q_actEstimator ║
//╚═════════════════╝
/*
q_act_est = q_ref*G1_rad_TUT(z) - q_spring
G1_rad_TUT(z) is shown below
y(z) 1.316 + 2.633*z^-1 + 1.316*z^-2 b0 + b1*z^-1 + b2*z^-2
----- = ----------------------------------- * 1e-04 = ------------------------
x(z) 1 - 0.8447*z^-1 + 0.1799*z^-2 1 - a1*z^-1 - a2*z^-2
q_ref═>[Σ]════════╦═>[b0]═>[Σ]═[1e-04]═>y
⇑ ⇓w0 ⇑
║ [1/z] ║
[Σ]<═[a1]<═╬═>[b1]═>[Σ]
⇑ ⇓w1 ⇑
║ [1/z] ║
╚═══[a2]<═╩═>[b2]═══╝
w2
*/
float q_act_est[3] = { //output of filter
0, 0, 0
};
float L_act_est = 0.0;
float P_act_est = 0.0;
float G1_w_n[3][3] = { //G1_rad_TUT's
{0, 0, 0}, //{w0, w1, w2}
{0, 0, 0}, //second sea
{0, 0, 0},
};
const float G1_a[3] = { //{a0, a1, a2}
1.00, 0.8447, -0.1799
};
const float G1_b[3] = { //{b0, b1, b2}
1.316, 2.633, 1.316
};
//╔═════════════════╗//╔══════════════════╗
//║ Comp_ref ║//║ Controller Input ║
//╚═════════════════╝//╚══════════════════╝
/*
---------o------↺ q_act_est[0]
/|
L1 / | ↻ q_act_est[1]
o |
L2 ／/
／ / ↻ q_act_est[2]
o /
1 - z^-1
PDn(z) = P + D*N*---------------------
1 - (1 - N*Ts)*z^-1
y(z) b0 + b1*z^-1 1 + (-1)*z^-1
----- = -------------- = ------------------
x(z) 1 - a1*z^-1 1 - 0.7*z^-1
x═>[Σ]═══════╦═>[b0]═>[Σ]═>[D*N]═>y
⇑ ⇓w0 ⇑
║ [1/z] ║
╚══[a1]<═╩═>[b1]═══╝
w1
*/
float K_c[3] = { //K_c reference impedence
Kpi_Hip, Kpi_Thigh, Kpi_Shin //{Hip, Thigh, Shin}
};
float B_c[3] = { //B_c = D*N (*20)reference damping (Dmax~0.4)(B_c_max~5) in joint!!
Kd_Hip*KN, Kd_Thigh*KN, Kd_Shin*KN //{Hip, Thigh, Shin}
};
float q_0_E[3] = { //"Controller Input" from upper controller in endpoint level (Operational Space)
0, 0.2, 0 //{Hip, leg length, leg angle} original point
};
float q_0_E_1_gain = 0.2f/255.0f;
float q_0_E_2_gain = (pi/4)/128.0f;
float K_c_gain = 4000.0f/255.0f;
float B_c_gain = 6.0f/255.0f;
float T_0[3] = { //"Controller Input" from upper controller in joint level
0, 0, 0 //{T_0_Hip, T_0_Tigh, F_0_Stretch} force feed foward
};
float q_err_E[3] = { //error in endpoint level
0, 0, 0
};
float q_err_D_E[3] = { //derivative error in endpoint level
0, 0, 0
};
float Cref_w[3][2] = { //filter storage
{0, 0},
{0, 0},
{0, 0},
};
const float Cref_a[2] = { //{a0, a1}
1.00, 0.70
};
const float Cref_b[2] = { //{b0, b1}
1.00, -1.00
};
float J32 = 0.0;
float J33 = 0.0;
float Phi1_0 = 0.0;
float Phi2_0 = 0.0;
//╔═════════════════╗
//║ Torque_reg ║
//╚═════════════════╝
/*
q_act_est T_ref_WD
q_ref = ----------- + -------------------------- + PI( T_ref - 2*K_spring*R^2*q_spring )
G1_rad_dc 2*K_spring*R^2*G1_rad_dc
PI(z) = P + I*dt / (z-1)
*/
const float Kp[3] = { //P gain for Torque REG
Kpt, Kpt, Kpt //{Hip, Tigh, Shin}
};
const float Kit[3] = { //I gain for Torque REG Kit = Ki*Ts
Ki*dt, Ki*dt, Ki*dt //{Hip, Tigh, Shin}
};
float T_ref[3] = { //command to T_reg in joint level
0, 0, 0 //{Hip, Tigh, Shin}
};
float T_ref_E[3] = { //command to T_reg in end point level
0, 0, 0 //{Hip, Len, Phi}
};
float T_ref_WD[3] = { //command without damping reference in joint level
0, 0, 0
};
float T_ref_WD_E[3] = { //command without damping reference in endpoint level
0, 0, 0
};
float T_err[3] = { //T_ref - T_act_est, T_act_est = q_spring * K_q
0, 0, 0 //{Hip, Tigh, Shin}
};
float q_ref[3] = { //output of T_reg
0, 0, 0 //{Hip, Thigh, Shin}
};
float q_spring[3] = { //read in spring travel
0, 0, 0
};
float q_spring_0[3] = { //neutral point of VR
0, 0, 0
};
float T_reg_I[3] = { //PI_w
0, 0, 0
};
//╔═════════════════╗
//║ I/O Serial ║
//╚═════════════════╝
uint8_t Buff[Buff_size];
uint8_t Recieve_index = 0;
uint8_t Cmd_Flag = 0;
uint8_t Rx_enable = 0;
char buffer[128];
//■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Varible registor■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//
//≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Function registor≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡//
void init_TIMER(); //set TT_main() rate
void TT_main(); //timebase function rated by TT
void init_q_spring_0(); //calibrating
void init_IO(); //initialize IO state
void q_spring_Est();
void q_act_Est();
void Impedence_Ref();
void Torque_Reg();
void Rx_irq();
void Rx_srt();
void Rx_end();
//void NVIC_DisableIRQ(USART1_IRQn);
//void NVIC_EnableIRQ(USART1_IRQn);
//■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Function registor■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//
//≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡main funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡//
int main()
{
Debug.baud(115200); //set baud rate
Cmd.baud(115200); //set baud rate
wait_ms(10);
init_q_spring_0(); //calibrating q_spring reference point
wait_ms(600);
init_IO(); //initialized value
init_TIMER(); //start TT_main
Cmd.attach(&Rx_irq,Serial::RxIrq); //start recieving message
// NVIC_DisableIRQ(USART1_IRQn); //stay down before CS1 signal
CS1.fall(&Rx_srt); //attach the address of Rx_srt to falling edge as start signal of communication
CS1.rise(&Rx_end); //attach the address of Rx_end to rise edge
//for joint endpoint transform
Phi1_0 = (acos((L0*L0+L1*L1-L2*L2)/(2*L0*L1)));
Phi2_0 = (Phi1_0 + acos((L0*L0+L2*L2-L1*L1)/(2*L0*L2)));
while(1) { //main() loop
//Task judging start
if(Flag_1 == 1) { //check pending
//Task_1 start
//read in condition & estimate state
q_spring_Est();
q_act_Est();
//calculate control effort
Impedence_Ref();
Torque_Reg();
//PWM Drive out process
for(int i=0; i<3; i++) {
PWM[i] = PWM_base[i] + q_ref[i]; // X + Controll
}
//PWM[0] = 1460;
//PWM[1] = 1330;
//PWM[2] = 1830;
PWM[0] = constrain(PWM[0],800 ,2100);//safty constrain
PWM[1] = constrain(PWM[1],800 ,2100);//safty constrain
PWM[2] = constrain(PWM[2],800 ,2100);//safty constrain
Drive1.pulsewidth_us(PWM[0]); //drive command
Drive2.pulsewidth_us(PWM[1]);
Drive3.pulsewidth_us(PWM[2]);
//for Serial-Oscilloscope
#if DEBUG
// q_0[2] = -0.9;
// Debug.printf("%f\n", q_act_est[1]);
// Debug.printf("%f, %f, %f\r", q_0[0], q_0[1], q_0[2]);
// Debug.printf("%lf\r", adc3.read());
// Debug.printf("%f\n", 123.23);
Debug.printf("%d %f\r", Buff[2], q_0_E[2]);
// Debug.printf("%.f\r", q_ref[0]);L_act_est
// Debug.printf("%.2f\r", q_spring_0[0]);
// Debug.printf("%.2f\r", J33);
#endif
//Task_1 done
Flag_1 = 0; //clear pending
Task_1 = 0; //clear sign
}
if(Cmd_Flag == 1) {
//{...}{Hip, Len, Phi}, {'#'}
//Received some shit, start decoding
led = 1;
if(Buff[3] == 'Q') {
//q_0 command
q_0_E[0] = (Buff[0] - 128);
q_0_E[1] = 0.1f + q_0_E_1_gain * Buff[1];
q_0_E[2] = q_0_E_2_gain * (Buff[2]-128);
} else if(Buff[3] == 'T') {
//T_0 command
T_0[0] = 1.0f * (Buff[0] - 128);
T_0[1] = 1.0f * (Buff[1] - 128);
T_0[2] = 1.0f * (Buff[2] - 128);
} else if(Buff[3] == 'K') {
K_c[0] = 0.02f * Buff[0];
K_c[1] = K_c_gain * Buff[1];
K_c[2] = Buff[2];
} else if (Buff[3] == 'B') {
B_c[0] = 0.02f * (Buff[0] - 128);
B_c[1] = (B_c_gain * Buff[1] - 1.0f)*KN;
B_c[2] = (B_c_gain * Buff[2] - 1.0f)*KN;
} else {
//Show error in communication
led2 = 1;
}
Buff[3] = 0x00; //clear end signals
Cmd_Flag = 0;
led = 0;
//Debug.printf("%.f, %.f, %.f, %.f, %.f, %.f\r", K_c[0], K_c[1], K_c[2], B_c[0], B_c[1], B_c[2]);
//Send out message threw Cmd
// Cmd.printf("%c %c %c\r", Dummy[0], Dummy[1], Dummy[2]);
}
}
}
//■■■■■■■■■■■■■■■■■■■■■■■■■■■end of main funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//
//≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Timebase funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡//
void init_TIMER() //set TT_main{} rate
{
TT.attach_us(&TT_main, Rms);
}
void TT_main() //interrupt function by TT
{
Task_1_count = Task_1_count + 1;
if(Task_1_count > Task_1_NN) {
Task_1_count = 0; //Task triggering
Flag_1 = 1;
Task_1 = 1; //show for indicator
}
Task_2_count = Task_2_count + 1;
if(Task_2_count > Task_2_NN) {
Task_2_count = 0;
//Task triggering
Flag_2 = 1;
Task_2 = 1; //show for indicator
}
}
//■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Timebase funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//
//≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡init_IO funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡//
void init_IO(void) //initialize
{
CS1.mode(PullUp); //A4
CS2.mode(PullUp); //A5
CSG = 0; //A6
led = 0;
Drive1.period_us(15000);
Drive2.period_us(15000);
Drive3.period_us(15000);
}
//■■■■■■■■■■■■■■■■■■■■■■■■■■■end of init_IO funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//
//≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡controller funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡//
//follow steps for discrete process in j oder
//1.update w0[n] threw input x[n] & wi[n] from last sequence
// w0[n] = x[n] + Zigma( ai*wi[n] ), i = 1, 2, 3...j-1, j.
// y[n] = Zigma( bi*wi[n] ), i = 0, 1, 2...j-1, j.
//2.update wi[n+1] for next sequence
// wi[n] = wi-1[n], i = j, j-1...2, 1.
void q_spring_Est(void)
{
//read in from adc
q_spring[0] = Kadc * adc1.read() - q_spring_0[0];
q_spring[1] = Kadc * adc2.read() - q_spring_0[1];
q_spring[2] = Kadc * adc3.read() - q_spring_0[2];
}
void q_act_Est(void)
{
for(int i=0; i<3; i++) {
q_ref[i] = PWM[i] - PWM_base[i];
}
//q_act_est = q_ref*G1_rad_TUT(z) - q_spring
//G1_rad_TUT(z) is done below
for(int i=0; i<3; i++) {
//update w0[n] threw input x[n] & wi[n] from last sequence
G1_w_n[i][0] = q_ref[i] + G1_a[1]*G1_w_n[i][1] + G1_a[2]*G1_w_n[i][2];
q_act_est[i] = G1_b[0]*G1_w_n[i][0] + G1_b[1]*G1_w_n[i][1] + G1_b[2]*G1_w_n[i][2];
q_act_est[i] = q_act_est[i]*0.0001f - q_spring[i];
//update wi[n+1]
G1_w_n[i][2] = G1_w_n[i][1];
G1_w_n[i][1] = G1_w_n[i][0];
}
//q_act_est[i] is now ready for following app
}
void Impedence_Ref(void)
{
//Transform q_act_est from joint to end point
q_act_est[1] -= Phi1_0;
q_act_est[2] += Phi2_0;
L_act_est = sqrt( LA + 2*LB*cos(q_act_est[2]) );
P_act_est = q_act_est[1] + asin( L2*sin(q_act_est[2]) / L_act_est );
//Calculate error in end point level
//q_0[i] => {Hip, Tigh, Shin}
q_err_E[0] = q_0_E[0] - q_act_est[0]; //Hip motor#1
q_err_E[1] = q_0_E[1] - L_act_est;
q_err_E[2] = q_0_E[2] - P_act_est;
for(int i=0; i<3; i++) {
//Generate T_ref in Operational Space (OSC control perhaps?)
//T_ref_E = PDn( q_err_E );
//Compliance
T_ref_WD_E[i] = K_c[i] * q_err_E[i];
//Damping
//update w0[n] threw input x[n] & wi[n] from last sequence
Cref_w[i][0] = q_err_E[i] + Cref_a[1]*Cref_w[i][1];
q_err_D_E[i] = Cref_b[0]*Cref_w[i][0] + Cref_b[1]*Cref_w[i][1];
//update wi[n+1]
Cref_w[i][1] = Cref_w[i][0];
//Generate T_ref in endpoint level
T_ref_E[i] = T_ref_WD_E[i] + B_c[i]*q_err_D_E[i];
}
//Transform torque to joint space, T_ref[i] = Σ( J[i][j] * T_ref_E[j] )
J32 = -LB*sin(q_act_est[2]) / L_act_est;
J33 = ( pow(J32,2)/LB + cos(q_act_est[2]) ) / (LC + cos(q_act_est[2]));
//Torque of Hip
T_ref[0] = T_ref_E[0];
T_ref_WD[0] = T_ref_WD_E[0];
//Torque of Tigh
T_ref[1] = T_ref_E[2] + T_0[1];
T_ref_WD[1] = T_ref_WD_E[2] + T_0[1];
//Torque of Shin
T_ref[2] = J32*T_ref_E[1] + J33*T_ref_E[2] + J32*T_0[1] + J33*T_0[2];
T_ref_WD[2] = J32*T_ref_WD_E[1] + J33*T_ref_WD_E[2] + J32*T_0[1] + J33*T_0[2];
}
void Torque_Reg(void)
{
if(CS2 == 1) {
//Transform q_act_est from end point to joint
q_act_est[1] += Phi1_0;
q_act_est[2] -= Phi2_0;
//Normal command
for(int i=0; i<3; i++) {
//FeedFoward
q_ref[i] = q_act_est[i]*Kqff + T_ref_WD[i]*KTff; //full feedfoward
//q_ref[i] = 0; //w/o feedfoward
//FeedBack
T_err[i] = T_ref[i] - K_q*q_spring[i];
T_reg_I[i] = T_reg_I[i] + T_err[i]*Kit[i];
T_reg_I[i] = constrain(T_reg_I[i], -200, 200);
q_ref[i] = T_err[i]*Kp[i] + T_reg_I[i] + q_ref[i];
}
q_ref[0] = (q_0_E[0]/128.0f)*225;
q_ref[0] = constrain(q_ref[0], -650, 650);
q_ref[1] = constrain(q_ref[1], -650, 650);
q_ref[2] = constrain(q_ref[2], -650, 650);
} else {
//Stretch command
for(int i=0; i<3; i++) {
T_reg_I[i] = 0;
q_ref[i] = 0;
}
}
}
//■■■■■■■■■■■■■■■■■■■■■■■■■■■end of controller funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//
//≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡init_q_spring_0 funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡//
void init_q_spring_0(void)
{
for(int i=0; i<1000; i++) {
q_spring_0[0] = q_spring_0[0] + Kadc * adc1.read();
q_spring_0[1] = q_spring_0[1] + Kadc * adc2.read();
q_spring_0[2] = q_spring_0[2] + Kadc * adc3.read();
wait_us(100);
}
q_spring_0[0] = q_spring_0[0] /1000.0f;
q_spring_0[1] = q_spring_0[1] /1000.0f;
q_spring_0[2] = q_spring_0[2] /1000.0f;
}
//■■■■■■■■■■■■■■■■■■■■■■■■■■■end of init_q_spring_0 funtion■■■■■■■■■■■■■■■■■■■■■■■■//
//≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Rx_xxx funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡//
void Rx_irq(void)
{
//Keep clear in Rx_irq and leave most work after Rx_end
Buff[Recieve_index] = Cmd.getc();
Recieve_index = Recieve_index + Rx_enable;
}
void Rx_srt(void)
{
//start receiving Serial message from Cmd
// NVIC_EnableIRQ(USART1_IRQn);
Rx_enable = 1;
}
void Rx_end(void)
{
//end of receiving Serial message from Cmd
// NVIC_DisableIRQ(USART1_IRQn);
Rx_enable = 0;
Recieve_index = 0;
Cmd_Flag = 1;
}
//■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Rx_irq funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//