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

rtos sd encoder systems

Dependencies:   mbed

main.cpp@2:00c6646f42fb, 2020-01-10 (annotated)

Committer:
eembed
Date:
Fri Jan 10 04:08:21 2020 +0000
Revision:
2:00c6646f42fb
Parent:
1:db36f62f783b 
first commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
eembed 2:00c6646f42fb 1 /*
eembed 2:00c6646f42fb 2 * Force Controlled Vibration Analysis -
eembed 2:00c6646f42fb 3 *
eembed 2:00c6646f42fb 4 * 22.08.2019
eembed 2:00c6646f42fb 5 * Theekshana
eembed 2:00c6646f42fb 6 */
eembed 2:00c6646f42fb 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:00c6646f42fb 12
eembed 2:00c6646f42fb 13 //---------------------------=Communication Pins=-------------------------------
eembed 2:00c6646f42fb 14
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 2:00c6646f42fb 18 Serial pc(USBTX, USBRX);
eembed 2:00c6646f42fb 19 //--------------------------=Motor Driver Controller Pin =-----------------------------
eembed 2:00c6646f42fb 20 DigitalOut Reset_AB(p29); // H bridge enable
eembed 2:00c6646f42fb 21 DigitalOut Reset_CD(p30);
eembed 2:00c6646f42fb 22 DigitalOut Reset_AB_s(p27);
eembed 2:00c6646f42fb 23 DigitalOut Reset_CD_s(p28);
eembed 2:00c6646f42fb 24
eembed 2:00c6646f42fb 25
eembed 2:00c6646f42fb 26 PwmOut pwm_clk_s(p23); // clockwise rotation pwm pin for Slave
eembed 2:00c6646f42fb 27 PwmOut pwm_anticlk_s(p24);
eembed 2:00c6646f42fb 28 PwmOut pwm_clk(p21); // clockwise rotation pwm pin for Master
eembed 2:00c6646f42fb 29 PwmOut pwm_anticlk(p22);
eembed 2:00c6646f42fb 30
eembed 2:00c6646f42fb 31 //--------------------------=Current Sensor MBED Pins=--------------------------
eembed 2:00c6646f42fb 32 int Master_Direction=0;
eembed 2:00c6646f42fb 33 int slave_Direction=0;
eembed 2:00c6646f42fb 34
eembed 2:00c6646f42fb 35 DigitalIn M_Dir(p10);
eembed 2:00c6646f42fb 36 DigitalIn S_Dir(p9);
eembed 2:00c6646f42fb 37 AnalogIn current_sensor_s_p(p15); // current sensor input for MASTER positive
eembed 2:00c6646f42fb 38 AnalogIn current_sensor_s_n(p16);
eembed 2:00c6646f42fb 39 AnalogIn current_sensor_m_p(p17); // current sensor input for MASTER positive
eembed 2:00c6646f42fb 40 AnalogIn current_sensor_m_n(p18); // current sensor input for MASTER negative
eembed 2:00c6646f42fb 41
eembed 2:00c6646f42fb 42 float I_res_m = 0.0;
eembed 2:00c6646f42fb 43 float I1_act_m=0.0;
eembed 2:00c6646f42fb 44 float I_msrd=0.0;
eembed 2:00c6646f42fb 45 float G_Cfilter=30.0;
eembed 2:00c6646f42fb 46
eembed 2:00c6646f42fb 47 //--------------------------=Ethernet Variable=---------------------------------
eembed 2:00c6646f42fb 48 Ethernet eth;
eembed 2:00c6646f42fb 49
eembed 2:00c6646f42fb 50
eembed 2:00c6646f42fb 51 //--------------------------=Thread Variables=-----------------------------------
eembed 1:db36f62f783b 52 Ticker time_up;
eembed 1:db36f62f783b 53 float startTime=0.0;
eembed 1:db36f62f783b 54 float tempTime = 0.0;
eembed 1:db36f62f783b 55 float endTime=0.0;
eembed 1:db36f62f783b 56 Timer timer;
eembed 1:db36f62f783b 57 float preTime = 0.0;
eembed 2:00c6646f42fb 58
eembed 2:00c6646f42fb 59 //--------------------------=Velocity reading variables=-------------------------
eembed 1:db36f62f783b 60 char buf[0x600];
eembed 1:db36f62f783b 61 float recv_angle = 0.0;
eembed 1:db36f62f783b 62 float x_res = 0.0;
eembed 2:00c6646f42fb 63 float dt = 0.000001*200.0;
eembed 1:db36f62f783b 64 float ve_sum = 0.0;
eembed 1:db36f62f783b 65 float dx_res = 0.0;
eembed 1:db36f62f783b 66 float G_filter_v = 100.0;
eembed 1:db36f62f783b 67 float duty = 0.0;
eembed 2:00c6646f42fb 68
eembed 2:00c6646f42fb 69 //--------------------------=DoB variables=--------------------------------------
eembed 1:db36f62f783b 70 float K_tn=0.135;
eembed 1:db36f62f783b 71 float J_n = 0.0000720;
eembed 2:00c6646f42fb 72 float B = 0.0;
eembed 2:00c6646f42fb 73
eembed 2:00c6646f42fb 74 float g_dis = 1.0;
eembed 1:db36f62f783b 75 float D_DoB = 0.0;
eembed 1:db36f62f783b 76 float D_BFilter = 0.0;
eembed 1:db36f62f783b 77 float D_AFilter = 0.0;
eembed 2:00c6646f42fb 78 float I_DoB = 0.0;
eembed 2:00c6646f42fb 79
eembed 2:00c6646f42fb 80 //--------------------------=RTOB Variables=-------------------------------------
eembed 2:00c6646f42fb 81 float RT_BFilter=0.0;
eembed 2:00c6646f42fb 82 float RT_AFilter = 0.0;
eembed 2:00c6646f42fb 83 float RTOB = 0.0;
eembed 2:00c6646f42fb 84 //-------------------------=slave current controller variables=-----------------
eembed 2:00c6646f42fb 85 int count = 0;
eembed 2:00c6646f42fb 86 int countprev = 0;
eembed 2:00c6646f42fb 87 float I_ref = 1.0;
eembed 2:00c6646f42fb 88 float K_pis =5.0;
eembed 2:00c6646f42fb 89 float K_dis = 0.0;
eembed 2:00c6646f42fb 90 float K_iis = 0.0;
eembed 0:5459cdde6298 91
eembed 2:00c6646f42fb 92 float I_error_s = 0.0;
eembed 2:00c6646f42fb 93 float I_ref_s = 0.0;
eembed 2:00c6646f42fb 94 float I_err_sum_s = 0.0;
eembed 2:00c6646f42fb 95 float I_PID_s = 0.0;
eembed 2:00c6646f42fb 96 float I_err_prev_s = 0.0;
eembed 2:00c6646f42fb 97 float duty_s = 0.0;
eembed 2:00c6646f42fb 98
eembed 2:00c6646f42fb 99 float I_res_s = 0.0;
eembed 2:00c6646f42fb 100 float I1_act_s = 0.0;
eembed 2:00c6646f42fb 101 float I_msrd_s = 0.0;
eembed 2:00c6646f42fb 102 //--------------------------=Velocity controller variables=---------------------
eembed 2:00c6646f42fb 103 float V_error=0.0;
eembed 2:00c6646f42fb 104 float V_cmd =50.0;
eembed 2:00c6646f42fb 105 float k_pv = 0.02;
eembed 2:00c6646f42fb 106 float V_pid = 0.0;
eembed 2:00c6646f42fb 107 float dx_res_prev = 0.0;
eembed 2:00c6646f42fb 108 float dxdx_res = 0.0;
eembed 2:00c6646f42fb 109
eembed 2:00c6646f42fb 110 //--------------------------=Read Velocity=-------------------------------------
eembed 1:db36f62f783b 111 void readVelocity(){
eembed 2:00c6646f42fb 112 int size2 = eth.receive();
eembed 2:00c6646f42fb 113 if(size2 > 0)
eembed 2:00c6646f42fb 114 {
eembed 2:00c6646f42fb 115 eth.read(buf, size2);
eembed 2:00c6646f42fb 116 memcpy(&recv_angle, buf, sizeof(float));
eembed 2:00c6646f42fb 117 x_res = -1*recv_angle;
eembed 2:00c6646f42fb 118 }
eembed 2:00c6646f42fb 119 dx_res_prev = dx_res;
eembed 2:00c6646f42fb 120 ve_sum += dx_res*dt;
eembed 2:00c6646f42fb 121 dx_res =G_filter_v*( x_res-ve_sum);
eembed 2:00c6646f42fb 122 dxdx_res = (dx_res-dx_res_prev)/dt;
eembed 2:00c6646f42fb 123 }
eembed 2:00c6646f42fb 124
eembed 2:00c6646f42fb 125
eembed 2:00c6646f42fb 126 //--------------------------=Distarbance Observer=------------------------------
eembed 2:00c6646f42fb 127 void DoB()
eembed 2:00c6646f42fb 128 {
eembed 2:00c6646f42fb 129 D_BFilter=(I_msrd*K_tn) + (dx_res*J_n*g_dis);
eembed 2:00c6646f42fb 130 D_AFilter += g_dis*(D_BFilter-D_AFilter)*dt;
eembed 2:00c6646f42fb 131 D_DoB = D_AFilter - (dx_res*J_n*g_dis);
eembed 2:00c6646f42fb 132 I_DoB = D_DoB/K_tn;
eembed 0:5459cdde6298 133
eembed 2:00c6646f42fb 134 //--------------------------=Reaction Force Observer=------------------------------
eembed 2:00c6646f42fb 135 /* RT_BFilter = D_BFilter - (T_f+B*dx_res);
eembed 2:00c6646f42fb 136 RT_AFilter += g_dis*(RT_BFilter-RT_AFilter)*dt;
eembed 2:00c6646f42fb 137 RTOB = RT_AFilter - (dx_res*J_n*g_dis);*/
eembed 2:00c6646f42fb 138 }
eembed 2:00c6646f42fb 139
eembed 2:00c6646f42fb 140
eembed 2:00c6646f42fb 141 //--------------------------=Motor PWM Initialization=--------------------------
eembed 2:00c6646f42fb 142 void motorPWM_init()
eembed 2:00c6646f42fb 143 {
eembed 1:db36f62f783b 144 pwm_clk.period_us(10);
eembed 2:00c6646f42fb 145 pwm_clk_s.period_us(10);
eembed 2:00c6646f42fb 146 pwm_anticlk_s.period_us(10);
eembed 1:db36f62f783b 147 pwm_anticlk.period_us(10);
eembed 1:db36f62f783b 148 pwm_clk.write(0.0f);
eembed 1:db36f62f783b 149 pwm_anticlk.write(0.0f);
eembed 2:00c6646f42fb 150 pwm_clk_s.write(0.0f);
eembed 2:00c6646f42fb 151 pwm_anticlk_s.write(0.0f);
eembed 1:db36f62f783b 152 Reset_AB = 1;
eembed 1:db36f62f783b 153 Reset_CD = 1;
eembed 2:00c6646f42fb 154 Reset_AB_s =1;
eembed 2:00c6646f42fb 155 Reset_CD_s =1;
eembed 2:00c6646f42fb 156 }
eembed 2:00c6646f42fb 157
eembed 2:00c6646f42fb 158 //--------------------------=Motor PWM Generation=------------------------------
eembed 2:00c6646f42fb 159 void motorPWM()
eembed 2:00c6646f42fb 160 {
eembed 2:00c6646f42fb 161 duty = V_pid;
eembed 2:00c6646f42fb 162 if (duty> 0.0)
eembed 2:00c6646f42fb 163 {
eembed 2:00c6646f42fb 164 if (duty > 0.9)
eembed 2:00c6646f42fb 165 {
eembed 2:00c6646f42fb 166 duty = 0.9;
eembed 1:db36f62f783b 167 }
eembed 1:db36f62f783b 168 pwm_clk = 0.0;
eembed 1:db36f62f783b 169 pwm_anticlk = duty;
eembed 1:db36f62f783b 170 }
eembed 0:5459cdde6298 171
eembed 2:00c6646f42fb 172 if (duty < 0.0)
eembed 2:00c6646f42fb 173 {
eembed 2:00c6646f42fb 174 if (duty< -0.9)
eembed 2:00c6646f42fb 175 {
eembed 2:00c6646f42fb 176 duty = -0.9;
eembed 1:db36f62f783b 177 }
eembed 1:db36f62f783b 178 pwm_anticlk = 0.0;
eembed 1:db36f62f783b 179 pwm_clk = -1.0 * duty;
eembed 1:db36f62f783b 180 }
eembed 2:00c6646f42fb 181 }
eembed 2:00c6646f42fb 182
eembed 2:00c6646f42fb 183 //---------------------------=Slave motor current reading=----------------------
eembed 2:00c6646f42fb 184 void slaveCurrentRead()
eembed 2:00c6646f42fb 185 {
eembed 2:00c6646f42fb 186 slave_Direction = S_Dir.read();
eembed 2:00c6646f42fb 187
eembed 2:00c6646f42fb 188 if(slave_Direction == 0)
eembed 2:00c6646f42fb 189 {
eembed 2:00c6646f42fb 190 I_res_s = current_sensor_s_p.read();
eembed 2:00c6646f42fb 191 I1_act_s = -1.0*((I_res_s*3.3/0.7) );//0.74787687701613 //0.717075441532258
eembed 2:00c6646f42fb 192
eembed 2:00c6646f42fb 193 }else if(slave_Direction == 1) { //master anticlockwise
eembed 2:00c6646f42fb 194 I_res_s = current_sensor_s_n.read();
eembed 2:00c6646f42fb 195 I1_act_s = 1.0*((I_res_s*3.3)/0.7); //0.713239227822580
eembed 1:db36f62f783b 196 }
eembed 2:00c6646f42fb 197 I_msrd_s += G_Cfilter*(I1_act_s-I_msrd_s)*dt;
eembed 2:00c6646f42fb 198
eembed 1:db36f62f783b 199 }
eembed 2:00c6646f42fb 200 //--------------------------=File print test=-----------------------------------
eembed 1:db36f62f783b 201 void sd_card_write_test()
eembed 1:db36f62f783b 202 {
eembed 1:db36f62f783b 203 mkdir("/sd/mydir", 0777);
eembed 1:db36f62f783b 204
eembed 1:db36f62f783b 205 FILE *fp = fopen("/sd/mydir/sdtest.txt", "w");
eembed 1:db36f62f783b 206 if(fp == NULL) {
eembed 1:db36f62f783b 207 error("Could not open file for write\n");
eembed 1:db36f62f783b 208 }
eembed 1:db36f62f783b 209 fprintf(fp, "Hello fun SD Card World!");
eembed 2:00c6646f42fb 210 //fclose(fp);
eembed 1:db36f62f783b 211
eembed 1:db36f62f783b 212 //fprintf(fp,"startTime\t I_ref\t I_msrd\t dx_res\t\n");
eembed 0:5459cdde6298 213 }
eembed 0:5459cdde6298 214
eembed 2:00c6646f42fb 215 //--------------------------=File print=----------------------------------------
eembed 1:db36f62f783b 216 void sd_card_write()
eembed 1:db36f62f783b 217 {
eembed 2:00c6646f42fb 218 //led1=!led1;
eembed 1:db36f62f783b 219 FILE *fp = fopen("/sd/mydir/sdtest.txt","a");
eembed 2:00c6646f42fb 220 fprintf(fp,"%f\t \r\n",I_msrd_s);
eembed 1:db36f62f783b 221 fclose(fp);
eembed 0:5459cdde6298 222 }
eembed 0:5459cdde6298 223
eembed 2:00c6646f42fb 224 //--------------------------=Ethernet Initialization=---------------------------
eembed 2:00c6646f42fb 225
eembed 1:db36f62f783b 226 void ethernet_init()
eembed 1:db36f62f783b 227 {
eembed 1:db36f62f783b 228 eth.set_link(Ethernet::HalfDuplex100);
eembed 1:db36f62f783b 229 wait_ms(1000); // Needed after startup.
eembed 1:db36f62f783b 230 if(eth.link())
eembed 1:db36f62f783b 231 {
eembed 1:db36f62f783b 232 for(int i=0;i<3;i++)
eembed 1:db36f62f783b 233 {
eembed 1:db36f62f783b 234 led3=!led3;
eembed 1:db36f62f783b 235 wait(1.0);
eembed 1:db36f62f783b 236 }
eembed 0:5459cdde6298 237 }
eembed 0:5459cdde6298 238 }
eembed 2:00c6646f42fb 239 //--------------------------=velocity controller=-------------------------------
eembed 2:00c6646f42fb 240 void velocitycontroller(){
eembed 2:00c6646f42fb 241 V_error=(V_cmd*0.1047198-dx_res);
eembed 2:00c6646f42fb 242 V_pid = k_pv*V_error;
eembed 2:00c6646f42fb 243
eembed 2:00c6646f42fb 244 }
eembed 2:00c6646f42fb 245 //--------------------------=Main Control Loop=---------------------------------
eembed 2:00c6646f42fb 246 void controlLoop(){
eembed 2:00c6646f42fb 247 count = count+1;
eembed 2:00c6646f42fb 248 if (count<100000){
eembed 2:00c6646f42fb 249 if (count>1000 and count<1500){
eembed 2:00c6646f42fb 250 V_cmd=100.0;}
eembed 2:00c6646f42fb 251 else if (count>1500 and count<2000){
eembed 2:00c6646f42fb 252 V_cmd=150.0;}
eembed 2:00c6646f42fb 253 else if (count>2000 and count<2500){
eembed 2:00c6646f42fb 254 V_cmd=200.0;}
eembed 2:00c6646f42fb 255 else if (count>2500 and count<3000){
eembed 2:00c6646f42fb 256 V_cmd=250.0;}
eembed 2:00c6646f42fb 257 else if (count>3000 and count<3500){
eembed 2:00c6646f42fb 258 V_cmd=400.0;}
eembed 2:00c6646f42fb 259 else if (count>3500 and count<4000){
eembed 2:00c6646f42fb 260 V_cmd=200.0;}
eembed 2:00c6646f42fb 261 else if (count>4000 and count<4500){
eembed 2:00c6646f42fb 262 V_cmd=400.0;}
eembed 2:00c6646f42fb 263 else if (count>4500 and count<20000){
eembed 2:00c6646f42fb 264 V_cmd=100.0+50.0*sin(0.00314159265*(count-4500));}
eembed 2:00c6646f42fb 265 else if (count>20000 and count<40000){
eembed 2:00c6646f42fb 266 V_cmd=200.0+100.0*sin(0.0062831853*(count-20000));}
eembed 2:00c6646f42fb 267 else if (count>40000 and count<60000){
eembed 2:00c6646f42fb 268 V_cmd=150.0;}
eembed 2:00c6646f42fb 269 else if (count>60000 and count<80000){
eembed 2:00c6646f42fb 270 V_cmd=300.0+150.0*sin(0.0062831853*(count-60000));}
eembed 2:00c6646f42fb 271 else if (count>80000 and count<100000){
eembed 2:00c6646f42fb 272 V_cmd = 250.0;}
eembed 2:00c6646f42fb 273 }
eembed 2:00c6646f42fb 274 else if (count>100000 and count<200000){
eembed 2:00c6646f42fb 275 if (count>100000 and count<100500){
eembed 2:00c6646f42fb 276 V_cmd=150.0;}
eembed 2:00c6646f42fb 277 else if (count>100500 and count<102000){
eembed 2:00c6646f42fb 278 V_cmd=50.0;}
eembed 2:00c6646f42fb 279 else if (count>102000 and count<102500){
eembed 2:00c6646f42fb 280 V_cmd=100.0;}
eembed 2:00c6646f42fb 281 else if (count>102500 and count<103000){
eembed 2:00c6646f42fb 282 V_cmd=200.0;}
eembed 2:00c6646f42fb 283 else if (count>103000 and count<103500){
eembed 2:00c6646f42fb 284 V_cmd=600.0;}
eembed 2:00c6646f42fb 285 else if (count>103500 and count<104000){
eembed 2:00c6646f42fb 286 V_cmd=300.0;}
eembed 2:00c6646f42fb 287 else if (count>104000 and count<104500){
eembed 2:00c6646f42fb 288 V_cmd=250.0;}
eembed 2:00c6646f42fb 289 else if (count>104500 and count<120000){
eembed 2:00c6646f42fb 290 V_cmd=150.0+60.0*sin(0.00314159265*(count-104500));}
eembed 2:00c6646f42fb 291 else if (count>120000 and count<140000){
eembed 2:00c6646f42fb 292 V_cmd=250.0+150.0*sin(0.0062831853*(count-120000));}
eembed 2:00c6646f42fb 293 else if (count>140000 and count<160000){
eembed 2:00c6646f42fb 294 V_cmd=550.0;}
eembed 2:00c6646f42fb 295 else if (count>160000 and count<180000){
eembed 2:00c6646f42fb 296 V_cmd=350.0+200.0*sin(0.0062831853*(count-160000));}
eembed 2:00c6646f42fb 297 else if (count>180000 and count<200000){
eembed 2:00c6646f42fb 298 V_cmd = 500.0;}
eembed 2:00c6646f42fb 299 }
eembed 2:00c6646f42fb 300 else if (count>200000 and count<300000){
eembed 2:00c6646f42fb 301 if (count>200000 and count<200500){
eembed 2:00c6646f42fb 302 V_cmd=70.0;}
eembed 2:00c6646f42fb 303 else if (count>200500 and count<202000){
eembed 2:00c6646f42fb 304 V_cmd=120.0;}
eembed 2:00c6646f42fb 305 else if (count>202000 and count<202500){
eembed 2:00c6646f42fb 306 V_cmd=300.0;}
eembed 2:00c6646f42fb 307 else if (count>202500 and count<203000){
eembed 2:00c6646f42fb 308 V_cmd=100.0;}
eembed 2:00c6646f42fb 309 else if (count>203000 and count<203500){
eembed 2:00c6646f42fb 310 V_cmd=500.0;}
eembed 2:00c6646f42fb 311 else if (count>203500 and count<204000){
eembed 2:00c6646f42fb 312 V_cmd=400.0;}
eembed 2:00c6646f42fb 313 else if (count>204000 and count<204500){
eembed 2:00c6646f42fb 314 V_cmd=250.0;}
eembed 2:00c6646f42fb 315 else if (count>204500 and count<220000){
eembed 2:00c6646f42fb 316 V_cmd=450.0+80.0*sin(0.00314159265*(count-204500));}
eembed 2:00c6646f42fb 317 else if (count>220000 and count<240000){
eembed 2:00c6646f42fb 318 V_cmd=350.0+150.0*sin(0.0062831853*(count-220000));}
eembed 2:00c6646f42fb 319 else if (count>240000 and count<260000){
eembed 2:00c6646f42fb 320 V_cmd=150.0;}
eembed 2:00c6646f42fb 321 else if (count>260000 and count<280000){
eembed 2:00c6646f42fb 322 V_cmd=350.0+200.0*sin(0.0062831853*(count-260000));}
eembed 2:00c6646f42fb 323 else if (count>280000 and count<300000){
eembed 2:00c6646f42fb 324 V_cmd = 400.0;}
eembed 2:00c6646f42fb 325 }
eembed 2:00c6646f42fb 326 slaveCurrentRead();
eembed 2:00c6646f42fb 327 readVelocity();
eembed 2:00c6646f42fb 328 velocitycontroller();
eembed 2:00c6646f42fb 329 DoB();
eembed 2:00c6646f42fb 330 motorPWM();
eembed 2:00c6646f42fb 331
eembed 2:00c6646f42fb 332 }
eembed 0:5459cdde6298 333
eembed 1:db36f62f783b 334 //----------------------------------=Main Loop=---------------------------------
eembed 1:db36f62f783b 335 int main()
eembed 1:db36f62f783b 336 {
eembed 2:00c6646f42fb 337
eembed 2:00c6646f42fb 338 sd_card_write_test();
eembed 1:db36f62f783b 339 ethernet_init();
eembed 1:db36f62f783b 340 motorPWM_init();
eembed 2:00c6646f42fb 341 FILE *fp = fopen("/sd/mydir/sdtest.txt","w");
eembed 0:5459cdde6298 342 timer.start();
eembed 1:db36f62f783b 343 time_up.attach(&controlLoop, dt);
eembed 2:00c6646f42fb 344 //Thread thread(*thread_2,NULL,osPriorityAboveNormal,DEFAULT_STACK_SIZE*10.0,NULL);
eembed 2:00c6646f42fb 345 while(1)
eembed 2:00c6646f42fb 346 {
eembed 2:00c6646f42fb 347 if (count<3500 and count!=countprev and count%2==0){ //350000
eembed 2:00c6646f42fb 348 led1=0;
eembed 2:00c6646f42fb 349 fprintf(fp,"%d %f %f %f %f\n",count, D_DoB,I_msrd_s,dx_res,dxdx_res); //9.54929658551
eembed 2:00c6646f42fb 350 countprev=count;
eembed 2:00c6646f42fb 351 }
eembed 2:00c6646f42fb 352 else if(count>5000){
eembed 2:00c6646f42fb 353 led1=1;
eembed 2:00c6646f42fb 354 fclose(fp);
eembed 2:00c6646f42fb 355 }
eembed 2:00c6646f42fb 356 //pc.printf("%f\t \r\n",I_msrd_s); //dx_res*9.549296586
eembed 2:00c6646f42fb 357 //sd_card_write();
eembed 2:00c6646f42fb 358 }
eembed 2:00c6646f42fb 359 }