Oscar Liao
balance
Diff: main.cpp
- Revision:
- 0:94cf69f1f327
- Child:
- 1:15dd461fbc2a
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Thu Dec 13 14:27:35 2018 +0000
@@ -0,0 +1,526 @@
+/*
+This is a program for globlelized controller of whole DOGO
+F746ZG
+*/
+
+#include "mbed.h"
+#include "LSM9DS0_SH.h"
+#define DEBUG 0
+
+#define pi 3.141592f
+#define d2r 0.01745329f
+
+#define Rms 5000 //TT rate
+#define dt 0.005f
+#define Task_1_NN 9
+#define Task_2_NN 5
+#define Task_4_NN 9
+
+//Structure
+#define Buff_size 16 //Serial Buffer
+
+//Initial Position
+#define init_leg_Angle1 0
+#define init_leg_length 0.22f
+#define init_leg_Angle2 0
+
+#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
+
+//≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡GPIO registor≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡//
+//╔═════════════════╗
+//║ Structure ║
+//╚═════════════════╝
+DigitalOut LD1(PB_0); //detection
+DigitalOut LD2(PB_7); //detection
+DigitalOut LD3(PB_14); //detection
+InterruptIn button(USER_BUTTON); //Button press;
+
+//╔═════════════════╗
+//║ Serial ║
+//╚═════════════════╝
+Serial Debug(PD_8, PD_9); //Serial_3 reg(TX RX) USB port
+
+Serial LF_Cmd(PD_5, PD_6); //Serial_2
+DigitalOut LF_CS(PD_7);
+
+Serial LH_Cmd(PC_12, PD_2); //Serial_5
+DigitalOut LH_CS(PG_2);
+
+Serial RF_Cmd(PG_14, PC_7); //Serial_6
+DigitalOut RF_CS(PG_9);
+
+Serial RH_Cmd(PE_8, PE_7); //Serial_7
+DigitalOut RH_CS(PE_10);
+
+//╔═════════════════╗
+//║ IMU_SPI ║
+//╚═════════════════╝
+DigitalOut SPI_CSG(PD_14,1); //low for GYRO enable
+DigitalOut SPI_CSXM(PD_15,1); //low for ACC/MAG enable
+SPI spi(PA_7, PA_6, PA_5); //MOSI MISO SCLK
+//■■■■■■■■■■■■■■■■■■■■■■■■■■■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 Task_3_count = 0; //3nd prior task count
+uint8_t Task_4_count = 0; //24nd prior task count
+uint8_t Flag_1 = 0; //1st prior task flag
+uint8_t Flag_2 = 0; //2nd prior task flag
+uint8_t Flag_3 = 0; //3nd prior task flag
+uint8_t Flag_4 = 0; //4nd prior task flag
+
+//╔═════════════════╗
+//║ I/O Serial ║
+//╚═════════════════╝
+uint8_t SerBuff[Buff_size];
+uint8_t Recieve_index = 0;
+
+//╔═════════════════╗
+//║ Leg Command ║
+//╚═════════════════╝
+
+float LF_q_0_E[3] = {
+ init_leg_Angle1, init_leg_length, init_leg_Angle2
+};
+float LH_q_0_E[3] = {
+ init_leg_Angle1, init_leg_length, init_leg_Angle2
+};
+float RF_q_0_E[3] = {
+ init_leg_Angle1, init_leg_length, init_leg_Angle2
+};
+float RH_q_0_E[3] = {
+ init_leg_Angle1, init_leg_length, init_leg_Angle2
+};
+
+uint8_t LF_Cmd_Hex[3] = {0x00, 0x00, 0x00};
+uint8_t LH_Cmd_Hex[3] = {0x00, 0x00, 0x00};
+uint8_t RF_Cmd_Hex[3] = {0x00, 0x00, 0x00};
+uint8_t RH_Cmd_Hex[3] = {0x00, 0x00, 0x00};
+
+//╔═════════════════╗
+//║ IMU_SPI ║
+//╚═════════════════╝
+short low_byte = 0x00; //buffer
+short high_byte = 0x00;
+short Buff = 0x00;
+float Wx = 0.0;
+float Wy = 0.0;
+float Wz = 0.0;
+float Ax = 0.0;
+float Ay = 0.0;
+float Az = 0.0;
+float gDIR[1][3] = { //g vector's direction , required unitconstrain
+ {0.0,0.0,0.0}, //X Y Z
+};
+float Bet = 0.002; //confidence of Acc data
+float gUnity = 0.0;
+float Gdx = 0.0;
+float Gdy = 0.0;
+float Gdz = 0.0;
+float Adx = 0.0;
+float Ady = 0.0;
+float Adz = 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;
+float Ele_control = 0.0;
+float Til_control = 0.0;
+
+//╔═════════════════╗
+//║ Balance ║
+//╚═════════════════╝
+float Kp = 0.04;
+float Ki = 0.0;
+float Kd = 0.0;
+float Pu = 0.0;
+float Iu = 0.0;
+float Du = 0.0;
+float Ele_phy_old = 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_IMU(); //initialize IMU
+void init_GXdrift(); //read GXdrift at start up
+void read_IMU(); //read IMU data give raw data
+void state_update(); //estimation of new attitude
+
+float lpf(float input, float output_old, float frequency); //lpf discrete
+
+void pressed();
+void Balance();
+
+void Rx_irq();
+//■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Function registor■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//
+
+
+//≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡main funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡//
+int main()
+{
+ init_IMU(); //initialize IMU
+ init_GXdrift(); //read GXdrift at start up
+
+ Debug.baud(115200); //set baud rate
+ LF_Cmd.baud(115200);
+ LH_Cmd.baud(115200);
+ RF_Cmd.baud(115200);
+ RH_Cmd.baud(115200);
+
+ wait_ms(10);
+ init_TIMER(); //start TT_main
+
+ Debug.attach(&Rx_irq,Serial::RxIrq); //Start recieving message
+ LF_CS = 1;
+
+ wait_ms(200);
+
+ button.fall(&pressed); //button pressed
+
+ while(1) { //main() loop
+
+// if(Debug.readable()) {
+// SerBuff[Recieve_index] = Debug.getc();
+// Debug.putc(SerBuff[Recieve_index]);
+// Recieve_index = Recieve_index + 1;
+// }
+
+// if(device.readable()) {
+// pc.putc(device.getc());
+// }
+ //Balance
+ if(Flag_1 == 1) {
+
+ //Initial Position
+ LF_q_0_E[1] = init_leg_length;
+ LH_q_0_E[1] = init_leg_length;
+ RF_q_0_E[1] = init_leg_length;
+ RH_q_0_E[1] = init_leg_length;
+
+ Balance();
+ Flag_1 = 0;
+ }
+
+ //Send Command
+ if(Flag_4 == 1) {
+
+ LF_Cmd_Hex[0] = (LF_q_0_E[0]+pi/4)/(pi/2)*255;
+ LF_Cmd_Hex[1] = ((LF_q_0_E[1]-0.1)/0.2)*255;
+ LF_Cmd_Hex[2] = (LF_q_0_E[2]+pi/4)/(pi/2)*255;
+
+ LH_Cmd_Hex[0] = (LH_q_0_E[0]+pi/4)/(pi/2)*255;
+ LH_Cmd_Hex[1] = ((LH_q_0_E[1]-0.1)/0.2)*255;
+ LH_Cmd_Hex[2] = (LH_q_0_E[2]+pi/4)/(pi/2)*255;
+
+ RF_Cmd_Hex[0] = (RF_q_0_E[0]+pi/4)/(pi/2)*255;
+ RF_Cmd_Hex[1] = ((RF_q_0_E[1]-0.1)/0.2)*255;
+ RF_Cmd_Hex[2] = (RF_q_0_E[2]+pi/4)/(pi/2)*255;
+
+ RH_Cmd_Hex[0] = (RH_q_0_E[0]+pi/4)/(pi/2)*255;
+ RH_Cmd_Hex[1] = ((RH_q_0_E[1]-0.1)/0.2)*255;
+ RH_Cmd_Hex[2] = (RH_q_0_E[2]+pi/4)/(pi/2)*255;
+
+ //Debug.printf("%d, %d, %d\r", LH_Cmd_Hex[0], LH_Cmd_Hex[1], LH_Cmd_Hex[2]);
+ //LED
+ LD1 = 1;
+
+ //Left Front Leg
+ LF_CS = 0;
+ wait_us(20);
+ LF_Cmd.putc(LF_Cmd_Hex[0]);
+ LF_Cmd.putc(LF_Cmd_Hex[1]);
+ LF_Cmd.putc(LF_Cmd_Hex[2]);
+ LF_Cmd.putc('Q');
+ wait_us(180);
+ LF_CS = 1;
+
+ //Left Hind Leg
+ LH_CS = 0;
+ wait_us(20);
+ LH_Cmd.putc(LH_Cmd_Hex[0]);
+ LH_Cmd.putc(LH_Cmd_Hex[1]);
+ LH_Cmd.putc(LH_Cmd_Hex[2]);
+ LH_Cmd.putc('Q');
+ wait_us(180);
+ LH_CS = 1;
+
+ //Right Front Leg
+ RF_CS = 0;
+ wait_us(20);
+ RF_Cmd.putc(RF_Cmd_Hex[0]);
+ RF_Cmd.putc(RF_Cmd_Hex[1]);
+ RF_Cmd.putc(RF_Cmd_Hex[2]);
+ RF_Cmd.putc('Q');
+ wait_us(180);
+ RF_CS = 1;
+
+ //Right Hind Leg
+ RH_CS = 0;
+ wait_us(20);
+ RH_Cmd.putc(RH_Cmd_Hex[0]);
+ RH_Cmd.putc(RH_Cmd_Hex[1]);
+ RH_Cmd.putc(RH_Cmd_Hex[2]);
+ RH_Cmd.putc('Q');
+ wait_us(180);
+ RH_CS = 1;
+
+ LD1 = 0;
+
+ Flag_4 = 0;
+ }
+ }
+
+
+}
+//■■■■■■■■■■■■■■■■■■■■■■■■■■■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
+{
+ read_IMU(); //read IMU data give raw data
+ state_update(); //estimation of new attitude
+
+ Task_1_count = Task_1_count + 1;
+ if(Task_1_count > Task_1_NN) {
+ Task_1_count = 0; //Task triggering
+ Flag_1 = 1;
+ }
+
+ Task_4_count = Task_4_count + 1;
+ if(Task_4_count > Task_4_NN) {
+ Task_4_count = 0; //Task triggering
+ Flag_4 = 1;
+ }
+}
+//■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Timebase funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//
+
+//≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Button funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡//
+void pressed(void)
+{
+ Ki = 0.005;
+ Kd = 0.001;
+}
+
+void released(void)
+{
+
+}
+//■■■■■■■■■■■■■■■■■■■■■■■■■■■end of button funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//
+
+//≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Balance funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡//
+
+void Balance(void)
+{
+ Pu = Kp*Ele_phy;
+ Iu += Ki*Ele_phy;
+ Du = Kd*(Ele_phy-Ele_phy_old)/(dt*(Task_1_count+1));
+
+ LF_q_0_E[1] = LF_q_0_E[1] - (Pu+Iu+Du);
+ RF_q_0_E[1] = RF_q_0_E[1] - (Pu+Iu+Du);
+ LH_q_0_E[1] = LH_q_0_E[1] + (Pu+Iu+Du);
+ RH_q_0_E[1] = RH_q_0_E[1] + (Pu+Iu+Du);
+
+ LF_q_0_E[2] = - Ele_phy;
+ RF_q_0_E[2] = - Ele_phy;
+ LH_q_0_E[2] = - Ele_phy;
+ RH_q_0_E[2] = - Ele_phy;
+
+ //Update
+ Ele_phy_old = Ele_phy;
+
+ Debug.printf("%.3f, %.3f, %.3f\r", Pu, Iu, Du);
+}
+
+//■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Balance funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//
+
+
+//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓init_IMU funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
+void init_IMU(void) //initialize
+{
+ //gloable config
+ SPI_CSXM = 1; //high as init for disable SPI
+ SPI_CSG = 1;
+ spi.format(8, 3); //byte width, spi mode
+ spi.frequency(4000000); //8MHz
+
+ //for GYRO config
+ SPI_CSG = 0; //start spi talking
+ spi.write(CTRL_REG1_G);
+ spi.write(0x9F); //data rate 380 Hz/ cut off 25 Hz
+ SPI_CSG = 1; //end spi talking
+
+ SPI_CSG = 0; //start spi talking
+ spi.write(CTRL_REG4_G);
+ spi.write(0x10); //Scle 500dps
+ SPI_CSG = 1; //end spi talking
+
+ //for ACC config
+ SPI_CSXM = 0; //start spi talking
+ spi.write(CTRL_REG1_XM);
+ spi.write(0x87); //data rate 400 Hz/ Enable
+ SPI_CSXM = 1; //end spi talking
+
+ SPI_CSXM = 0; //start spi talking
+ spi.write(CTRL_REG2_XM);
+ spi.write(0xC8); //cut off 50 Hz/ Scale +-4g
+ SPI_CSXM = 1; //end spi talking
+}
+//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of init_IMU funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
+
+
+
+//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓init_G/Xdrift funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
+void init_GXdrift(void) //initialize
+{
+ for(int i=0; i<1000; i++) {
+ read_IMU(); //note GXdrift = 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;
+ //Debug.printf("%.4f,%.4f,%.4f\r", Gdx, Gdy, Gdz);
+
+ for(int i=0; i<1000; i++) {
+ read_IMU(); //note GXdrift = 0 at this moment
+ gDIR[0][0] = gDIR[0][0] - Ax;
+ gDIR[0][1] = gDIR[0][1] - Ay;
+ gDIR[0][2] = gDIR[0][2] - Az;
+ wait_ms(2);
+ }
+ Adx = gDIR[0][0] /1000.0f;
+ Ady = gDIR[0][1] /1000.0f;
+ Adz = gDIR[0][2] /1000.0f - 1.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_GXdrift funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
+
+
+
+//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓read_IMU funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
+void read_IMU(void) //read IMU data give raw data
+{
+ //Wx
+ SPI_CSG = 0; //start spi talking Wx
+ spi.write(0xE8); //read B11101000 read/multi/address
+ low_byte = spi.write(0);
+ high_byte = spi.write(0);
+ Buff = high_byte << 8 |low_byte;
+ SPI_CSG = 1; //end spi talking
+// Wx = Buff * Gpx + Gdx;
+ Wx = lpf(Buff * Gpx + Gdx, Wx, 48.0f);
+ //Wy
+ SPI_CSG = 0; //start spi talking Wx
+ spi.write(0xEA); //read B11101010 read/multi/address
+ low_byte = spi.write(0);
+ high_byte = spi.write(0);
+ Buff = high_byte << 8 |low_byte;
+ SPI_CSG = 1; //end spi talking
+// Wy = Buff * Gpy + Gdy;
+ Wy = lpf(Buff * Gpy + Gdy, Wy, 48.0f);
+ //Wz
+ SPI_CSG = 0; //start spi talking Wx
+ spi.write(0xEC); //read B11101100 read/multi/address
+ low_byte = spi.write(0);
+ high_byte = spi.write(0);
+ Buff = high_byte << 8 |low_byte;
+ SPI_CSG = 1; //end spi talking
+// Wz = Buff * Gpz + Gdz;
+ Wz = lpf(Buff * Gpz + Gdz, Wz, 48.0f);
+ //Ax
+ SPI_CSXM = 0; //start spi talking Ax
+ spi.write(0xE8); //read B11101000 read/multi/address
+ low_byte = spi.write(0);
+ high_byte = spi.write(0);
+ Buff = high_byte << 8 |low_byte;
+ SPI_CSXM = 1; //end spi talking
+ Ax = lpf(Buff*Apx + Adx, Ax, 13.0f);
+ //Ay
+ SPI_CSXM = 0; //start spi talking Ax
+ spi.write(0xEA); //read B11101010 read/multi/address
+ low_byte = spi.write(0);
+ high_byte = spi.write(0);
+ Buff = high_byte << 8 |low_byte;
+ SPI_CSXM = 1; //end spi talking
+ Ay = lpf(Buff*Apy + Ady, Ay, 13.0f);
+ //Az
+ SPI_CSXM = 0; //start spi talking Ax
+ spi.write(0xEC); //read B11101100 read/multi/address
+ low_byte = spi.write(0);
+ high_byte = spi.write(0);
+ Buff = high_byte << 8 |low_byte;
+ SPI_CSXM = 1; //end spi talking
+ Az = lpf(Buff*Apz + Adz, Az, 13.0f);
+ //Debug.printf("%.4f,%.4f,%.4f\r", Ax, Ay, Az);
+}
+//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of read_IMU funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
+
+
+
+//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓state_update funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
+void state_update(void) //estimation of new attitude
+{
+ //predict
+ 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 = 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));
+}
+//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of state_update 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↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
+
+
+//≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Rx_irq funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡//
+void Rx_irq(void)
+{
+ SerBuff[0] = Debug.getc();
+ Debug.putc(SerBuff[0]);
+}
+//■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Rx_irq funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//