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

Force controlled vibration analysis

Dependencies:   mbed

main.cpp@5:9ab19c63203e, 2019-12-03 (annotated)

Committer:
eembed
Date:
Tue Dec 03 10:40:16 2019 +0000
Revision:
5:9ab19c63203e
Parent:
4:8432d3d42835 
first commit;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
eembed 5:9ab19c63203e 1
eembed 2:409ee7fcbd8a 2 /*
eembed 2:409ee7fcbd8a 3 * Force Controlled Vibration Analysis -
eembed 2:409ee7fcbd8a 4 *
eembed 2:409ee7fcbd8a 5 * 22.08.2019
eembed 2:409ee7fcbd8a 6 * Theekshana
eembed 2:409ee7fcbd8a 7 */
eembed 2:409ee7fcbd8a 8
eembed 0:5459cdde6298 9 #include "mbed.h"
eembed 0:5459cdde6298 10 #include "rtos.h"
eembed 0:5459cdde6298 11 #include "SDFileSystem.h"
eembed 0:5459cdde6298 12 #include "qeihw.h"
eembed 2:409ee7fcbd8a 13
eembed 2:409ee7fcbd8a 14
eembed 2:409ee7fcbd8a 15 //---------------------------=Communication Pins=-------------------------------
eembed 5:9ab19c63203e 16 Serial pc(USBTX, USBRX);
eembed 1:db36f62f783b 17 DigitalOut led3(LED3);
eembed 1:db36f62f783b 18 DigitalOut led1(LED1);
eembed 1:db36f62f783b 19 SDFileSystem sd(p5, p6, p7, p8, "sd"); // pintout for sd card
eembed 2:409ee7fcbd8a 20
eembed 2:409ee7fcbd8a 21 //--------------------------=Motor Driver Controller Pin =-----------------------------
eembed 2:409ee7fcbd8a 22 DigitalOut Reset_AB(p29); // H bridge enable
eembed 2:409ee7fcbd8a 23 DigitalOut Reset_CD(p30);
eembed 2:409ee7fcbd8a 24 DigitalOut Reset_AB_s(p27);
eembed 2:409ee7fcbd8a 25 DigitalOut Reset_CD_s(p28);
eembed 2:409ee7fcbd8a 26
eembed 2:409ee7fcbd8a 27 PwmOut pwm_clk_s(p21); // clockwise rotation pwm pin for Slave
eembed 2:409ee7fcbd8a 28 PwmOut pwm_anticlk_s(p22);
eembed 2:409ee7fcbd8a 29 PwmOut pwm_clk(p23); // clockwise rotation pwm pin for Master
eembed 2:409ee7fcbd8a 30 PwmOut pwm_anticlk(p24);
eembed 2:409ee7fcbd8a 31
eembed 2:409ee7fcbd8a 32
eembed 2:409ee7fcbd8a 33 //--------------------------=Current Sensor MBED Pins=--------------------------
eembed 2:409ee7fcbd8a 34 int Master_Direction=0;
eembed 2:409ee7fcbd8a 35 int slave_Direction=0;
eembed 2:409ee7fcbd8a 36
eembed 2:409ee7fcbd8a 37 DigitalIn M_Dir(p10);
eembed 2:409ee7fcbd8a 38 DigitalIn S_Dir(p9);
eembed 2:409ee7fcbd8a 39 AnalogIn current_sensor_s_p(p15); // current sensor input for MASTER positive
eembed 2:409ee7fcbd8a 40 AnalogIn current_sensor_s_n(p16);
eembed 2:409ee7fcbd8a 41 AnalogIn current_sensor_m_p(p17); // current sensor input for MASTER positive
eembed 2:409ee7fcbd8a 42 AnalogIn current_sensor_m_n(p18); // current sensor input for MASTER negative
eembed 2:409ee7fcbd8a 43
eembed 2:409ee7fcbd8a 44 float I_res_m = 0.0;
eembed 2:409ee7fcbd8a 45 float I1_act_m=0.0;
eembed 2:409ee7fcbd8a 46 float I_msrd=0.0;
eembed 2:409ee7fcbd8a 47 float G_Cfilter=30.0;
eembed 2:409ee7fcbd8a 48
eembed 2:409ee7fcbd8a 49 //--------------------------=Ethernet Variable=---------------------------------
eembed 2:409ee7fcbd8a 50 Ethernet eth;
eembed 4:8432d3d42835 51 float I_sin = 0.0;
eembed 4:8432d3d42835 52 float I_DoB_prev = 0.0;
eembed 4:8432d3d42835 53 float I_sin_prev =0.0;
eembed 4:8432d3d42835 54
eembed 2:409ee7fcbd8a 55
eembed 2:409ee7fcbd8a 56
eembed 2:409ee7fcbd8a 57 //--------------------------=Thread Variables=-----------------------------------
eembed 1:db36f62f783b 58 Ticker time_up;
eembed 1:db36f62f783b 59 float startTime=0.0;
eembed 1:db36f62f783b 60 float tempTime = 0.0;
eembed 1:db36f62f783b 61 float endTime=0.0;
eembed 1:db36f62f783b 62 Timer timer;
eembed 1:db36f62f783b 63 float preTime = 0.0;
eembed 2:409ee7fcbd8a 64
eembed 2:409ee7fcbd8a 65 //--------------------------=Velocity reading variables=-------------------------
eembed 1:db36f62f783b 66 char buf[0x600];
eembed 1:db36f62f783b 67 float recv_angle = 0.0;
eembed 1:db36f62f783b 68 float x_res = 0.0;
eembed 5:9ab19c63203e 69 float dt = 0.000001*150.0;
eembed 1:db36f62f783b 70 float ve_sum = 0.0;
eembed 1:db36f62f783b 71 float dx_res = 0.0;
eembed 2:409ee7fcbd8a 72 float G_filter_v = 150.0;
eembed 1:db36f62f783b 73 float duty = 0.0;
eembed 2:409ee7fcbd8a 74
eembed 2:409ee7fcbd8a 75 //--------------------------=DoB variables=--------------------------------------
eembed 1:db36f62f783b 76 float K_tn=0.135;
eembed 1:db36f62f783b 77 float J_n = 0.0000720;
eembed 5:9ab19c63203e 78 float B = 0.000107;
eembed 2:409ee7fcbd8a 79
eembed 1:db36f62f783b 80 float g_dis = 100.0;
eembed 5:9ab19c63203e 81 float g_Tdis = 5.0;
eembed 1:db36f62f783b 82 float D_DoB = 0.0;
eembed 1:db36f62f783b 83 float D_BFilter = 0.0;
eembed 1:db36f62f783b 84 float D_AFilter = 0.0;
eembed 2:409ee7fcbd8a 85 float I_DoB = 0.0;
eembed 2:409ee7fcbd8a 86 float R_BFilter=0.0;
eembed 2:409ee7fcbd8a 87
eembed 2:409ee7fcbd8a 88 //--------------------------=RTOB Variables=-------------------------------------
eembed 2:409ee7fcbd8a 89 float RT_BFilter=0.0;
eembed 2:409ee7fcbd8a 90 float RT_AFilter = 0.0;
eembed 2:409ee7fcbd8a 91 float RTOB = 0.0;
eembed 2:409ee7fcbd8a 92
eembed 4:8432d3d42835 93 float K_pi = 40.0; // tested value for smooth oparation
eembed 4:8432d3d42835 94 float K_di = 0.0;
eembed 4:8432d3d42835 95 float K_ii = 0.0;
eembed 4:8432d3d42835 96 float I_error = 0.0;
eembed 4:8432d3d42835 97 float I_err_sum = 0.0;
eembed 4:8432d3d42835 98 float I_err_prev = 0.0;
eembed 4:8432d3d42835 99
eembed 4:8432d3d42835 100 float I_PID = 0.0;
eembed 4:8432d3d42835 101
eembed 2:409ee7fcbd8a 102 //--------------------------=Force Controller Variables=-------------------------
eembed 2:409ee7fcbd8a 103 float F_ref = 500.0;
eembed 4:8432d3d42835 104 float T_ref = 0.15;
eembed 2:409ee7fcbd8a 105 float T_f = 0.025;
eembed 5:9ab19c63203e 106 float K_pf = 10000.0; // tested value for smooth oparation
eembed 2:409ee7fcbd8a 107 float K_df = 0.0;
eembed 2:409ee7fcbd8a 108 float K_if = 0.0;
eembed 2:409ee7fcbd8a 109 float F_error = 0.0;
eembed 2:409ee7fcbd8a 110 float F_err_sum = 0.0;
eembed 2:409ee7fcbd8a 111 float F_err_prev = 0.0;
eembed 2:409ee7fcbd8a 112 float I_cmd = 0.0;
eembed 2:409ee7fcbd8a 113 float F_PID = 0.0;
eembed 5:9ab19c63203e 114 float I_ref = 0.5;
eembed 2:409ee7fcbd8a 115
eembed 2:409ee7fcbd8a 116 //-------------------------=slave current controller variables=-----------------
eembed 2:409ee7fcbd8a 117 int count = 0;
eembed 2:409ee7fcbd8a 118 float f = 20.0;
eembed 2:409ee7fcbd8a 119 float K_pis =10.0;
eembed 2:409ee7fcbd8a 120 float K_dis = 0.0;
eembed 2:409ee7fcbd8a 121 float K_iis = 0.0;
eembed 2:409ee7fcbd8a 122
eembed 2:409ee7fcbd8a 123 float I_error_s = 0.0;
eembed 2:409ee7fcbd8a 124 float I_ref_s = 0.0;
eembed 2:409ee7fcbd8a 125 float I_err_sum_s = 0.0;
eembed 2:409ee7fcbd8a 126 float I_PID_s = 0.0;
eembed 2:409ee7fcbd8a 127 float I_err_prev_s = 0.0;
eembed 2:409ee7fcbd8a 128 float duty_s = 0.0;
eembed 2:409ee7fcbd8a 129
eembed 2:409ee7fcbd8a 130 float I_res_s = 0.0;
eembed 2:409ee7fcbd8a 131 float I1_act_s = 0.0;
eembed 2:409ee7fcbd8a 132 float I_msrd_s = 0.0;
eembed 2:409ee7fcbd8a 133
eembed 2:409ee7fcbd8a 134 //--------------------------=Fucion declaration=--------------------------------
eembed 2:409ee7fcbd8a 135 void controlLoop();
eembed 2:409ee7fcbd8a 136 void readVelocity();
eembed 2:409ee7fcbd8a 137 void readCurrent();
eembed 2:409ee7fcbd8a 138 void forceController();
eembed 2:409ee7fcbd8a 139 void DoB();
eembed 2:409ee7fcbd8a 140 void motorPWM_init();
eembed 4:8432d3d42835 141
eembed 2:409ee7fcbd8a 142 void motorPWM();
eembed 0:5459cdde6298 143
eembed 2:409ee7fcbd8a 144 void slaveCurrentRead();
eembed 2:409ee7fcbd8a 145 void slaveCurrentController();
eembed 2:409ee7fcbd8a 146 void motorpwm_s();
eembed 2:409ee7fcbd8a 147
eembed 2:409ee7fcbd8a 148 void thread_2(void const *argument);
eembed 2:409ee7fcbd8a 149 void sd_card_write_test();
eembed 2:409ee7fcbd8a 150 void sd_card_write();
eembed 2:409ee7fcbd8a 151 void ethernet_init();
eembed 2:409ee7fcbd8a 152
eembed 5:9ab19c63203e 153 //FILE *fp = fopen("/sd/mydir/sdtest.txt","w");
eembed 5:9ab19c63203e 154
eembed 5:9ab19c63203e 155
eembed 2:409ee7fcbd8a 156
eembed 2:409ee7fcbd8a 157
eembed 2:409ee7fcbd8a 158 //----------------------------------=Main Loop=---------------------------------
eembed 2:409ee7fcbd8a 159 int main()
eembed 2:409ee7fcbd8a 160 {
eembed 4:8432d3d42835 161 ethernet_init();
eembed 5:9ab19c63203e 162 //sd_card_write_test();
eembed 5:9ab19c63203e 163 //FILE *fp = fopen("/sd/mydir/sdtest.txt","w");
eembed 2:409ee7fcbd8a 164 motorPWM_init();
eembed 2:409ee7fcbd8a 165 time_up.attach(&controlLoop, dt);
eembed 4:8432d3d42835 166
eembed 2:409ee7fcbd8a 167 while(1)
eembed 2:409ee7fcbd8a 168 {
eembed 5:9ab19c63203e 169 pc.printf("%f\t %f\t %f\t \r\n", I_msrd,I_ref,RTOB);
eembed 5:9ab19c63203e 170 /*
eembed 5:9ab19c63203e 171 if (count>20000 and count<50000 and count!=count_prev ){
eembed 5:9ab19c63203e 172 led1 =1;
eembed 5:9ab19c63203e 173 //sd_card_write();
eembed 5:9ab19c63203e 174 count_prev = count;
eembed 5:9ab19c63203e 175 //datacount++;
eembed 5:9ab19c63203e 176 }
eembed 5:9ab19c63203e 177 else if(count>50000){
eembed 5:9ab19c63203e 178 led1=0;
eembed 5:9ab19c63203e 179 fclose(fp);
eembed 5:9ab19c63203e 180 }
eembed 5:9ab19c63203e 181
eembed 5:9ab19c63203e 182 */
eembed 2:409ee7fcbd8a 183 }
eembed 2:409ee7fcbd8a 184 }
eembed 2:409ee7fcbd8a 185
eembed 2:409ee7fcbd8a 186 //--------------------------=Main Control Loop=---------------------------------
eembed 2:409ee7fcbd8a 187 void controlLoop(){
eembed 2:409ee7fcbd8a 188 if (x_res>0.0)
eembed 2:409ee7fcbd8a 189 {
eembed 2:409ee7fcbd8a 190 T_f = -0.025;
eembed 2:409ee7fcbd8a 191 }
eembed 2:409ee7fcbd8a 192 else
eembed 2:409ee7fcbd8a 193 {
eembed 2:409ee7fcbd8a 194 T_f = 0.025;
eembed 2:409ee7fcbd8a 195 }
eembed 2:409ee7fcbd8a 196
eembed 2:409ee7fcbd8a 197 count = count+1;
eembed 5:9ab19c63203e 198 I_ref_s = 0.15*sin(2*3.14159*dt*f*count); // 2*3.14159*dt =
eembed 2:409ee7fcbd8a 199 readVelocity();
eembed 2:409ee7fcbd8a 200 readCurrent();
eembed 2:409ee7fcbd8a 201 forceController();
eembed 5:9ab19c63203e 202 DoB();
eembed 2:409ee7fcbd8a 203 motorPWM();
eembed 2:409ee7fcbd8a 204 slaveCurrentRead();
eembed 2:409ee7fcbd8a 205 slaveCurrentController();
eembed 2:409ee7fcbd8a 206 motorpwm_s();
eembed 2:409ee7fcbd8a 207 }
eembed 2:409ee7fcbd8a 208
eembed 4:8432d3d42835 209
eembed 2:409ee7fcbd8a 210 //--------------------------=Read Velocity=-------------------------------------
eembed 1:db36f62f783b 211 void readVelocity(){
eembed 2:409ee7fcbd8a 212 int size2 = eth.receive();
eembed 2:409ee7fcbd8a 213 if(size2 > 0)
eembed 2:409ee7fcbd8a 214 {
eembed 2:409ee7fcbd8a 215 eth.read(buf, size2);
eembed 2:409ee7fcbd8a 216 memcpy(&recv_angle, buf, sizeof(float));
eembed 2:409ee7fcbd8a 217 x_res = -1*recv_angle;
eembed 2:409ee7fcbd8a 218 }
eembed 2:409ee7fcbd8a 219 ve_sum += dx_res*dt;
eembed 2:409ee7fcbd8a 220 dx_res =G_filter_v*( x_res-ve_sum);
eembed 2:409ee7fcbd8a 221 }
eembed 2:409ee7fcbd8a 222
eembed 2:409ee7fcbd8a 223 //--------------------------=Current Meassurement=------------------------------
eembed 2:409ee7fcbd8a 224 void readCurrent()
eembed 2:409ee7fcbd8a 225 {
eembed 1:db36f62f783b 226 Master_Direction = M_Dir.read();
eembed 2:409ee7fcbd8a 227 //master clockwise
eembed 2:409ee7fcbd8a 228 if(Master_Direction == 0)
eembed 2:409ee7fcbd8a 229 {
eembed 0:5459cdde6298 230 I_res_m = current_sensor_m_p.read();
eembed 4:8432d3d42835 231 I1_act_m = -1.0*(I_res_m*4.71428); //0.74787687701613 //0.717075441532258
eembed 0:5459cdde6298 232
eembed 4:8432d3d42835 233 }else { //master anticlockwise
eembed 0:5459cdde6298 234 I_res_m = current_sensor_m_n.read();
eembed 4:8432d3d42835 235 I1_act_m = (I_res_m*4.71428); //0.713239227822580
eembed 0:5459cdde6298 236 }
eembed 4:8432d3d42835 237 I_msrd += 30.0*(I1_act_m-I_msrd)*dt;
eembed 2:409ee7fcbd8a 238 }
eembed 2:409ee7fcbd8a 239
eembed 2:409ee7fcbd8a 240
eembed 2:409ee7fcbd8a 241
eembed 2:409ee7fcbd8a 242 //--------------------------=Velocity Controller=-------------------------------
eembed 2:409ee7fcbd8a 243 void forceController()
eembed 2:409ee7fcbd8a 244 {
eembed 4:8432d3d42835 245 //T_ref = F_ref*6.1/10000.0;
eembed 2:409ee7fcbd8a 246 F_error = T_ref - RTOB;
eembed 2:409ee7fcbd8a 247 F_err_sum += F_error*dt;
eembed 5:9ab19c63203e 248 F_PID = (K_pf*F_error);
eembed 2:409ee7fcbd8a 249 F_err_prev = F_error;
eembed 2:409ee7fcbd8a 250 I_cmd = F_PID*J_n/K_tn;
eembed 4:8432d3d42835 251 I_ref = I_cmd +(I_DoB);
eembed 4:8432d3d42835 252
eembed 2:409ee7fcbd8a 253 }
eembed 2:409ee7fcbd8a 254
eembed 2:409ee7fcbd8a 255 //--------------------------=Distarbance Observer=------------------------------
eembed 2:409ee7fcbd8a 256 void DoB()
eembed 2:409ee7fcbd8a 257 {
eembed 5:9ab19c63203e 258 //D_BFilter=(I_msrd*K_tn) + (dx_res*J_n*g_dis);
eembed 5:9ab19c63203e 259 D_BFilter=(I_msrd*K_tn) + (dx_res*J_n*g_dis) ;
eembed 2:409ee7fcbd8a 260 D_AFilter += g_dis*(D_BFilter-D_AFilter)*dt;
eembed 2:409ee7fcbd8a 261 D_DoB = D_AFilter - (dx_res*J_n*g_dis);
eembed 5:9ab19c63203e 262 I_DoB = (D_DoB)/K_tn;
eembed 5:9ab19c63203e 263
eembed 0:5459cdde6298 264
eembed 2:409ee7fcbd8a 265 //--------------------------=Reaction Force Observer=------------------------------
eembed 2:409ee7fcbd8a 266 R_BFilter=(I_msrd*K_tn) + (dx_res*J_n*g_Tdis);
eembed 2:409ee7fcbd8a 267 RT_BFilter = R_BFilter - (T_f+B*dx_res);
eembed 2:409ee7fcbd8a 268 RT_AFilter += g_Tdis*(RT_BFilter-RT_AFilter)*dt;
eembed 2:409ee7fcbd8a 269 RTOB = RT_AFilter - (dx_res*J_n*g_Tdis);
eembed 2:409ee7fcbd8a 270 }
eembed 2:409ee7fcbd8a 271
eembed 2:409ee7fcbd8a 272
eembed 2:409ee7fcbd8a 273 //--------------------------=Motor PWM Initialization=--------------------------
eembed 2:409ee7fcbd8a 274 void motorPWM_init()
eembed 2:409ee7fcbd8a 275 {
eembed 1:db36f62f783b 276 pwm_clk.period_us(10);
eembed 2:409ee7fcbd8a 277 pwm_clk_s.period_us(10);
eembed 2:409ee7fcbd8a 278 pwm_anticlk_s.period_us(10);
eembed 1:db36f62f783b 279 pwm_anticlk.period_us(10);
eembed 1:db36f62f783b 280 pwm_clk.write(0.0f);
eembed 1:db36f62f783b 281 pwm_anticlk.write(0.0f);
eembed 2:409ee7fcbd8a 282 pwm_clk_s.write(0.0f);
eembed 2:409ee7fcbd8a 283 pwm_anticlk_s.write(0.0f);
eembed 1:db36f62f783b 284 Reset_AB = 1;
eembed 1:db36f62f783b 285 Reset_CD = 1;
eembed 2:409ee7fcbd8a 286 Reset_AB_s =1;
eembed 2:409ee7fcbd8a 287 Reset_CD_s =1;
eembed 2:409ee7fcbd8a 288 }
eembed 2:409ee7fcbd8a 289
eembed 2:409ee7fcbd8a 290 //--------------------------=Motor PWM Generation=------------------------------
eembed 2:409ee7fcbd8a 291 void motorPWM()
eembed 2:409ee7fcbd8a 292 {
eembed 5:9ab19c63203e 293 duty = I_ref/5.1;
eembed 5:9ab19c63203e 294 //duty = 1.62/5.1;
eembed 5:9ab19c63203e 295
eembed 2:409ee7fcbd8a 296 if (duty> 0.0)
eembed 2:409ee7fcbd8a 297 {
eembed 4:8432d3d42835 298 if (duty > 0.95)
eembed 2:409ee7fcbd8a 299 {
eembed 4:8432d3d42835 300 duty = 0.95;
eembed 1:db36f62f783b 301 }
eembed 1:db36f62f783b 302 pwm_clk = 0.0;
eembed 1:db36f62f783b 303 pwm_anticlk = duty;
eembed 4:8432d3d42835 304 }else
eembed 2:409ee7fcbd8a 305 {
eembed 4:8432d3d42835 306 if (duty< -0.95)
eembed 2:409ee7fcbd8a 307 {
eembed 4:8432d3d42835 308 duty = -0.95;
eembed 1:db36f62f783b 309 }
eembed 1:db36f62f783b 310 pwm_anticlk = 0.0;
eembed 1:db36f62f783b 311 pwm_clk = -1.0 * duty;
eembed 1:db36f62f783b 312 }
eembed 2:409ee7fcbd8a 313 }
eembed 2:409ee7fcbd8a 314
eembed 2:409ee7fcbd8a 315 //---------------------------=Slave motor=--------------------------------------
eembed 2:409ee7fcbd8a 316 //---------------------------=Slave motor current reading=----------------------
eembed 2:409ee7fcbd8a 317 void slaveCurrentRead()
eembed 2:409ee7fcbd8a 318 {
eembed 2:409ee7fcbd8a 319 slave_Direction = S_Dir.read();
eembed 2:409ee7fcbd8a 320
eembed 2:409ee7fcbd8a 321 if(slave_Direction == 0)
eembed 2:409ee7fcbd8a 322 {
eembed 2:409ee7fcbd8a 323 I_res_s = current_sensor_s_p.read();
eembed 4:8432d3d42835 324 I1_act_s = -1.0*((I_res_s*4.71428) );//0.74787687701613 //0.717075441532258
eembed 2:409ee7fcbd8a 325
eembed 4:8432d3d42835 326 }else { //master anticlockwise
eembed 2:409ee7fcbd8a 327 I_res_s = current_sensor_s_n.read();
eembed 4:8432d3d42835 328 I1_act_s = 1.0*((I_res_s*4.71428)); //0.713239227822580
eembed 1:db36f62f783b 329 }
eembed 4:8432d3d42835 330 I_msrd_s += 30.0*(I1_act_s-I_msrd_s)*dt; //G_Cfilter = 30.00
eembed 2:409ee7fcbd8a 331
eembed 2:409ee7fcbd8a 332 }
eembed 2:409ee7fcbd8a 333
eembed 2:409ee7fcbd8a 334 //-----------------------=slaveCurrentController=-------------------------------
eembed 2:409ee7fcbd8a 335 void slaveCurrentController()
eembed 4:8432d3d42835 336 {
eembed 2:409ee7fcbd8a 337 I_error_s = I_ref_s - I_msrd_s;
eembed 2:409ee7fcbd8a 338 I_err_sum_s += I_error_s*dt;
eembed 5:9ab19c63203e 339 I_PID_s = (K_pis*I_error_s);
eembed 2:409ee7fcbd8a 340 I_err_prev_s = I_error_s;
eembed 1:db36f62f783b 341 }
eembed 2:409ee7fcbd8a 342
eembed 2:409ee7fcbd8a 343 //----------------------=Slave Motor run=---------------------------------------
eembed 2:409ee7fcbd8a 344 void motorpwm_s(){
eembed 2:409ee7fcbd8a 345 duty_s = I_PID_s;
eembed 2:409ee7fcbd8a 346 if (duty_s> 0.0)
eembed 2:409ee7fcbd8a 347 {
eembed 4:8432d3d42835 348 if (duty_s > 0.95)
eembed 2:409ee7fcbd8a 349 {
eembed 4:8432d3d42835 350 duty_s = 0.95;
eembed 2:409ee7fcbd8a 351 }
eembed 2:409ee7fcbd8a 352 pwm_clk_s = 0.0;
eembed 2:409ee7fcbd8a 353 pwm_anticlk_s = duty_s;
eembed 2:409ee7fcbd8a 354 }
eembed 2:409ee7fcbd8a 355
eembed 4:8432d3d42835 356 else
eembed 2:409ee7fcbd8a 357 {
eembed 4:8432d3d42835 358 if (duty_s< -0.95)
eembed 2:409ee7fcbd8a 359 {
eembed 4:8432d3d42835 360 duty_s = -0.95;
eembed 2:409ee7fcbd8a 361 }
eembed 2:409ee7fcbd8a 362 pwm_anticlk_s = 0.0;
eembed 2:409ee7fcbd8a 363 pwm_clk_s = -1.0 * duty_s;
eembed 2:409ee7fcbd8a 364 }
eembed 2:409ee7fcbd8a 365 }
eembed 2:409ee7fcbd8a 366
eembed 2:409ee7fcbd8a 367
eembed 2:409ee7fcbd8a 368 //--------------------------=Data writting to file=-----------------------------
eembed 2:409ee7fcbd8a 369
eembed 2:409ee7fcbd8a 370
eembed 2:409ee7fcbd8a 371 //--------------------------=File print test=-----------------------------------
eembed 1:db36f62f783b 372 void sd_card_write_test()
eembed 1:db36f62f783b 373 {
eembed 1:db36f62f783b 374 mkdir("/sd/mydir", 0777);
eembed 1:db36f62f783b 375
eembed 1:db36f62f783b 376 FILE *fp = fopen("/sd/mydir/sdtest.txt", "w");
eembed 1:db36f62f783b 377 if(fp == NULL) {
eembed 1:db36f62f783b 378 error("Could not open file for write\n");
eembed 1:db36f62f783b 379 }
eembed 2:409ee7fcbd8a 380 fprintf(fp, "count \t x_res\t RTOB \n");
eembed 2:409ee7fcbd8a 381 fclose(fp);
eembed 1:db36f62f783b 382
eembed 4:8432d3d42835 383
eembed 0:5459cdde6298 384 }
eembed 0:5459cdde6298 385
eembed 2:409ee7fcbd8a 386 //--------------------------=File print=----------------------------------------
eembed 1:db36f62f783b 387 void sd_card_write()
eembed 1:db36f62f783b 388 {
eembed 2:409ee7fcbd8a 389 //sprintf (str, "%d\n",count);
eembed 5:9ab19c63203e 390 //FILE *fp = fopen("/sd/mydir/sdtest.txt","a");
eembed 2:409ee7fcbd8a 391 //setvbuf(fp, NULL, _IONBF, 0);
eembed 2:409ee7fcbd8a 392 //fwrite(&K_tn,1,sizeof(K_tn), fp);
eembed 5:9ab19c63203e 393 //xres = ceil(x_res*1000);
eembed 5:9ab19c63203e 394 //fprintf(fp,"%d\t %f\t %f\t \r\n",count,I_DoB,RTOB);
eembed 5:9ab19c63203e 395 //fclose(fp);
eembed 0:5459cdde6298 396 }
eembed 0:5459cdde6298 397
eembed 2:409ee7fcbd8a 398 //--------------------------=Ethernet Initialization=---------------------------
eembed 2:409ee7fcbd8a 399
eembed 1:db36f62f783b 400 void ethernet_init()
eembed 1:db36f62f783b 401 {
eembed 1:db36f62f783b 402 eth.set_link(Ethernet::HalfDuplex100);
eembed 1:db36f62f783b 403 wait_ms(1000); // Needed after startup.
eembed 1:db36f62f783b 404 if(eth.link())
eembed 1:db36f62f783b 405 {
eembed 1:db36f62f783b 406 for(int i=0;i<3;i++)
eembed 1:db36f62f783b 407 {
eembed 1:db36f62f783b 408 led3=!led3;
eembed 1:db36f62f783b 409 wait(1.0);
eembed 1:db36f62f783b 410 }
eembed 0:5459cdde6298 411 }
eembed 0:5459cdde6298 412 }