Department of Electrical Eng University of Moratuwa / Mbed 2 deprecated Vibration_Analysis_Force_Control

Force controlled vibration analysis

Dependencies:   mbed

main.cpp@0:5459cdde6298, 2019-08-01 (annotated)

Committer:
eembed
Date:
Thu Aug 01 08:00:33 2019 +0000
Revision:
0:5459cdde6298
Child:
1:db36f62f783b
first commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
eembed 0:5459cdde6298 1 #include "mbed.h"
eembed 0:5459cdde6298 2 #include "rtos.h"
eembed 0:5459cdde6298 3 #include "SDFileSystem.h"
eembed 0:5459cdde6298 4 #include "qeihw.h"
eembed 0:5459cdde6298 5
eembed 0:5459cdde6298 6 #define Kp 80000.0 //8000.0 //max 100,000 //7000 //100000
eembed 0:5459cdde6298 7 #define Kv 50.0 //50.0 //500.0 //500
eembed 0:5459cdde6298 8 #define Gd 300.0 //200
eembed 0:5459cdde6298 9 #define PI 3.141592653
eembed 0:5459cdde6298 10
eembed 0:5459cdde6298 11 QEIHW qei_s(QEI_DIRINV_NONE, QEI_SIGNALMODE_QUAD, QEI_CAPMODE_4X, QEI_INVINX_NONE );
eembed 0:5459cdde6298 12
eembed 0:5459cdde6298 13 //LocalFileSystem local("local");
eembed 0:5459cdde6298 14 int position_m = 0;
eembed 0:5459cdde6298 15 int position = 0;
eembed 0:5459cdde6298 16 int position_s = 0;
eembed 0:5459cdde6298 17 float encoder_m_prv=0.0,encoder_s_prv=0.0;
eembed 0:5459cdde6298 18
eembed 0:5459cdde6298 19 float const G_filcon_I1_m = 350.0;
eembed 0:5459cdde6298 20 float const G_filcon_I1_s = 350.0;
eembed 0:5459cdde6298 21 float I1_act_m=0.0;
eembed 0:5459cdde6298 22 float I1_act_s=0.0;
eembed 0:5459cdde6298 23
eembed 0:5459cdde6298 24 // Mutex stdio_mutex;
eembed 0:5459cdde6298 25 // Configuring two encoder modules
eembed 0:5459cdde6298 26 void ethernet_init();
eembed 0:5459cdde6298 27 Ethernet eth;
eembed 0:5459cdde6298 28
eembed 0:5459cdde6298 29 //Variable for get angle from ethernet
eembed 0:5459cdde6298 30 char buf[0x600];
eembed 0:5459cdde6298 31 float recv_m_angle = 0.0;
eembed 0:5459cdde6298 32 float recv_s_angle = 0.0;
eembed 0:5459cdde6298 33 float inc_now = 0.0, inc_pre = 0.0;
eembed 0:5459cdde6298 34
eembed 0:5459cdde6298 35 // Safety for mbed unused pins
eembed 0:5459cdde6298 36 DigitalIn safety_19(p19);
eembed 0:5459cdde6298 37 DigitalIn safety_20(p20);
eembed 0:5459cdde6298 38 DigitalIn safety_25(p25);
eembed 0:5459cdde6298 39 DigitalIn safety_26(p26);
eembed 0:5459cdde6298 40
eembed 0:5459cdde6298 41 PwmOut pwm_m_clk(p21); // clockwise rotation pwm pin for MASTER
eembed 0:5459cdde6298 42 PwmOut pwm_m_anticlk(p22); // anti - clockwise rotation pwm pin for MASTER
eembed 0:5459cdde6298 43 PwmOut pwm_s_clk(p23); // clockwise rotation pwm pin for SLAVE
eembed 0:5459cdde6298 44 PwmOut pwm_s_anticlk(p24); // anti-clockwise rotation pwm pin for SLAVE
eembed 0:5459cdde6298 45
eembed 0:5459cdde6298 46 DigitalOut Reset_AB_M(p27);
eembed 0:5459cdde6298 47 DigitalOut Reset_CD_M(p28);
eembed 0:5459cdde6298 48 DigitalOut Reset_AB_S(p29);
eembed 0:5459cdde6298 49 DigitalOut Reset_CD_S(p30);
eembed 0:5459cdde6298 50
eembed 0:5459cdde6298 51 DigitalIn M_Dir(p9);
eembed 0:5459cdde6298 52 DigitalIn S_Dir(p10);
eembed 0:5459cdde6298 53 AnalogIn current_sensor_m_p(p15); // current sensor input for MASTER positive
eembed 0:5459cdde6298 54 AnalogIn current_sensor_m_n(p16); // current sensor input for MASTER negative
eembed 0:5459cdde6298 55 AnalogIn current_sensor_s_p(p17); // current sensor input for SLAVE positive
eembed 0:5459cdde6298 56 AnalogIn current_sensor_s_n(p18); // current sensor input for SLAVE negative
eembed 0:5459cdde6298 57
eembed 0:5459cdde6298 58 DigitalOut led1(LED1);
eembed 0:5459cdde6298 59
eembed 0:5459cdde6298 60 DigitalOut led3(LED3);
eembed 0:5459cdde6298 61
eembed 0:5459cdde6298 62 Timer timer; // For the controller
eembed 0:5459cdde6298 63 FILE *fp;
eembed 0:5459cdde6298 64 Ticker ticker;
eembed 0:5459cdde6298 65 //Mutex stdio_mutex;
eembed 0:5459cdde6298 66
eembed 0:5459cdde6298 67 int dt_us= 150, ramp_time=0.0; // define main loop time in us
eembed 0:5459cdde6298 68 float dt; //loop time in seconds for calculations
eembed 0:5459cdde6298 69
eembed 0:5459cdde6298 70 int counter_time;
eembed 0:5459cdde6298 71 int counter=0;
eembed 0:5459cdde6298 72 int counter_old=0;
eembed 0:5459cdde6298 73
eembed 0:5459cdde6298 74 //Current Sensor Directions
eembed 0:5459cdde6298 75 int Master_Direction=0;
eembed 0:5459cdde6298 76 int Slave_Direction = 0;
eembed 0:5459cdde6298 77
eembed 0:5459cdde6298 78 // Encoder Constants
eembed 0:5459cdde6298 79 float const encoder_pulses_s = 2400.0;
eembed 0:5459cdde6298 80
eembed 0:5459cdde6298 81 // PID parameters for Current - Loop
eembed 0:5459cdde6298 82
eembed 0:5459cdde6298 83 //float const Ikp_m = 1.0, Iki_m =2.0, Ikd_m = 0.01;
eembed 0:5459cdde6298 84 //float const Ikp_m = 0.01/*0.09*/, Iki_m =0.00, Ikd_m = 0.001;
eembed 0:5459cdde6298 85 //float const Ikp_s = 0.04/*4.5*/, Iki_s = 0.00, Ikd_s = 0.01;
eembed 0:5459cdde6298 86 float const Ikp_m = 0.026/*0.2*/, Iki_m =0.00, Ikd_m = 0.0001; //j=0.0000720,kp=0.026
eembed 0:5459cdde6298 87 float const Ikp_s = 0.026/*0.2*/, Iki_s = 0.00, Ikd_s = 0.0001;
eembed 0:5459cdde6298 88
eembed 0:5459cdde6298 89
eembed 0:5459cdde6298 90 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;
eembed 0:5459cdde6298 91 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;
eembed 0:5459cdde6298 92 //some may not use..
eembed 0:5459cdde6298 93 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;
eembed 0:5459cdde6298 94 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;
eembed 0:5459cdde6298 95
eembed 0:5459cdde6298 96 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;
eembed 0:5459cdde6298 97 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;
eembed 0:5459cdde6298 98
eembed 0:5459cdde6298 99 float duty_m = 0.0; // PWM duty for master
eembed 0:5459cdde6298 100 float duty_s = 0.0;
eembed 0:5459cdde6298 101
eembed 0:5459cdde6298 102 float const G_filcon_I_m = 100.0; // Low pass filter gain for Current - loop default 1
eembed 0:5459cdde6298 103 float const G_filcon_I_s = 100.0;
eembed 0:5459cdde6298 104
eembed 0:5459cdde6298 105 float const G_filcon_v_m = 100.0; // Low pass filter gain Velocity estimation
eembed 0:5459cdde6298 106 float const G_filcon_v_s = 100.0;
eembed 0:5459cdde6298 107
eembed 0:5459cdde6298 108 float I_act_m = 0.0; // Storing actual current flow
eembed 0:5459cdde6298 109 float I_act_s = 0.0;
eembed 0:5459cdde6298 110
eembed 0:5459cdde6298 111 float x_res_m_prv = 0.0; // Parameters to calculate current rotational speed
eembed 0:5459cdde6298 112 float x_res_s_prv = 0.0;
eembed 0:5459cdde6298 113 float encoder_m_now = 0.0;
eembed 0:5459cdde6298 114 float encoder_s_now = 0.0;
eembed 0:5459cdde6298 115
eembed 0:5459cdde6298 116 // Motor Constant and Inertia
eembed 0:5459cdde6298 117 float const J_const_m = 0.0000720;//0.0000010;// //0.0000800;//0.000072
eembed 0:5459cdde6298 118 float const J_const_s = 0.0000720;//0.0000010;// //0.0000800;
eembed 0:5459cdde6298 119 float const Kt_const_m = 0.135;
eembed 0:5459cdde6298 120 float const Kt_const_s = 0.135;//0.134
eembed 0:5459cdde6298 121 float const Kt_constinv_m = 7.407;//1.43;
eembed 0:5459cdde6298 122 float const Kt_constinv_s = 7.463;//1.43;
eembed 0:5459cdde6298 123
eembed 0:5459cdde6298 124 float ddx_ref_m=0.0, ddx_ref_s=0.0;
eembed 0:5459cdde6298 125
eembed 0:5459cdde6298 126 float tmp_m = 0.0,ob_sum_m = 0.0;
eembed 0:5459cdde6298 127 float tmp_s = 0.0,ob_sum_s = 0.0;
eembed 0:5459cdde6298 128 float i_com_m = 0.0;
eembed 0:5459cdde6298 129 float i_com_s = 0.0;
eembed 0:5459cdde6298 130 float fric_m = 0.0,fric_s = 0.0,i_rto_m = 0.0,i_rto_s = 0.0;
eembed 0:5459cdde6298 131
eembed 0:5459cdde6298 132 float x_res_m = 0.0;
eembed 0:5459cdde6298 133 float x_res_s = 0.0;
eembed 0:5459cdde6298 134 float ve_sum_m = 0.0;
eembed 0:5459cdde6298 135 float ve_sum_s = 0.0;
eembed 0:5459cdde6298 136 float pwm_I_M= 0.0;
eembed 0:5459cdde6298 137 float pwm_I_S= 0.0;
eembed 0:5459cdde6298 138 float pid_V_I_M= 0.0;
eembed 0:5459cdde6298 139 float pid_V_I_S= 0.0;
eembed 0:5459cdde6298 140
eembed 0:5459cdde6298 141
eembed 0:5459cdde6298 142 float temp_a=0.0;
eembed 0:5459cdde6298 143 float temp_b=0.0;
eembed 0:5459cdde6298 144 float temp_a2=0.0;
eembed 0:5459cdde6298 145 float temp_b2=0.0;
eembed 0:5459cdde6298 146 float g_a1=1.0;
eembed 0:5459cdde6298 147 float g_b1=1.4;
eembed 0:5459cdde6298 148 float g_b2=1.4;
eembed 0:5459cdde6298 149 float x_res_s_filter=0.0;
eembed 0:5459cdde6298 150 float x_res_m_filter=0.0;
eembed 0:5459cdde6298 151 float dx_res_s_filter=0.0;
eembed 0:5459cdde6298 152 float dx_res_m_filter=0.0;
eembed 0:5459cdde6298 153 float ob_sum_m1=0.0;
eembed 0:5459cdde6298 154 float ob_sum_s1=0.0,torque_dob_m=0.0,torque_dob_s=0.0;
eembed 0:5459cdde6298 155 //float error=0.0;
eembed 0:5459cdde6298 156
eembed 0:5459cdde6298 157 void pwm_init(void) {
eembed 0:5459cdde6298 158
eembed 0:5459cdde6298 159 pwm_m_clk.period_us(10);
eembed 0:5459cdde6298 160 pwm_m_anticlk.period_us(10);
eembed 0:5459cdde6298 161 pwm_s_anticlk.period_us(10);
eembed 0:5459cdde6298 162 pwm_s_clk.period_us(10);
eembed 0:5459cdde6298 163
eembed 0:5459cdde6298 164 pwm_m_clk.write(0.0f); // Set the ouput duty-cycle, specified as a percentage (float)
eembed 0:5459cdde6298 165 pwm_m_anticlk.write(0.0f);
eembed 0:5459cdde6298 166 pwm_s_anticlk.write(0.0f);
eembed 0:5459cdde6298 167 pwm_s_clk.write(0.0f);
eembed 0:5459cdde6298 168
eembed 0:5459cdde6298 169 Reset_AB_M = 1; //ENABLE RUNNING MODE (H BRIDGE ENABLE)
eembed 0:5459cdde6298 170 Reset_CD_M = 1;
eembed 0:5459cdde6298 171 Reset_AB_S = 1;
eembed 0:5459cdde6298 172 Reset_CD_S = 1;
eembed 0:5459cdde6298 173 }
eembed 0:5459cdde6298 174
eembed 0:5459cdde6298 175 void velocity_m() {
eembed 0:5459cdde6298 176
eembed 0:5459cdde6298 177 int size2 = eth.receive();
eembed 0:5459cdde6298 178
eembed 0:5459cdde6298 179 if(size2 > 0) {
eembed 0:5459cdde6298 180 eth.read(buf, size2);
eembed 0:5459cdde6298 181 memcpy(&recv_m_angle, buf, sizeof(float));
eembed 0:5459cdde6298 182 x_res_m = recv_m_angle;
eembed 0:5459cdde6298 183 }
eembed 0:5459cdde6298 184
eembed 0:5459cdde6298 185 ve_sum_m += dx_res_m*dt;
eembed 0:5459cdde6298 186 dx_res_m =G_filcon_v_m*( x_res_m-ve_sum_m);
eembed 0:5459cdde6298 187
eembed 0:5459cdde6298 188 }
eembed 0:5459cdde6298 189
eembed 0:5459cdde6298 190 void velocity_s() {
eembed 0:5459cdde6298 191
eembed 0:5459cdde6298 192 int32_t temp; position = 0;
eembed 0:5459cdde6298 193 float d_v_err_s=0.0;
eembed 0:5459cdde6298 194 qei_s.SetDigiFilter(480UL);
eembed 0:5459cdde6298 195 qei_s.SetMaxPosition(0xFFFFFFFF);
eembed 0:5459cdde6298 196 position = qei_s.GetPosition();
eembed 0:5459cdde6298 197
eembed 0:5459cdde6298 198 x_res_s = -1.0*position * 2.0 * PI / encoder_pulses_s;
eembed 0:5459cdde6298 199 ve_sum_s += dx_res_s*dt;
eembed 0:5459cdde6298 200 dx_res_s = G_filcon_v_s*(x_res_s-ve_sum_s);
eembed 0:5459cdde6298 201
eembed 0:5459cdde6298 202 }
eembed 0:5459cdde6298 203
eembed 0:5459cdde6298 204 void current_pid_m(){
eembed 0:5459cdde6298 205 Master_Direction = M_Dir.read();
eembed 0:5459cdde6298 206
eembed 0:5459cdde6298 207 if(Master_Direction == 0){ //master clockwise
eembed 0:5459cdde6298 208 I_res_m = current_sensor_m_p.read();
eembed 0:5459cdde6298 209 I1_act_m = 1.0*((I_res_m*3.3/0.717075441532258) ); //0.74787687701613
eembed 0:5459cdde6298 210
eembed 0:5459cdde6298 211 }else if(Master_Direction == 1) { //master anticlockwise
eembed 0:5459cdde6298 212 I_res_m = current_sensor_m_n.read();
eembed 0:5459cdde6298 213 I1_act_m = -1.0*((I_res_m*3.3)/0.717075441532258); //0.713239227822580
eembed 0:5459cdde6298 214 }
eembed 0:5459cdde6298 215 //I_act_m += G_filcon_I1_m*(I1_act_m-I_act_m)*dt;
eembed 0:5459cdde6298 216 I_act_m = 1*I1_act_m;
eembed 0:5459cdde6298 217
eembed 0:5459cdde6298 218 I_err_m = I_ref_m - I_act_m;
eembed 0:5459cdde6298 219 I_tmp_m += (Iki_m * dt * I_err_m);
eembed 0:5459cdde6298 220
eembed 0:5459cdde6298 221 tem_I_m += G_filcon_I_m*d_I_m*dt;
eembed 0:5459cdde6298 222 d_I_m = I_err_m - tem_I_m;
eembed 0:5459cdde6298 223
eembed 0:5459cdde6298 224 pwm_I_M=((I_err_m * Ikp_m) + I_tmp_m + (d_I_m * Ikd_m));
eembed 0:5459cdde6298 225 }
eembed 0:5459cdde6298 226
eembed 0:5459cdde6298 227 void current_pid_s(){
eembed 0:5459cdde6298 228 Slave_Direction = S_Dir.read();
eembed 0:5459cdde6298 229 if(Slave_Direction == 0){ //slave clockwise
eembed 0:5459cdde6298 230 I_res_s = current_sensor_s_p.read();
eembed 0:5459cdde6298 231 I1_act_s = 1.0*((I_res_s*3.3)/0.717075441532258 ); //0.717075441532258
eembed 0:5459cdde6298 232 }else{
eembed 0:5459cdde6298 233 I_res_s = current_sensor_s_n.read(); //slave anticlockwise
eembed 0:5459cdde6298 234 I1_act_s = -1.0*((I_res_s*3.3)/0.717075441532258 ); //0.724138445564516
eembed 0:5459cdde6298 235 }
eembed 0:5459cdde6298 236
eembed 0:5459cdde6298 237 //I_act_s += G_filcon_I1_s*(I1_act_s-I_act_s)*dt;
eembed 0:5459cdde6298 238 I_act_s = I1_act_s;
eembed 0:5459cdde6298 239
eembed 0:5459cdde6298 240 I_err_s = I_ref_s - I_act_s;
eembed 0:5459cdde6298 241 I_tmp_s += Iki_s * dt * I_err_s;
eembed 0:5459cdde6298 242 tem_I_s += G_filcon_I_s*d_I_s*dt;
eembed 0:5459cdde6298 243 d_I_s = I_err_s - tem_I_s;
eembed 0:5459cdde6298 244
eembed 0:5459cdde6298 245 pwm_I_S=((I_err_s * Ikp_s) + I_tmp_s + (d_I_s * Ikd_s));
eembed 0:5459cdde6298 246 }
eembed 0:5459cdde6298 247
eembed 0:5459cdde6298 248 void Disob() {
eembed 0:5459cdde6298 249
eembed 0:5459cdde6298 250 tmp_m = Gd*J_const_s*dx_res_m;
eembed 0:5459cdde6298 251 //ob_sum_m += (Gd*(Kt_const_s*I_act_m+tmp_m-ob_sum_m)*dt);
eembed 0:5459cdde6298 252 ob_sum_m1 = Kt_const_m*I_act_m+tmp_m;
eembed 0:5459cdde6298 253 ob_sum_m += Gd*(ob_sum_m1-ob_sum_m)*dt;
eembed 0:5459cdde6298 254 i_com_m = (ob_sum_m - tmp_m)*Kt_constinv_m; //read current
eembed 0:5459cdde6298 255 //i_com_m=0.0;
eembed 0:5459cdde6298 256 torque_dob_m= i_com_m*Kt_const_m;
eembed 0:5459cdde6298 257
eembed 0:5459cdde6298 258 fric_m = 0.0;
eembed 0:5459cdde6298 259 i_rto_m = (i_com_m-fric_m); //friction should be in current terms
eembed 0:5459cdde6298 260
eembed 0:5459cdde6298 261 tmp_s = Gd*J_const_s*dx_res_s;
eembed 0:5459cdde6298 262 //ob_sum_s += (Gd*(Kt_const_s*I_act_s+tmp_s-ob_sum_s)*dt);
eembed 0:5459cdde6298 263 ob_sum_s1 = Kt_const_s*I_act_s+tmp_s;
eembed 0:5459cdde6298 264 ob_sum_s += Gd*(ob_sum_s1-ob_sum_s)*dt;
eembed 0:5459cdde6298 265 i_com_s = (ob_sum_s - tmp_s)*Kt_constinv_s; //read current
eembed 0:5459cdde6298 266 //i_com_s = 0.0;
eembed 0:5459cdde6298 267 torque_dob_s = i_com_s*Kt_const_s;
eembed 0:5459cdde6298 268 fric_s = 0.0;
eembed 0:5459cdde6298 269 i_rto_s = (i_com_s-fric_s); //friction should be in current terms
eembed 0:5459cdde6298 270 }
eembed 0:5459cdde6298 271
eembed 0:5459cdde6298 272 int Controller(void) {
eembed 0:5459cdde6298 273
eembed 0:5459cdde6298 274 ddx_ref_m = Kp*(x_res_s-x_res_m) + Kv*(dx_res_s-dx_res_m);
eembed 0:5459cdde6298 275 I_ref_m1 = Kt_constinv_m*J_const_m*ddx_ref_m;
eembed 0:5459cdde6298 276 I_ref_m = I_ref_m1 + i_com_m - (i_rto_m+i_rto_s);
eembed 0:5459cdde6298 277
eembed 0:5459cdde6298 278 //I_ref_m = 0.1;
eembed 0:5459cdde6298 279 ddx_ref_s = Kp*(x_res_m-x_res_s) + Kv*(dx_res_m-dx_res_s);
eembed 0:5459cdde6298 280
eembed 0:5459cdde6298 281 I_ref_s1 = Kt_constinv_s*J_const_s*ddx_ref_s;
eembed 0:5459cdde6298 282 I_ref_s =I_ref_s1 + i_com_s - (i_rto_s+i_rto_m);
eembed 0:5459cdde6298 283
eembed 0:5459cdde6298 284 //I_ref_s = 0.1;
eembed 0:5459cdde6298 285
eembed 0:5459cdde6298 286 return 0;
eembed 0:5459cdde6298 287 }
eembed 0:5459cdde6298 288
eembed 0:5459cdde6298 289 void PWM_Generator_m() {
eembed 0:5459cdde6298 290 duty_m = pwm_I_M;
eembed 0:5459cdde6298 291
eembed 0:5459cdde6298 292 if (duty_m> 0.0) {
eembed 0:5459cdde6298 293 if (duty_m > 0.5) {
eembed 0:5459cdde6298 294 duty_m = 0.5;
eembed 0:5459cdde6298 295 }
eembed 0:5459cdde6298 296 pwm_m_clk = 0.0;
eembed 0:5459cdde6298 297 pwm_m_anticlk = duty_m;
eembed 0:5459cdde6298 298 }
eembed 0:5459cdde6298 299
eembed 0:5459cdde6298 300 if (duty_m < 0.0) {
eembed 0:5459cdde6298 301 if (duty_m < -0.5) {
eembed 0:5459cdde6298 302 duty_m = -0.5;
eembed 0:5459cdde6298 303 }
eembed 0:5459cdde6298 304 pwm_m_anticlk = 0.0;
eembed 0:5459cdde6298 305 pwm_m_clk = -1.0 * duty_m;
eembed 0:5459cdde6298 306 }
eembed 0:5459cdde6298 307 }
eembed 0:5459cdde6298 308
eembed 0:5459cdde6298 309 void PWM_Generator_s() {
eembed 0:5459cdde6298 310 duty_s = pwm_I_S;
eembed 0:5459cdde6298 311
eembed 0:5459cdde6298 312 if (duty_s > 0.0) {
eembed 0:5459cdde6298 313 if (duty_s > 0.5) {
eembed 0:5459cdde6298 314 duty_s = 0.5;
eembed 0:5459cdde6298 315 }
eembed 0:5459cdde6298 316 pwm_s_clk = 0.0;
eembed 0:5459cdde6298 317 pwm_s_anticlk = duty_s;
eembed 0:5459cdde6298 318 }
eembed 0:5459cdde6298 319
eembed 0:5459cdde6298 320 if (duty_s < 0.0) {
eembed 0:5459cdde6298 321 if (duty_s < -0.5) {
eembed 0:5459cdde6298 322 duty_s = -0.5;
eembed 0:5459cdde6298 323 }
eembed 0:5459cdde6298 324 pwm_s_anticlk = 0.0;
eembed 0:5459cdde6298 325 pwm_s_clk = -1.0 * duty_s;
eembed 0:5459cdde6298 326 }
eembed 0:5459cdde6298 327 }
eembed 0:5459cdde6298 328
eembed 0:5459cdde6298 329 void cleanup_module(void){
eembed 0:5459cdde6298 330
eembed 0:5459cdde6298 331 pwm_m_clk = 0.0; // pwm cleanup module
eembed 0:5459cdde6298 332 pwm_m_anticlk = 0.0;
eembed 0:5459cdde6298 333 pwm_s_anticlk = 0.0;
eembed 0:5459cdde6298 334 pwm_s_clk = 0.0;
eembed 0:5459cdde6298 335
eembed 0:5459cdde6298 336 Reset_AB_M = 0; //RESET H BRIDGE
eembed 0:5459cdde6298 337 Reset_CD_M = 0;
eembed 0:5459cdde6298 338 Reset_AB_S = 0;
eembed 0:5459cdde6298 339 Reset_CD_S = 0;
eembed 0:5459cdde6298 340
eembed 0:5459cdde6298 341 led1=0;
eembed 0:5459cdde6298 342 led3=0;
eembed 0:5459cdde6298 343 }
eembed 0:5459cdde6298 344
eembed 0:5459cdde6298 345 //RTOS
eembed 0:5459cdde6298 346
eembed 0:5459cdde6298 347 void Control_body() {
eembed 0:5459cdde6298 348 // Control Part - main code
eembed 0:5459cdde6298 349 Controller();
eembed 0:5459cdde6298 350 velocity_m();
eembed 0:5459cdde6298 351 velocity_s ();
eembed 0:5459cdde6298 352 current_pid_m();
eembed 0:5459cdde6298 353 current_pid_s ();
eembed 0:5459cdde6298 354 PWM_Generator_m();
eembed 0:5459cdde6298 355 PWM_Generator_s();
eembed 0:5459cdde6298 356 Disob();
eembed 0:5459cdde6298 357 //led1=!led1;
eembed 0:5459cdde6298 358 counter++;
eembed 0:5459cdde6298 359 }
eembed 0:5459cdde6298 360
eembed 0:5459cdde6298 361 void thread_2(void const *argument){
eembed 0:5459cdde6298 362 led1=1;
eembed 0:5459cdde6298 363 SDFileSystem sd(p5, p6, p7, p8, "sd");
eembed 0:5459cdde6298 364 FILE *fp = fopen("/sd/BC.txt", "w");
eembed 0:5459cdde6298 365
eembed 0:5459cdde6298 366 if(fp == NULL) {
eembed 0:5459cdde6298 367 for(int i=0;i<5;i++){
eembed 0:5459cdde6298 368 led3=!led3;
eembed 0:5459cdde6298 369 wait(1.0);
eembed 0:5459cdde6298 370 }
eembed 0:5459cdde6298 371 }
eembed 0:5459cdde6298 372
eembed 0:5459cdde6298 373 while(counter<80000){
eembed 0:5459cdde6298 374 if(counter>=(counter_old+100)){
eembed 0:5459cdde6298 375 //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);
eembed 0:5459cdde6298 376 fprintf(fp, "%d %f %f %f %f \n",timer.read_us(),torque_dob_m,torque_dob_s,x_res_m,x_res_s);
eembed 0:5459cdde6298 377 counter_old=counter;
eembed 0:5459cdde6298 378 led3=!led3;
eembed 0:5459cdde6298 379 }
eembed 0:5459cdde6298 380 }
eembed 0:5459cdde6298 381
eembed 0:5459cdde6298 382 fclose(fp);
eembed 0:5459cdde6298 383 timer.stop();
eembed 0:5459cdde6298 384 cleanup_module();
eembed 0:5459cdde6298 385 ticker.detach ();
eembed 0:5459cdde6298 386
eembed 0:5459cdde6298 387 wait(1.0);
eembed 0:5459cdde6298 388 }
eembed 0:5459cdde6298 389
eembed 0:5459cdde6298 390 void ethernet_init(){
eembed 0:5459cdde6298 391 eth.set_link(Ethernet::HalfDuplex100);
eembed 0:5459cdde6298 392 wait_ms(1000); // Needed after startup.
eembed 0:5459cdde6298 393 if(eth.link()) {
eembed 0:5459cdde6298 394 for(int i=0;i<3;i++){
eembed 0:5459cdde6298 395 led3=!led3;
eembed 0:5459cdde6298 396 wait(1.0);
eembed 0:5459cdde6298 397 }
eembed 0:5459cdde6298 398 }
eembed 0:5459cdde6298 399 }
eembed 0:5459cdde6298 400
eembed 0:5459cdde6298 401 int main() {
eembed 0:5459cdde6298 402
eembed 0:5459cdde6298 403 ethernet_init();
eembed 0:5459cdde6298 404 pwm_init();
eembed 0:5459cdde6298 405 timer.start();
eembed 0:5459cdde6298 406 dt=dt_us/1000000.0;
eembed 0:5459cdde6298 407
eembed 0:5459cdde6298 408 ticker.attach_us(&Control_body, dt_us);
eembed 0:5459cdde6298 409 Thread thread(*thread_2,NULL,osPriorityAboveNormal,DEFAULT_STACK_SIZE*10.0,NULL);
eembed 0:5459cdde6298 410 }
eembed 0:5459cdde6298 411