Department of Electrical Eng University of Moratuwa
rtos sd encoder systems
Diff: main.cpp
- Revision:
- 1:db36f62f783b
- Parent:
- 0:5459cdde6298
- Child:
- 2:00c6646f42fb
- Child:
- 3:a921792d9913
--- a/main.cpp Thu Aug 01 08:00:33 2019 +0000
+++ b/main.cpp Mon Aug 19 18:01:21 2019 +0000
@@ -2,410 +2,197 @@
#include "rtos.h"
#include "SDFileSystem.h"
#include "qeihw.h"
-
-#define Kp 80000.0 //8000.0 //max 100,000 //7000 //100000
-#define Kv 50.0 //50.0 //500.0 //500
-#define Gd 300.0 //200
-#define PI 3.141592653
-
-QEIHW qei_s(QEI_DIRINV_NONE, QEI_SIGNALMODE_QUAD, QEI_CAPMODE_4X, QEI_INVINX_NONE );
-
-//LocalFileSystem local("local");
-int position_m = 0;
-int position = 0;
-int position_s = 0;
-float encoder_m_prv=0.0,encoder_s_prv=0.0;
-
-float const G_filcon_I1_m = 350.0;
-float const G_filcon_I1_s = 350.0;
-float I1_act_m=0.0;
-float I1_act_s=0.0;
-
-// Mutex stdio_mutex;
-// Configuring two encoder modules
-void ethernet_init();
+//------------------------------------------------------------------------------
+Serial pc(USBTX, USBRX);
+DigitalOut led3(LED3);
+DigitalOut led1(LED1);
+SDFileSystem sd(p5, p6, p7, p8, "sd"); // pintout for sd card
+//-------------------------=Thread Variables=-----------------------------------
+Ticker time_up;
+float startTime=0.0;
+float tempTime = 0.0;
+float endTime=0.0;
+Timer timer;
+float preTime = 0.0;
+//-------------------------=Velocity reading variables=-------------------------
+char buf[0x600];
+float recv_angle = 0.0;
+float x_res = 0.0;
+float dt = 0.000001*150.0;
+float ve_sum = 0.0;
+float dx_res = 0.0;
+float G_filter_v = 100.0;
+float duty = 0.0;
+//-------------------------=Current PID Variables=------------------------------
+float K_pi=25.0;
+float K_di=0.0;
+float K_ii=1.0;
+float I_ref = 0.02;
+float I_error = 0.0;
+float I_PID = 0.0;
+float I_err_sum = 0.0;
+float I_err_prev = 0.0;
+//--------------------------=Motor Variables=-----------------------------------
+DigitalOut Reset_AB(p29);
+DigitalOut Reset_CD(p30);
+PwmOut pwm_clk(p23);
+PwmOut pwm_anticlk(p24);
+//-------------------------=DoB variables=--------------------------------------
+float K_tn=0.135;
+float J_n = 0.0000720;
+float g_dis = 100.0;
+float D_DoB = 0.0;
+float D_BFilter = 0.0;
+float D_AFilter = 0.0;
+//-------------------------=Ethernet Variables=---------------------------------
Ethernet eth;
-//Variable for get angle from ethernet
-char buf[0x600];
-float recv_m_angle = 0.0;
-float recv_s_angle = 0.0;
-float inc_now = 0.0, inc_pre = 0.0;
-
-// Safety for mbed unused pins
-DigitalIn safety_19(p19);
-DigitalIn safety_20(p20);
-DigitalIn safety_25(p25);
-DigitalIn safety_26(p26);
-
-PwmOut pwm_m_clk(p21); // clockwise rotation pwm pin for MASTER
-PwmOut pwm_m_anticlk(p22); // anti - clockwise rotation pwm pin for MASTER
-PwmOut pwm_s_clk(p23); // clockwise rotation pwm pin for SLAVE
-PwmOut pwm_s_anticlk(p24); // anti-clockwise rotation pwm pin for SLAVE
-
-DigitalOut Reset_AB_M(p27);
-DigitalOut Reset_CD_M(p28);
-DigitalOut Reset_AB_S(p29);
-DigitalOut Reset_CD_S(p30);
-
-DigitalIn M_Dir(p9);
-DigitalIn S_Dir(p10);
-AnalogIn current_sensor_m_p(p15); // current sensor input for MASTER positive
-AnalogIn current_sensor_m_n(p16); // current sensor input for MASTER negative
-AnalogIn current_sensor_s_p(p17); // current sensor input for SLAVE positive
-AnalogIn current_sensor_s_n(p18); // current sensor input for SLAVE negative
-
-DigitalOut led1(LED1);
-
-DigitalOut led3(LED3);
-
-Timer timer; // For the controller
-FILE *fp;
-Ticker ticker;
-//Mutex stdio_mutex;
-
-int dt_us= 150, ramp_time=0.0; // define main loop time in us
-float dt; //loop time in seconds for calculations
-
-int counter_time;
-int counter=0;
-int counter_old=0;
-
-//Current Sensor Directions
+//-------------------------=Current controller parameters=----------------------
int Master_Direction=0;
-int Slave_Direction = 0;
-
-// Encoder Constants
-float const encoder_pulses_s = 2400.0;
-
-// PID parameters for Current - Loop
-
-//float const Ikp_m = 1.0, Iki_m =2.0, Ikd_m = 0.01;
-//float const Ikp_m = 0.01/*0.09*/, Iki_m =0.00, Ikd_m = 0.001;
-//float const Ikp_s = 0.04/*4.5*/, Iki_s = 0.00, Ikd_s = 0.01;
-float const Ikp_m = 0.026/*0.2*/, Iki_m =0.00, Ikd_m = 0.0001; //j=0.0000720,kp=0.026
-float const Ikp_s = 0.026/*0.2*/, Iki_s = 0.00, Ikd_s = 0.0001;
-
-
-float I_ref_m = 0.0, I_err_m = 0.0, I_res_m = 0.0, I_tmp_m = 0.0, tem_I_m = 0.0, d_I_m = 0.0,I_ref_m1 = 0.0;
-float I_ref_s = 0.0, I_err_s = 0.0, I_res_s = 0.0, I_tmp_s = 0.0, tem_I_s = 0.0, d_I_s = 0.0,I_ref_s1 = 0.0;
-//some may not use..
-float v_ref_m = 00.0, v_err_m = 0.0, v_res_m = 0.0,v_res_m_rpm = 0.0, v_tmp_m = 0.0, tem_v_m = 0.0, d_v_m = 0.0;
-float v_ref_s = 00.0, v_err_s = 0.0, v_res_s = 0.0,v_res_s_rpm = 0.0, v_tmp_s = 0.0, tem_v_s = 0.0, d_v_s = 0.0;
-
-float De_m=0.0, dx_res_m = 0.0, prev_v_err_m=0.0,I_v_err_m=0.0,de_m =0.0;
-float De_s=0.0, dx_res_s = 0.0, prev_v_err_s=0.0,I_v_err_s=0.0,de_s =0.0;
-
-float duty_m = 0.0; // PWM duty for master
-float duty_s = 0.0;
-
-float const G_filcon_I_m = 100.0; // Low pass filter gain for Current - loop default 1
-float const G_filcon_I_s = 100.0;
-
-float const G_filcon_v_m = 100.0; // Low pass filter gain Velocity estimation
-float const G_filcon_v_s = 100.0;
-
-float I_act_m = 0.0; // Storing actual current flow
-float I_act_s = 0.0;
-
-float x_res_m_prv = 0.0; // Parameters to calculate current rotational speed
-float x_res_s_prv = 0.0;
-float encoder_m_now = 0.0;
-float encoder_s_now = 0.0;
-
-// Motor Constant and Inertia
-float const J_const_m = 0.0000720;//0.0000010;// //0.0000800;//0.000072
-float const J_const_s = 0.0000720;//0.0000010;// //0.0000800;
-float const Kt_const_m = 0.135;
-float const Kt_const_s = 0.135;//0.134
-float const Kt_constinv_m = 7.407;//1.43;
-float const Kt_constinv_s = 7.463;//1.43;
-
-float ddx_ref_m=0.0, ddx_ref_s=0.0;
-
-float tmp_m = 0.0,ob_sum_m = 0.0;
-float tmp_s = 0.0,ob_sum_s = 0.0;
-float i_com_m = 0.0;
-float i_com_s = 0.0;
-float fric_m = 0.0,fric_s = 0.0,i_rto_m = 0.0,i_rto_s = 0.0;
-
-float x_res_m = 0.0;
-float x_res_s = 0.0;
-float ve_sum_m = 0.0;
-float ve_sum_s = 0.0;
-float pwm_I_M= 0.0;
-float pwm_I_S= 0.0;
-float pid_V_I_M= 0.0;
-float pid_V_I_S= 0.0;
-
-
-float temp_a=0.0;
-float temp_b=0.0;
-float temp_a2=0.0;
-float temp_b2=0.0;
-float g_a1=1.0;
-float g_b1=1.4;
-float g_b2=1.4;
-float x_res_s_filter=0.0;
-float x_res_m_filter=0.0;
-float dx_res_s_filter=0.0;
-float dx_res_m_filter=0.0;
-float ob_sum_m1=0.0;
-float ob_sum_s1=0.0,torque_dob_m=0.0,torque_dob_s=0.0;
-//float error=0.0;
-
- void pwm_init(void) {
-
- pwm_m_clk.period_us(10);
- pwm_m_anticlk.period_us(10);
- pwm_s_anticlk.period_us(10);
- pwm_s_clk.period_us(10);
-
- pwm_m_clk.write(0.0f); // Set the ouput duty-cycle, specified as a percentage (float)
- pwm_m_anticlk.write(0.0f);
- pwm_s_anticlk.write(0.0f);
- pwm_s_clk.write(0.0f);
+DigitalIn M_Dir(p10);
+AnalogIn current_sensor_m_p(p17); // current sensor input for MASTER positive
+AnalogIn current_sensor_m_n(p18); // current sensor input for MASTER negative
+float I_res_m = 0.0;
+float I1_act_m=0.0;
+float I_msrd=0.0;
+float G_Cfilter=30.0;
+//---------------------------------=Velocity=-----------------------------------
+void readVelocity(){
+ int size2 = eth.receive();
+ if(size2 > 0)
+ {
+ eth.read(buf, size2);
+ memcpy(&recv_angle, buf, sizeof(float));
+ x_res = -1*recv_angle;
+ }
+ ve_sum += dx_res*dt;
+ dx_res =G_filter_v*( x_res-ve_sum);
+}//-----------------------------=Current Meassurement=--------------------------
+void readCurrent(){
+ Master_Direction = M_Dir.read();
- Reset_AB_M = 1; //ENABLE RUNNING MODE (H BRIDGE ENABLE)
- Reset_CD_M = 1;
- Reset_AB_S = 1;
- Reset_CD_S = 1;
-}
-
-void velocity_m() {
-
- int size2 = eth.receive();
-
- if(size2 > 0) {
- eth.read(buf, size2);
- memcpy(&recv_m_angle, buf, sizeof(float));
- x_res_m = recv_m_angle;
- }
-
- ve_sum_m += dx_res_m*dt;
- dx_res_m =G_filcon_v_m*( x_res_m-ve_sum_m);
-
-}
-
-void velocity_s() {
-
- int32_t temp; position = 0;
- float d_v_err_s=0.0;
- qei_s.SetDigiFilter(480UL);
- qei_s.SetMaxPosition(0xFFFFFFFF);
- position = qei_s.GetPosition();
-
- x_res_s = -1.0*position * 2.0 * PI / encoder_pulses_s;
- ve_sum_s += dx_res_s*dt;
- dx_res_s = G_filcon_v_s*(x_res_s-ve_sum_s);
-
-}
-
-void current_pid_m(){
- Master_Direction = M_Dir.read();
-
- if(Master_Direction == 0){ //master clockwise
+ if(Master_Direction == 0){ //master clockwise
I_res_m = current_sensor_m_p.read();
- I1_act_m = 1.0*((I_res_m*3.3/0.717075441532258) ); //0.74787687701613
+ I1_act_m = -1.0*((I_res_m*3.3/0.7) ); //0.74787687701613 //0.717075441532258
}else if(Master_Direction == 1) { //master anticlockwise
I_res_m = current_sensor_m_n.read();
- I1_act_m = -1.0*((I_res_m*3.3)/0.717075441532258); //0.713239227822580
+ I1_act_m = 1.0*((I_res_m*3.3)/0.7); //0.713239227822580
}
- //I_act_m += G_filcon_I1_m*(I1_act_m-I_act_m)*dt;
- I_act_m = 1*I1_act_m;
-
- I_err_m = I_ref_m - I_act_m;
- I_tmp_m += (Iki_m * dt * I_err_m);
-
- tem_I_m += G_filcon_I_m*d_I_m*dt;
- d_I_m = I_err_m - tem_I_m;
-
- pwm_I_M=((I_err_m * Ikp_m) + I_tmp_m + (d_I_m * Ikd_m));
- }
-
-void current_pid_s(){
- Slave_Direction = S_Dir.read();
- if(Slave_Direction == 0){ //slave clockwise
- I_res_s = current_sensor_s_p.read();
- I1_act_s = 1.0*((I_res_s*3.3)/0.717075441532258 ); //0.717075441532258
- }else{
- I_res_s = current_sensor_s_n.read(); //slave anticlockwise
- I1_act_s = -1.0*((I_res_s*3.3)/0.717075441532258 ); //0.724138445564516
+ I_msrd += G_Cfilter*(I1_act_m-I_msrd)*dt;
}
- //I_act_s += G_filcon_I1_s*(I1_act_s-I_act_s)*dt;
- I_act_s = I1_act_s;
-
- I_err_s = I_ref_s - I_act_s;
- I_tmp_s += Iki_s * dt * I_err_s;
- tem_I_s += G_filcon_I_s*d_I_s*dt;
- d_I_s = I_err_s - tem_I_s;
+//-----------------------------=Current Controller PID=-------------------------
+void currentPID(){
+ I_error = I_ref - I_msrd;
+ I_err_sum+= (I_error*dt);
+ I_PID = (K_pi * I_error)+(K_di*(I_error-I_err_prev)/dt)+(K_ii*I_err_sum);
+ I_err_prev = I_error;
+ }
+//-----------------------------=Motor PWM Initialization=-----------------------
+void motorPWM_init(){
+ pwm_clk.period_us(10);
+ pwm_anticlk.period_us(10);
+ pwm_clk.write(0.0f);
+ pwm_anticlk.write(0.0f);
+ Reset_AB = 1;
+ Reset_CD = 1;
+ }
+//-----------------------------=Motor PWM Generation=---------------------------
+void motorPWM(){
+ duty = I_PID;
+ if (duty> 0.0) {
+ if (duty > 0.5) {
+ duty = 0.5;
+ }
+ pwm_clk = 0.0;
+ pwm_anticlk = duty;
+ }
- pwm_I_S=((I_err_s * Ikp_s) + I_tmp_s + (d_I_s * Ikd_s));
+ if (duty < 0.0) {
+ if (duty< -0.5) {
+ duty = -0.5;
+ }
+ pwm_anticlk = 0.0;
+ pwm_clk = -1.0 * duty;
+ }
+ }
+//--------------------------=Distarbance Observer=------------------------------
+void DoB(){
+ D_BFilter=(I_msrd*K_tn) + (dx_res*J_n*g_dis);
+ D_AFilter += g_dis*(D_BFilter-D_AFilter)*dt;
+ D_DoB = D_AFilter - (dx_res*J_n*g_dis);
+
+ }
+//--------------------------=Control Loop=--------------------------------------
+void controlLoop(){
+ tempTime = timer.read_us();
+ startTime = tempTime-preTime;
+ preTime = tempTime;
+ readVelocity();
+ readCurrent();
+ currentPID();
+ motorPWM();
+ DoB();
+
+}
+//------------------------------=File print test=-------------------------------
+void sd_card_write_test()
+{
+ mkdir("/sd/mydir", 0777);
+
+ FILE *fp = fopen("/sd/mydir/sdtest.txt", "w");
+ if(fp == NULL) {
+ error("Could not open file for write\n");
+ }
+ fprintf(fp, "Hello fun SD Card World!");
+ fclose(fp);
+
+ //fprintf(fp,"startTime\t I_ref\t I_msrd\t dx_res\t\n");
+
}
-void Disob() {
-
- tmp_m = Gd*J_const_s*dx_res_m;
- //ob_sum_m += (Gd*(Kt_const_s*I_act_m+tmp_m-ob_sum_m)*dt);
- ob_sum_m1 = Kt_const_m*I_act_m+tmp_m;
- ob_sum_m += Gd*(ob_sum_m1-ob_sum_m)*dt;
- i_com_m = (ob_sum_m - tmp_m)*Kt_constinv_m; //read current
- //i_com_m=0.0;
- torque_dob_m= i_com_m*Kt_const_m;
-
- fric_m = 0.0;
- i_rto_m = (i_com_m-fric_m); //friction should be in current terms
-
- tmp_s = Gd*J_const_s*dx_res_s;
- //ob_sum_s += (Gd*(Kt_const_s*I_act_s+tmp_s-ob_sum_s)*dt);
- ob_sum_s1 = Kt_const_s*I_act_s+tmp_s;
- ob_sum_s += Gd*(ob_sum_s1-ob_sum_s)*dt;
- i_com_s = (ob_sum_s - tmp_s)*Kt_constinv_s; //read current
- //i_com_s = 0.0;
- torque_dob_s = i_com_s*Kt_const_s;
- fric_s = 0.0;
- i_rto_s = (i_com_s-fric_s); //friction should be in current terms
+//----------------------------=File print=--------------------------------------
+void sd_card_write()
+{
+ led1=!led1;
+ FILE *fp = fopen("/sd/mydir/sdtest.txt","a");
+ fprintf(fp,"%f\t %f\t %f\t %f\t\r\n",startTime, I_ref, I_msrd, dx_res);
+ fclose(fp);
}
-int Controller(void) {
-
- ddx_ref_m = Kp*(x_res_s-x_res_m) + Kv*(dx_res_s-dx_res_m);
- I_ref_m1 = Kt_constinv_m*J_const_m*ddx_ref_m;
- I_ref_m = I_ref_m1 + i_com_m - (i_rto_m+i_rto_s);
-
- //I_ref_m = 0.1;
- ddx_ref_s = Kp*(x_res_m-x_res_s) + Kv*(dx_res_m-dx_res_s);
-
- I_ref_s1 = Kt_constinv_s*J_const_s*ddx_ref_s;
- I_ref_s =I_ref_s1 + i_com_s - (i_rto_s+i_rto_m);
-
- //I_ref_s = 0.1;
-
- return 0;
+//------------------------=Printing to a file=----------------------------------
+void thread_2(void const *argument)
+{
+ while(1)
+ {
+ //pc.printf("%f %f\n",I1_act_m,I_msrd);
+ pc.printf("%f %f %f %f %f %f \n",startTime,I_ref,I_msrd,I_PID,x_res,D_DoB);
+ //sd_card_write();
+ }
}
-
-void PWM_Generator_m() {
- duty_m = pwm_I_M;
-
- if (duty_m> 0.0) {
- if (duty_m > 0.5) {
- duty_m = 0.5;
- }
- pwm_m_clk = 0.0;
- pwm_m_anticlk = duty_m;
- }
-
- if (duty_m < 0.0) {
- if (duty_m < -0.5) {
- duty_m = -0.5;
- }
- pwm_m_anticlk = 0.0;
- pwm_m_clk = -1.0 * duty_m;
+//-----------------------------=Ethernet Initialization=------------------------
+void ethernet_init()
+{
+ eth.set_link(Ethernet::HalfDuplex100);
+ wait_ms(1000); // Needed after startup.
+ if(eth.link())
+ {
+ for(int i=0;i<3;i++)
+ {
+ led3=!led3;
+ wait(1.0);
+ }
}
}
-void PWM_Generator_s() {
- duty_s = pwm_I_S;
-
- if (duty_s > 0.0) {
- if (duty_s > 0.5) {
- duty_s = 0.5;
- }
- pwm_s_clk = 0.0;
- pwm_s_anticlk = duty_s;
- }
-
- if (duty_s < 0.0) {
- if (duty_s < -0.5) {
- duty_s = -0.5;
- }
- pwm_s_anticlk = 0.0;
- pwm_s_clk = -1.0 * duty_s;
- }
-}
-
-void cleanup_module(void){
-
- pwm_m_clk = 0.0; // pwm cleanup module
- pwm_m_anticlk = 0.0;
- pwm_s_anticlk = 0.0;
- pwm_s_clk = 0.0;
-
- Reset_AB_M = 0; //RESET H BRIDGE
- Reset_CD_M = 0;
- Reset_AB_S = 0;
- Reset_CD_S = 0;
-
- led1=0;
- led3=0;
-}
-
-//RTOS
-
-void Control_body() {
- // Control Part - main code
- Controller();
- velocity_m();
- velocity_s ();
- current_pid_m();
- current_pid_s ();
- PWM_Generator_m();
- PWM_Generator_s();
- Disob();
- //led1=!led1;
- counter++;
- }
-
-void thread_2(void const *argument){
- led1=1;
- SDFileSystem sd(p5, p6, p7, p8, "sd");
- FILE *fp = fopen("/sd/BC.txt", "w");
-
- if(fp == NULL) {
- for(int i=0;i<5;i++){
- led3=!led3;
- wait(1.0);
- }
- }
-
- while(counter<80000){
- if(counter>=(counter_old+100)){
- //fprintf(fp, "%d %f %f %f %f %f %f %f %f \n",timer.read_us(), x_res_m,x_res_s,I_act_m,I_act_s,torque_dob_s,torque_dob_m,i_com_s,i_com_m);
- fprintf(fp, "%d %f %f %f %f \n",timer.read_us(),torque_dob_m,torque_dob_s,x_res_m,x_res_s);
- counter_old=counter;
- led3=!led3;
- }
- }
-
- fclose(fp);
- timer.stop();
- cleanup_module();
- ticker.detach ();
-
- wait(1.0);
- }
-
- void ethernet_init(){
- eth.set_link(Ethernet::HalfDuplex100);
- wait_ms(1000); // Needed after startup.
- if(eth.link()) {
- for(int i=0;i<3;i++){
- led3=!led3;
- wait(1.0);
- }
- }
-}
-
-int main() {
-
- ethernet_init();
- pwm_init();
+//----------------------------------=Main Loop=---------------------------------
+int main()
+{
+ //sd_card_write_test();
+ ethernet_init();
+ motorPWM_init();
timer.start();
- dt=dt_us/1000000.0;
-
- ticker.attach_us(&Control_body, dt_us);
+ time_up.attach(&controlLoop, dt);
Thread thread(*thread_2,NULL,osPriorityAboveNormal,DEFAULT_STACK_SIZE*10.0,NULL);
- }
-
\ No newline at end of file
+}
\ No newline at end of file