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

Force controlled vibration analysis

Dependencies:   mbed

main.cpp@2:409ee7fcbd8a, 2019-08-30 (annotated)

Committer:
eembed
Date:
Fri Aug 30 12:11:12 2019 +0000
Revision:
2:409ee7fcbd8a
Parent:
1:db36f62f783b
Child:
4:8432d3d42835
first commit;

Who changed what in which revision?

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