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