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

Force controlled vibration analysis

Dependencies:   mbed

main.cpp@4:8432d3d42835, 2019-09-06 (annotated)

Committer:
eembed
Date:
Fri Sep 06 08:59:53 2019 +0000
Revision:
4:8432d3d42835
Parent:
2:409ee7fcbd8a
Child:
5:9ab19c63203e
force control

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