Department of Electrical Eng University of Moratuwa
bilateral system
Diff: main.cpp
- Revision:
- 2:a921792d9913
- Parent:
- 1:db36f62f783b
--- a/main.cpp Mon Aug 19 18:01:21 2019 +0000
+++ b/main.cpp Fri Aug 30 11:55:47 2019 +0000
@@ -1,77 +1,196 @@
+/*
+* Force Controlled Vibration Analysis -
+*
+* 22.08.2019
+* Theekshana
+*/
+
#include "mbed.h"
#include "rtos.h"
#include "SDFileSystem.h"
#include "qeihw.h"
-//------------------------------------------------------------------------------
-Serial pc(USBTX, USBRX);
+
+//---------------------------=Communication Pins=-------------------------------
+//Serial pc(USBTX, USBRX);
DigitalOut led3(LED3);
DigitalOut led1(LED1);
SDFileSystem sd(p5, p6, p7, p8, "sd"); // pintout for sd card
-//-------------------------=Thread Variables=-----------------------------------
+
+//--------------------------=Motor Driver Controller Pin =-----------------------------
+DigitalOut Reset_AB(p29); // H bridge enable
+DigitalOut Reset_CD(p30);
+DigitalOut Reset_AB_s(p27);
+DigitalOut Reset_CD_s(p28);
+
+PwmOut pwm_clk_s(p21); // clockwise rotation pwm pin for Slave
+PwmOut pwm_anticlk_s(p22);
+PwmOut pwm_clk(p23); // clockwise rotation pwm pin for Master
+PwmOut pwm_anticlk(p24);
+
+//--------------------------=Current Sensor MBED Pins=--------------------------
+int Master_Direction=0;
+int slave_Direction=0;
+
+DigitalIn M_Dir(p10);
+DigitalIn S_Dir(p9);
+AnalogIn current_sensor_s_p(p15); // current sensor input for MASTER positive
+AnalogIn current_sensor_s_n(p16);
+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;
+
+//--------------------------=Ethernet Variable=---------------------------------
+Ethernet eth;
+
+
+//--------------------------=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=-------------------------
+
+//--------------------------=Velocity reading variables=-------------------------
char buf[0x600];
float recv_angle = 0.0;
float x_res = 0.0;
-float dt = 0.000001*150.0;
+float dt = 0.000001*200.0;
float ve_sum = 0.0;
float dx_res = 0.0;
-float G_filter_v = 100.0;
+float G_filter_v = 150.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=--------------------------------------
+
+//--------------------------=DoB variables=--------------------------------------
float K_tn=0.135;
float J_n = 0.0000720;
-float g_dis = 100.0;
+float B = 0.0;
+
+float g_dis = 1.0;
float D_DoB = 0.0;
float D_BFilter = 0.0;
float D_AFilter = 0.0;
-//-------------------------=Ethernet Variables=---------------------------------
-Ethernet eth;
+float I_DoB = 0.0;
+float I_sin = 0.0;
+float I_sin_prev = 0.0;
+float I_DoB_prev = 0.0;
+
+//--------------------------=RTOB Variables=-------------------------------------
+float RT_BFilter=0.0;
+float RT_AFilter = 0.0;
+float RTOB = 0.0;
+
+//--------------------------=Force Controller Variables=-------------------------
+
+
+float T_f = 0.025;
+float K_pf = 500.0; // tested value for smooth oparation
+float K_df = 0.0;
+float K_if = 0.0;
+float I_error = 0.0;
+float I_err_sum = 0.0;
+float I_err_prev = 0.0;
+float I_cmd = 1.3;
+float I_PID = 0.0;
+float I_ref = 0.0;
+
+//-------------------------=slave current controller variables=-----------------
+int count = 0;
+float f = 10.0;
+float K_pis =10.0;
+float K_dis = 0.0;
+float K_iis = 0.0;
+
+float I_error_s = 0.0;
+float I_ref_s = 0.0;
+float I_err_sum_s = 0.0;
+float I_PID_s = 0.0;
+float I_err_prev_s = 0.0;
+float duty_s = 0.0;
+float I_sin_gen = 0.0;
+float I_res_s = 0.0;
+float I1_act_s = 0.0;
+float I_msrd_s = 0.0;
+
-//-------------------------=Current controller parameters=----------------------
-int Master_Direction=0;
-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=-----------------------------------
+//--------------------------=Fucion declaration=--------------------------------
+void controlLoop();
+void readVelocity();
+void readCurrent();
+void CController();
+void DoB();
+void motorPWM_init();
+void motorPWM();
+
+void slaveCurrentRead();
+void slaveCurrentController();
+void motorpwm_s();
+
+void thread_2(void const *argument);
+void sd_card_write_test();
+void sd_card_write();
+void ethernet_init();
+
+
+
+
+//----------------------------------=Main Loop=---------------------------------
+int main()
+{
+ sd_card_write_test();
+ ethernet_init();
+ motorPWM_init();
+ timer.start();
+ time_up.attach(&controlLoop, dt);
+ Thread thread(*thread_2,NULL,osPriorityAboveNormal,DEFAULT_STACK_SIZE*10.0,NULL);
+}
+
+//--------------------------=Main Control Loop=---------------------------------
+void controlLoop(){
+ /*if (x_res>0.0)
+ {
+ T_f = -0.025;
+ }
+ else
+ {
+ T_f = 0.025;
+ }*/
+ count = count+1;
+ I_sin_gen = 0.25*sin(2*3.14159*dt*f*count);
+ readVelocity();
+ readCurrent();
+ CController();
+ DoB();
+ motorPWM();
+ slaveCurrentRead();
+ slaveCurrentController();
+ motorpwm_s();
+}
+
+//--------------------------=Read 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(){
+ 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();
-
- if(Master_Direction == 0){ //master clockwise
+ //master clockwise
+ if(Master_Direction == 0)
+ {
I_res_m = current_sensor_m_p.read();
I1_act_m = -1.0*((I_res_m*3.3/0.7) ); //0.74787687701613 //0.717075441532258
@@ -80,63 +199,148 @@
I1_act_m = 1.0*((I_res_m*3.3)/0.7); //0.713239227822580
}
I_msrd += G_Cfilter*(I1_act_m-I_msrd)*dt;
- }
-
-//-----------------------------=Current Controller PID=-------------------------
-void currentPID(){
+}
+
+
+
+//--------------------------=Velocity Controller=-------------------------------
+void CController()
+{
+ I_ref = I_cmd;// -I_sin_gen;
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_sum += I_error*dt;
+ I_PID = (K_pf*I_error)+(K_df*(I_error-I_err_prev)/dt) + (K_if*I_err_sum);
I_err_prev = I_error;
- }
-//-----------------------------=Motor PWM Initialization=-----------------------
-void motorPWM_init(){
+}
+
+//--------------------------=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);
+ I_DoB = D_DoB/K_tn;
+ I_sin = I_DoB-I_DoB_prev+ 0.999998*I_sin_prev;
+ I_DoB_prev = I_DoB;
+ I_sin_prev = I_sin;
+//--------------------------=Reaction Force Observer=------------------------------
+ RT_BFilter = D_BFilter - (T_f+B*dx_res);
+ RT_AFilter += g_dis*(RT_BFilter-RT_AFilter)*dt;
+ RTOB = RT_AFilter - (dx_res*J_n*g_dis);
+}
+
+
+//--------------------------=Motor PWM Initialization=--------------------------
+void motorPWM_init()
+{
pwm_clk.period_us(10);
+ pwm_clk_s.period_us(10);
+ pwm_anticlk_s.period_us(10);
pwm_anticlk.period_us(10);
pwm_clk.write(0.0f);
pwm_anticlk.write(0.0f);
+ pwm_clk_s.write(0.0f);
+ pwm_anticlk_s.write(0.0f);
Reset_AB = 1;
Reset_CD = 1;
- }
-//-----------------------------=Motor PWM Generation=---------------------------
-void motorPWM(){
+ Reset_AB_s =1;
+ Reset_CD_s =1;
+}
+
+//--------------------------=Motor PWM Generation=------------------------------
+void motorPWM()
+{
duty = I_PID;
- if (duty> 0.0) {
- if (duty > 0.5) {
- duty = 0.5;
+ if (duty> 0.0)
+ {
+ if (duty > 0.9)
+ {
+ duty = 0.9;
}
pwm_clk = 0.0;
pwm_anticlk = duty;
}
- if (duty < 0.0) {
- if (duty< -0.5) {
- duty = -0.5;
+ if (duty < 0.0)
+ {
+ if (duty< -0.9)
+ {
+ duty = -0.9;
}
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);
-
+}
+
+//---------------------------=Slave motor=--------------------------------------
+//---------------------------=Slave motor current reading=----------------------
+void slaveCurrentRead()
+{
+ slave_Direction = S_Dir.read();
+
+ if(slave_Direction == 0)
+ {
+ I_res_s = current_sensor_s_p.read();
+ I1_act_s = -1.0*((I_res_s*3.3/0.7) );//0.74787687701613 //0.717075441532258
+
+ }else if(slave_Direction == 1) { //master anticlockwise
+ I_res_s = current_sensor_s_n.read();
+ I1_act_s = 1.0*((I_res_s*3.3)/0.7); //0.713239227822580
}
-//--------------------------=Control Loop=--------------------------------------
-void controlLoop(){
- tempTime = timer.read_us();
- startTime = tempTime-preTime;
- preTime = tempTime;
- readVelocity();
- readCurrent();
- currentPID();
- motorPWM();
- DoB();
-
+ I_msrd_s += G_Cfilter*(I1_act_s-I_msrd_s)*dt;
+
+}
+
+//-----------------------=slaveCurrentController=-------------------------------
+void slaveCurrentController()
+{
+ I_ref_s = I_sin_gen; // 2 x PI x t x
+ I_error_s = I_ref_s - I_msrd_s;
+ I_err_sum_s += I_error_s*dt;
+ I_PID_s = (K_pis*I_error_s)+(K_dis*(I_error_s-I_err_prev_s)/dt) + (K_iis*I_err_sum_s);
+ I_err_prev_s = I_error_s;
}
-//------------------------------=File print test=-------------------------------
+
+//----------------------=Slave Motor run=---------------------------------------
+void motorpwm_s(){
+ duty_s = I_PID_s;
+ if (duty_s> 0.0)
+ {
+ if (duty_s > 0.9)
+ {
+ duty_s = 0.9;
+ }
+ pwm_clk_s = 0.0;
+ pwm_anticlk_s = duty_s;
+ }
+
+ if (duty_s < 0.0)
+ {
+ if (duty_s< -0.9)
+ {
+ duty_s = -0.9;
+ }
+ pwm_anticlk_s = 0.0;
+ pwm_clk_s = -1.0 * duty_s;
+ }
+}
+
+
+//--------------------------=Data writting to file=-----------------------------
+//--------------------------=Printing to a file=--------------------------------
+void thread_2(void const *argument)
+{
+ while(1)
+ {
+
+ //pc.printf("%f %f\n",I1_act_m,I_msrd);
+ //pc.printf("%d\t %f\t %f\t %f\t %f\t\r\n",count,I_cmd,I_ref,I_msrd,x_res);
+ sd_card_write();
+ }
+}
+
+
+//--------------------------=File print test=-----------------------------------
void sd_card_write_test()
{
mkdir("/sd/mydir", 0777);
@@ -145,33 +349,23 @@
if(fp == NULL) {
error("Could not open file for write\n");
}
- fprintf(fp, "Hello fun SD Card World!");
- fclose(fp);
+ fprintf(fp, "count \t x_res\t RTOB \n");
+ fclose(fp);
//fprintf(fp,"startTime\t I_ref\t I_msrd\t dx_res\t\n");
-
}
-//----------------------------=File print=--------------------------------------
+//--------------------------=File print=----------------------------------------
void sd_card_write()
{
- led1=!led1;
+ //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);
+ fprintf(fp,"%d\t %f\t %f\t \r\n",count,I_sin,I_sin_gen);
fclose(fp);
}
-//------------------------=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();
- }
-}
-//-----------------------------=Ethernet Initialization=------------------------
+//--------------------------=Ethernet Initialization=---------------------------
+
void ethernet_init()
{
eth.set_link(Ethernet::HalfDuplex100);
@@ -185,14 +379,3 @@
}
}
}
-
-//----------------------------------=Main Loop=---------------------------------
-int main()
-{
- //sd_card_write_test();
- ethernet_init();
- motorPWM_init();
- timer.start();
- time_up.attach(&controlLoop, dt);
- Thread thread(*thread_2,NULL,osPriorityAboveNormal,DEFAULT_STACK_SIZE*10.0,NULL);
-}
\ No newline at end of file