Department of Electrical Eng University of Moratuwa
bilateral system
Diff: main.cpp
- Revision:
- 0:5459cdde6298
- Child:
- 1:db36f62f783b
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Thu Aug 01 08:00:33 2019 +0000
@@ -0,0 +1,411 @@
+#include "mbed.h"
+#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();
+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
+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);
+
+ 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
+ I_res_m = current_sensor_m_p.read();
+ I1_act_m = 1.0*((I_res_m*3.3/0.717075441532258) ); //0.74787687701613
+
+ }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
+ }
+ //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_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;
+
+ pwm_I_S=((I_err_s * Ikp_s) + I_tmp_s + (d_I_s * Ikd_s));
+}
+
+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
+}
+
+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;
+}
+
+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;
+ }
+}
+
+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();
+ timer.start();
+ dt=dt_us/1000000.0;
+
+ ticker.attach_us(&Control_body, dt_us);
+ Thread thread(*thread_2,NULL,osPriorityAboveNormal,DEFAULT_STACK_SIZE*10.0,NULL);
+ }
+
\ No newline at end of file