Francesco Pistone
new_2019
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 //******************************************************************************************************************** 00002 //******************************************************************************************************************** 00003 // FIRMWARE SEMINATRICE MODULA 00004 // VERSIONE PER SCHEDA DI CONTROLLO CON DRIVER INTEGRATI 00005 // V4 - ATTENZIONE - LA VERSIONE V4 HA IL DRIVER STEPPER LV8727 00006 // IL MOTORE DC E' GESTITO CON IL DRIVER VNH3SP30-E E CON LA LETTURA 00007 // DELLA CORRENTE ASSORBITA TRAMITE IL CONVERTITORE MLX91210-CAS102 CON 50A FONDOSCALA 00008 // CHE FORNISCE UNA TENSIONE DI USCITA PARI A 40mV/A 00009 // FIRST RELEASE OF BOARD DEC 2017 00010 // FIRST RELEASE OF FIRMWARE JAN 2018 00011 // 00012 // THIS RELEASE: 29 MAJ 2018 00013 // 00014 // APPLICATION: MODULA CON DISTRIBUTORE ZUCCA OPPURE RISO E PUO' FUNZIONARE ANCHE CON SENSORE A 25 FORI SUL DISCO O 00015 // ENCODER MOTORE SETTANDO GLI APPOSITI FLAGS 00016 // 00017 // 29 05 2018 - INSERITO SECONDO ENCODER VIRTUALE PER LA GESTIONE DEL SINCRONISMO TRA TAMBURO E RUOTA DI SEMINA 00018 // IN PRATICA IL PRIMO ENCODER è SINCRONO CON IL SEGNALE DEI BECCHI E VIENE AZZERATO DA QUESTI, MENTRE 00019 // IL SECONDO E' INCREMENTATO IN SINCRONO CON IL PRIMO MA AZZERATO DALLA FASE. IL SUO VALORE E' POI DIVISO 00020 // PER IL RAPPORTO RUOTE E LA CORREZIONE AGISCE SULLA VELOCITA' DEL TAMBURO PER MANTENERE LA FASE DEL SECONDO 00021 // ENCODER 00022 // 05 06 2018 - INSERITO IL CONTROLLO DI GESTIONE DEL QUINCONCE SENZA ENCODER 00023 // 00024 //******************************************************************************************************************** 00025 //******************************************************************************************************************** 00026 00027 #include "main.hpp" 00028 #include "timeandtick.hpp" 00029 #include "canbus.hpp" 00030 #include "iodefinition.hpp" 00031 #include "parameters.hpp" 00032 #include "variables.hpp" 00033 //******************************************************************************************************************** 00034 //******************************************************************************************************************** 00035 // ------------------------------------------------------------------------------------------------------------------------------------------------------------------ 00036 // TASK SECTION 00037 // ------------------------------------------------------------------------------------------------------------------------------------------------------------------ 00038 //************************************************************************ 00039 // rise of seed speed 25 pulse sensor 00040 void sd25Fall(){ 00041 timeHole=metalTimer.read_ms(); 00042 int memo_TimeHole= (memoTimeHole + timeHole)/ (int)2; 00043 memoTimeHole = timeHole; 00044 metalTimer.reset(); 00045 if (speedFromPick==0){ 00046 speedOfSeedWheel=((seedPerimeter/25.0f)/(double)memo_TimeHole)*1000.0f; //mtS 00047 } 00048 } 00049 //************************************************** 00050 // generate speed clock when speed is simulated from Tritecnica display 00051 void speedSimulationClock(){ 00052 lastPulseRead=speedTimer.read_us(); 00053 oldLastPulseRead=lastPulseRead; 00054 speedTimer.reset(); 00055 pulseRised=1; 00056 } 00057 //******************************************************* 00058 // interrupt task for tractor speed reading 00059 //******************************************************* 00060 void tractorReadSpeed(){ 00061 if ((oldTractorSpeedRead==0)){ 00062 lastPulseRead=speedTimer.read_us(); 00063 oldLastPulseRead=lastPulseRead; 00064 speedTimer.reset(); 00065 pulseRised=1; 00066 oldTractorSpeedRead=1; 00067 } 00068 speedFilter.reset(); 00069 speedClock=1; 00070 } 00071 //******************************************************* 00072 void speedMediaCalc(){ 00073 double lastPd=(double) lastPulseRead/1000.0f; 00074 pulseSpeedInterval = (mediaSpeed[0]+lastPd)/2.0f; 00075 if (enableSimula==1){ 00076 double TMT = (double)(speedSimula) * 100.0f /3600.0f; 00077 pulseSpeedInterval = pulseDistance / TMT; 00078 } 00079 mediaSpeed[0]=lastPd; 00080 OLDpulseSpeedInterval=pulseSpeedInterval; 00081 } 00082 00083 //******************************************************* 00084 // clocked task for manage virtual encoder of seed wheel i/o 00085 //******************************************************* 00086 //******************************************************* 00087 void step_SDPulseOut(){ 00088 SDactualPosition++; 00089 prePosSD++; 00090 } 00091 //******************************************************* 00092 void step_TBPulseOut(){ 00093 TBmotorStepOut=!TBmotorStepOut; 00094 if (TBmotorStepOut==0){ 00095 TBactualPosition++; 00096 } 00097 } 00098 //******************************************************* 00099 // aggiornamento parametri di lavoro fissi e da Tritecnica 00100 void aggiornaParametri(){ 00101 speedPerimeter = Pi * speedWheelDiameter ; // perimeter of speed wheel 00102 pulseDistance = (speedPerimeter / speedWheelPulse)*1000.0f; // linear space between speed wheel pulse 00103 seedPerimeter = Pi * (seedWheelDiameter-(deepOfSeed*2.0f)); // perimeter of seed wheel 00104 intraPickDistance = seedPerimeter/pickNumber; 00105 K_WheelRPM = 60.0f/seedPerimeter; // calcola il K per i giri al minuto della ruota di semina 00106 K_WhellFrequency = (seedWheelMotorSteps*SDreductionRatio)/60.0f; // calcola il K per la frequenza di comando del motore di semina 00107 rapportoRuote = pickNumber/cellsNumber; // calcola il rapporto tra il numero di becchi ed il numero di celle 00108 K_TBfrequency = (TBmotorSteps*TBreductionRatio)/240.0f; 00109 K_percentuale = TBmotorSteps*TBreductionRatio; 00110 SDsectorStep = (double)fixedStepGiroSD / (double)pickNumber; 00111 TBsectorStep = (TBmotorSteps*TBreductionRatio)/cellsNumber; 00112 KcorT = (SDsectorStep/TBsectorStep);///2.0f; 00113 angoloFase=angoloPh; 00114 avvioGradi=angoloAv; 00115 stepGrado=fixedStepGiroSD/360.0f; 00116 TBdeltaStep=(fixedStepGiroSD/pickNumber)+(stepGrado*avvioGradi); 00117 TBfaseStep = (stepGrado*angoloFase); 00118 K_WheelRPM = 60.0f/seedPerimeter; // calcola il K per i giri al minuto della ruota di semina 25.4 25,40 00119 if (speedFromPick==1) { 00120 intraPickDistance = seedPerimeter/pickNumber; 00121 }else{ 00122 intraPickDistance = seedPerimeter/25.0f; // 25 è il numero di fori presenti nel disco di semina 00123 } 00124 if (anticipoMax > ((360.0f/pickNumber)-(avvioGradi+16.0f))){ 00125 anticipoMax=(360.0f/pickNumber)-(avvioGradi+16.0f); 00126 } 00127 } 00128 //******************************************************* 00129 void cambiaTB(double perio){ 00130 // update TB frequency 00131 double TBper=0.0f; 00132 if (aspettaStart==0){ 00133 if (perio<250.0f){perio=500.0f;} 00134 double scala =0.0f; 00135 if (lowSpeed==1){ 00136 scala =2.0f; 00137 }else{ 00138 scala =1.8f; 00139 } 00140 TBper=perio/scala; 00141 if (oldPeriodoTB!=TBper){ 00142 if (TBper >= 250.0f){ 00143 TBticker.attach_us(&step_TBPulseOut,TBper); // clock time are milliseconds and attach seed motor stepper controls 00144 }else{ 00145 TBticker.detach(); 00146 } 00147 oldPeriodoTB=TBper; 00148 } 00149 } 00150 } 00151 //******************************************************* 00152 void seedCorrect(){ 00153 /* 00154 posError determina la posizione relativa di TB rispetto ad SD 00155 la reale posizione di SD viene modificata in funzione della velocità per 00156 traslare la posizione relativa di TB. All'aumentare della velocità la posizione 00157 di SD viene incrementata così che TB acceleri per raggiungerla in modo da rilasciare il seme prima 00158 La taratura del sistema avviene determinando prima il valore di angoloFase alla minima velocità, 00159 poi, alla massima velocità, dovrebbe spostarsi la posizione relativa con una variabile proporzionale alla velocità, ma c'è un però. 00160 Il problema è che il momento di avvio determina una correzione dell'angolo di partenza del tamburo 00161 angolo che viene rideterminato ogni volta che il sensore becchi legge un transito. 00162 Di fatto c'è una concorrenza tra l'angolo di avvio determinato e la correzione di posizione relativa 00163 del tamburo. E' molto probabile che convenga modificare solo la posizione relativa e non anche l'angolo di avvio 00164 Ancora di più se viene eliminata la parte gestita da ciclata. 00165 In questo modo dovrebbe esserci solo un andamento in accelerazione di TB che viene poi eventualmente decelerato 00166 dal passaggio sul sensore di TB. Funzione corretta perchè il sincronismo tra i sensori genera l'inibizione della correzione 00167 di fase di TB. In pratica il ciclo viene resettato al passaggio sul sensore di SD che riporta a 0 la posizione di SD. 00168 Appena il sensore di TB viene impegnato allora viene abilitato il controllo di fase del tamburo. 00169 Questo si traduce nel fatto che il controllo di posizione viene gestito solo all'interno di uno slot di semina in modo che 00170 il tamburo non risenta della condizione di reset della posizione di SD mentre lui è ancora nella fase precedente. Si fermerebbe. 00171 00172 // La considerazione finale è che mantenendo l'angolo di avvio fisso e regolato sulla bassa velocità, intervenendo solo sulla correzione 00173 // di posizione in questa routine, dovrebbe essere possibile seminare correttamente a tutte le velocità regolando solo 2 parametri. 00174 */ 00175 /* 00176 SDsectorStep = (double)fixedStepGiroSD / (double)pickNumber; 00177 TBsectorStep = (TBmotorSteps*TBreductionRatio)/cellsNumber; 00178 KcorT = (SDsectorStep/TBsectorStep); 00179 angoloFase=angoloPh; 00180 stepGrado=fixedStepGiroSD/360.0f; 00181 avvioGradi = costante da terminale tritecnica 00182 TBdeltaStep=(fixedStepGiroSD/pickNumber)-(stepGrado*avvioGradi); 00183 TBfaseStep = (stepGrado*angoloFase); 00184 anticipoMax = valore in gradi del massimo anticipo proporzionale alla velocità della ruota di semina 00185 */ 00186 00187 if ((tractorSpeed_MtS_timed>0.01f)){ 00188 if (inhibit==0){ 00189 double posError =0.0f; 00190 double posSD=((double)SDactualPosition)/KcorT; 00191 posError = posSD - (double)TBactualPosition; 00192 // interviene sulla velocità di TB per raggiungere la corretta posizione relativa 00193 if((lowSpeed==0)&&(aspettaStart==0)){ 00194 if (posError>50.0f){posError=50.0f;} 00195 if (posError<-50.0f){posError=-50.0f;} 00196 if ((posError >=1.0f)||(posError<=-1.0f)){ 00197 ePpos = periodo /(1.0f+ ((posError/100.0f))); 00198 cambiaTB(ePpos); 00199 } 00200 } 00201 } 00202 #if defined(pcSerial) 00203 #if defined(SDreset) 00204 pc.printf("Fase: %d",fase); 00205 pc.printf(" PrePosSD: %d",prePosSD); 00206 pc.printf(" PosSD: %d",SDactualPosition); 00207 pc.printf(" speed: %f",tractorSpeed_MtS_timed); 00208 pc.printf(" Trigger: %d \n", trigRepos); 00209 #endif 00210 #endif 00211 } 00212 } 00213 //******************************************************* 00214 void videoUpdate(){ 00215 for(int aa=0;aa<4;aa++){speedForDisplay[aa]=speedForDisplay[aa+1];} 00216 speedForDisplay[4]=tractorSpeed_MtS_timed; 00217 totalSpeed=0.0f; 00218 for (int aa=0; aa<5; aa++){totalSpeed += speedForDisplay[aa];} 00219 totalSpeed = totalSpeed / 5.0f; 00220 } 00221 //******************************************************* 00222 void ciclaTB(){ 00223 if ((startCicloTB==1)&&(cicloTbinCorso==0)){ 00224 TBticker.attach_us(&step_TBPulseOut,TBperiod/2.5f); // clock time are milliseconds and attach seed motor stepper controls 00225 cicloTbinCorso = 1; 00226 startCicloTB=0; 00227 } 00228 if ((loadDaCan==1)&&(loadDaCanInCorso==0)){ 00229 TBticker.attach_us(&step_TBPulseOut,1000.0f); // clock time are milliseconds and attach seed motor stepper controls 00230 loadDaCanInCorso=1; 00231 stopCicloTB=0; 00232 } 00233 if ((stopCicloTB==1)&&(TBactualPosition>5)){ 00234 TBticker.detach(); 00235 cicloTbinCorso = 0; 00236 stopCicloTB=0; 00237 loadDaCanInCorso=0; 00238 loadDaCan=0; 00239 } 00240 } 00241 //******************************************************* 00242 void allarmi(){ 00243 uint8_t alarmLowRegister1=0x00; 00244 alarmLowRegister=0x00; 00245 alarmHighRegister=0x80; 00246 00247 //alarmLowRegister=alarmLowRegister+(all_semiFiniti*0x01); // manca il sensore 00248 alarmLowRegister=alarmLowRegister+(all_pickSignal*0x02); // fatto 00249 alarmLowRegister=alarmLowRegister+(all_cellSignal*0x04); // fatto 00250 //alarmLowRegister=alarmLowRegister+(all_lowBattery*0x08); // non gestito 00251 //alarmLowRegister=alarmLowRegister+(all_overCurrDC*0x10); 00252 alarmLowRegister=alarmLowRegister+(all_stopSistem*0x20); // verificarne la necessità 00253 //alarmLowRegister=alarmLowRegister+(all_upElements*0x40); // manca il sensore 00254 //alarmLowRegister=alarmLowRegister+(all_noSeedOnCe*0x80); // manca il sensore 00255 00256 //alarmLowRegister1=alarmLowRegister1+(all_cfgnErrors*0x01); // da scrivere 00257 alarmLowRegister1=alarmLowRegister1+(all_noDcRotati*0x02); // fatto 00258 alarmLowRegister1=alarmLowRegister1+(all_noStepRota*0x04); // fatto 00259 alarmLowRegister1=alarmLowRegister1+(all_speedError*0x08); // fatto 00260 alarmLowRegister1=alarmLowRegister1+(all_noSpeedSen*0x10); // fatto 00261 alarmLowRegister1=alarmLowRegister1+(all_no_Zeroing*0x20); // fatto 00262 alarmLowRegister1=alarmLowRegister1+(all_genericals*0x40); 00263 if (alarmLowRegister1 > 0){ 00264 alarmHighRegister = 0x81; 00265 alarmLowRegister = alarmLowRegister1; 00266 } 00267 00268 #if defined(pcSerial) 00269 #if defined(VediAllarmi) 00270 if (all_pickSignal==1){pc.printf("AllarmeBecchi\n");} 00271 if (all_cellSignal==1){pc.printf("AllarmeCelle\n");} 00272 if (all_stopSistem==1){pc.printf("AllarmeStop\n");} 00273 if (all_noDcRotati==1){pc.printf("AllarmeDCnoRotation\n");} 00274 if (all_noStepRota==1){pc.printf("AllarmeNoStepRotation\n");} 00275 if (all_speedError==1){pc.printf("AllarmeSpeedError\n");} 00276 if (all_noSpeedSen==1){pc.printf("AllarmeNoSpeedSensor\n");} 00277 if (all_no_Zeroing==1){pc.printf("AllarmeNoZero\n");} 00278 if (all_genericals==1){pc.printf("AllarmeGenerico\n");} 00279 pc.printf("Code: 0x%x%x\n",alarmHighRegister,alarmLowRegister); 00280 #endif 00281 #endif 00282 all_semiFiniti=0; 00283 all_pickSignal=0; 00284 all_cellSignal=0; 00285 all_lowBattery=0; 00286 all_overCurrDC=0; 00287 all_stopSistem=0; 00288 all_upElements=0; 00289 all_noSeedOnCe=0; 00290 all_cfgnErrors=0; 00291 all_noDcRotati=0; 00292 all_noStepRota=0; 00293 all_speedError=0; 00294 all_noSpeedSen=0; 00295 all_no_Zeroing=0; 00296 all_genericals=0; 00297 } 00298 //******************************************************* 00299 void upDateSincro(){ 00300 char val1[8]={0,0,0,0,0,0,0,0}; 00301 val1[0]=(prePosSD /0x00FF0000)&0x000000FF; 00302 val1[1]=(prePosSD /0x0000FF00)&0x000000FF; 00303 val1[2]=(prePosSD /0x000000FF)&0x000000FF; 00304 val1[3]=prePosSD & 0x000000FF; 00305 #if defined(canbusActive) 00306 #if defined(speedMaster) 00307 if(can1.write(CANMessage(TX_SI, *&val1,8))){ 00308 checkState=0; 00309 } 00310 #endif 00311 #endif 00312 } 00313 //******************************************************* 00314 void upDateSpeed(){ 00315 /* 00316 aggiorna dati OPUSA3 00317 val1[0] contiene il dato di velocità 00318 val1[1] contiene il byte basso della tabella allarmi 00319 uint8_t all_semiFiniti = 0; // semi finiti (generato dal relativo sensore) 00320 uint8_t all_pickSignal = 0; // errore segnale becchi (generato dal tempo tra un becco ed il successivo) 00321 uint8_t all_cellSignal = 0; // errore segnale celle (generato dal sensore tamburo ) 00322 uint8_t all_lowBattery = 0; // allarme batteria (valore interno di tritecnica) 00323 uint8_t all_overCurrDC = 0; // sovracorrente motore DC (generato dal sensore di corrente) 00324 uint8_t all_stopSistem = 0; // sistema in stop (a tempo con ruota ferma) 00325 uint8_t all_upElements = 0; // elementi sollevati (generato dal relativo sensore) 00326 uint8_t all_noSeedOnCe = 0; // fallanza di semina (manca il seme nella cella) 00327 uint8_t all_cfgnErrors = 0; // errore di configurazione (incongruenza dei parametri impostati) 00328 uint8_t all_noDcRotati = 0; // ruota di semina bloccata (arriva la velocità ma non i becchi) 00329 uint8_t all_noStepRota = 0; // tamburo di semina bloccato (comando il tamburo ma non ricevo il sensore) 00330 uint8_t all_speedError = 0; // errore sensore velocità (tempo impulsi non costante) 00331 uint8_t all_noSpeedSen = 0; // mancanza segnale di velocità (sento i becchi ma non la velocita) 00332 uint8_t all_no_Zeroing = 0; // mancato azzeramento sistema (generato dal timeout del comando motore DC) 00333 uint8_t all_genericals = 0; // allarme generico 00334 val1[2] contiene il byte alto della tabella di allarmi 00335 val1[3] contiene i segnali per la diagnostica 00336 bit 0= sensore ruota fonica 00337 bit 1= sensore celle 00338 bit 2= sensore posizione 00339 bit 3= sensore becchi 00340 bit 4= motore DC 00341 bit 5= controller 00342 bit 6= motore stepper 00343 */ 00344 char val1[8]={0,0,0,0,0,0,0,0}; 00345 00346 val1[3]=0; 00347 val1[3]=val1[3]+(tractorSpeedRead*0x01); 00348 val1[3]=val1[3]+(TBzeroPinInputRev*0x02); 00349 val1[3]=val1[3]+(ElementPosition*0x04); 00350 val1[3]=val1[3]+(seedWheelZeroPinInputRev*0x08); 00351 #if defined(simulaPerCan) 00352 if (buttonUser==0){ 00353 val1[1]=0x02; 00354 }else{ 00355 val1[1]=0x00; 00356 } 00357 val1[3]=valore; 00358 valore+=1; 00359 if(valore>50){ 00360 valore=0; 00361 } 00362 tractorSpeed_MtS_timed=valore; 00363 #endif 00364 allarmi(); 00365 valore = totalSpeed*36.0f; // tractorSpeed_MtS_timed*36.0f; 00366 val1[0]=valore; 00367 val1[1]=alarmHighRegister; // registro alto allarmi. Parte sempre da 0x80 00368 val1[2]=alarmLowRegister; // registro basso allarmi 00369 //val1[3]=val1[3];// registro di stato degli ingressi 00370 val1[4]=resetComandi; 00371 val1[5]=cellsCounterLow; 00372 val1[6]=cellsCounterHig; 00373 #if defined(canbusActive) 00374 if(can1.write(CANMessage(TX_ID, *&val1,8))){ 00375 checkState=0; 00376 } 00377 #endif 00378 } 00379 //******************************************************* 00380 void leggiCAN(){ 00381 #if defined(canbusActive) 00382 if(can1.read(rxMsg)) { 00383 if (firstStart==1){ 00384 #if defined(speedMaster) 00385 sincUpdate.attach(&upDateSincro,0.01f); 00386 #endif 00387 canUpdate.attach(&upDateSpeed,0.11f); 00388 firstStart=0; 00389 } 00390 00391 if (rxMsg.id==RX_ID){ 00392 #if defined(pcSerial) 00393 #if defined(canDataReceived) 00394 pc.printf("Messaggio ricevuto\n"); 00395 #endif 00396 #endif 00397 } 00398 if (rxMsg.id==RX_Broadcast){ 00399 #if defined(pcSerial) 00400 #if defined(canDataReceived) 00401 pc.printf("BroadCast ricevuto\n"); 00402 #endif 00403 #endif 00404 enableSimula= rxMsg.data[0]; 00405 speedSimula = rxMsg.data[1]; 00406 avviaSimula = rxMsg.data[2]; 00407 selezionato = rxMsg.data[3]; 00408 comandiDaCan = rxMsg.data[4]; 00409 #if defined(pcSerial) 00410 #if defined(canDataReceived) 00411 pc.printf("Comandi: %x\n",comandiDaCan); 00412 #endif 00413 #endif 00414 switch (comandiDaCan){ 00415 case 1: 00416 case 3: 00417 azzeraDaCan=1; 00418 resetComandi=0x01; 00419 comandiDaCan=0; 00420 break; 00421 case 2: 00422 loadDaCan=1; 00423 resetComandi=0x02; 00424 comandiDaCan=0; 00425 break; 00426 default: 00427 comandiDaCan=0; 00428 resetComandi=0xFF; 00429 break; 00430 } 00431 #if defined(pcSerial) 00432 #if defined(canDataReceived) 00433 pc.printf("Comandi: %x\n",resetComandi); 00434 #endif 00435 #endif 00436 if (speedSimula>45){ 00437 speedSimula=45; 00438 } 00439 // modulo 1 00440 if (RX_ID==0x100){ 00441 if ((selezionato&0x01)==0x01){ 00442 simOk=1; 00443 }else{ 00444 simOk=0; 00445 } 00446 } 00447 // modulo 2 00448 if (RX_ID==0x102){ 00449 if ((selezionato&0x02)==0x02){ 00450 simOk=1; 00451 }else{ 00452 simOk=0; 00453 } 00454 } 00455 // modulo 3 00456 if (RX_ID==0x104){ 00457 if ((selezionato&0x04)==0x04){ 00458 simOk=1; 00459 }else{ 00460 simOk=0; 00461 } 00462 } 00463 // modulo 4 00464 if (RX_ID==0x106){ 00465 if ((selezionato&0x08)==0x08){ 00466 simOk=1; 00467 }else{ 00468 simOk=0; 00469 } 00470 } 00471 // modulo 5 00472 if (RX_ID==0x108){ 00473 if ((selezionato&0x10)==0x10){ 00474 simOk=1; 00475 }else{ 00476 simOk=0; 00477 } 00478 } 00479 00480 } 00481 if (rxMsg.id==RX_Settings){ 00482 if (tractorSpeed_MtS_timed==0.0f){ 00483 pickNumber = rxMsg.data[0]; // numero di becchi installati sulla ruota di semina 00484 cellsNumber = rxMsg.data[1]; // numero di celle del tamburo 00485 deepOfSeed=(rxMsg.data[2]/10000.0f); // deep of seeding 00486 seedWheelDiameter = ((rxMsg.data[4]*0xFF)+rxMsg.data[3])/10000.0f; // seed wheel diameter setting 00487 speedWheelDiameter = ((rxMsg.data[6]*0xFF)+rxMsg.data[5])/10000.0f; // variable for tractor speed calculation (need to be set from UI) ( Unit= meters ) 00488 speedWheelPulse = rxMsg.data[7]; // variable which define the number of pulse each turn of tractor speed wheel (need to be set from UI) 00489 aggiornaParametri(); 00490 #if defined(pcSerial) 00491 #if defined(canDataReceived) 00492 pc.printf("Seed wheel diameter %f \n",seedWheelDiameter); 00493 pc.printf("Speed wheel diameter %f \n",speedWheelDiameter); 00494 #endif 00495 #endif 00496 } 00497 } 00498 if (rxMsg.id==RX_AngoloPh){ 00499 if (tractorSpeed_MtS_timed==0.0f){ 00500 #if defined(M1) 00501 angoloPh = (double) rxMsg.data[0] ; 00502 aggiornaParametri(); 00503 #endif 00504 #if defined(M2) 00505 angoloPh = (double) rxMsg.data[1] ; 00506 aggiornaParametri(); 00507 #endif 00508 #if defined(M3) 00509 angoloPh = (double) rxMsg.data[2] ; 00510 aggiornaParametri(); 00511 #endif 00512 #if defined(M4) 00513 angoloPh = (double) rxMsg.data[3] ; 00514 aggiornaParametri(); 00515 #endif 00516 #if defined(M5) 00517 angoloPh = (double) rxMsg.data[4] ; 00518 aggiornaParametri(); 00519 #endif 00520 #if defined(M6) 00521 angoloPh = (double) rxMsg.data[5] ; 00522 aggiornaParametri(); 00523 #endif 00524 } 00525 } 00526 if (rxMsg.id==RX_AngoloAv){ 00527 if (tractorSpeed_MtS_timed==0.0f){ 00528 #if defined(M1) 00529 angoloAv = (double) rxMsg.data[0] ; 00530 aggiornaParametri(); 00531 #endif 00532 #if defined(M2) 00533 angoloAv = (double) rxMsg.data[1] ; 00534 aggiornaParametri(); 00535 #endif 00536 #if defined(M3) 00537 angoloAv = (double) rxMsg.data[2] ; 00538 aggiornaParametri(); 00539 #endif 00540 #if defined(M4) 00541 angoloAv = (double) rxMsg.data[3] ; 00542 aggiornaParametri(); 00543 #endif 00544 #if defined(M5) 00545 angoloAv = (double) rxMsg.data[4] ; 00546 aggiornaParametri(); 00547 #endif 00548 #if defined(M6) 00549 angoloAv = (double) rxMsg.data[5] ; 00550 aggiornaParametri(); 00551 #endif 00552 } 00553 } 00554 if (rxMsg.id==RX_Quinconce){ 00555 if (tractorSpeed_MtS_timed==0.0f){ 00556 quinconceActive = (uint8_t) rxMsg.data[0]; 00557 aggiornaParametri(); 00558 } 00559 } 00560 if (rxMsg.id==RX_QuincSinc){ 00561 masterSinc = (uint32_t) rxMsg.data[0] * 0x00FF0000; 00562 masterSinc = masterSinc + ((uint32_t) rxMsg.data[1] * 0x0000FF00); 00563 masterSinc = masterSinc + ((uint32_t) rxMsg.data[2] * 0x000000FF); 00564 masterSinc = masterSinc + ((uint32_t) rxMsg.data[3]); 00565 quincVerify=1; 00566 //pc.printf("pippo"); 00567 } 00568 } 00569 #endif 00570 #if defined(overWriteCanSimulation) 00571 enableSimula=1; 00572 speedSimula=25; 00573 avviaSimula=1; 00574 simOk=1; 00575 #endif 00576 00577 } 00578 00579 //******************************************************* 00580 void DC_CheckCurrent(){ 00581 00582 // TODO: tabella di riferimento assorbimenti alle varie velocità al fine di gestire 00583 // gli allarmi e le correzioni di velocità 00584 00585 //float SD_analogMatrix[10]={0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f}; 00586 //int SD_analogIndex[10]={0,0,0,0,0,0,0,0,0,0}; 00587 // Analog reading 00588 //number = floor(number * 100) / 100; 00589 //if (pwmCheck==1){ 00590 timeout.detach(); 00591 for (int ii=1;ii<20;ii++){ 00592 SD_analogMatrix[ii]=SD_analogMatrix[ii+1]; 00593 } 00594 SD_CurrentAnalog = floor(SDcurrent.read()*100)/100; // valore in ingresso compreso tra 0.0 e 1.0 00595 SD_analogMatrix[20]=SD_CurrentAnalog; 00596 if (SDmotorPWM==0.0f){ 00597 SD_CurrentStart=SD_CurrentAnalog; 00598 } 00599 float sommaTutto=0.0f; 00600 for (int ii=1;ii<21;ii++){ 00601 sommaTutto=sommaTutto+SD_analogMatrix[ii]; 00602 } 00603 float SD_CurrentAnalogica=sommaTutto/20.0f; 00604 SD_CurrentScaled = floor((((SD_CurrentAnalogica) - (SD_CurrentStart))*3.3f)/(SD_CurrentFactor/1000.0f)*10)/10; 00605 #if defined(pcSerial) 00606 #if defined(correnteAssorbita) 00607 pc.printf("Corrente: %f \n",SD_CurrentAnalogica); 00608 #endif 00609 #endif 00610 if (SD_CurrentAnalogica < -10.0f){ 00611 #if defined(canbusActive) 00612 int SD_AllarmeUndervoltage=1; // da implementare nel CAN 00613 #endif 00614 } 00615 if (SD_CurrentAnalogica > 10.0f){ 00616 #if defined(canbusActive) 00617 all_overCurrDC=1; 00618 #endif 00619 } 00620 //} 00621 } 00622 //******************************************************* 00623 void DC_prepare(){ 00624 // direction or brake preparation 00625 if (DC_brake==1){SDmotorInA=1;SDmotorInB=1;}else{if (DC_forward==1){SDmotorInA=1;SDmotorInB=0;}else{SDmotorInA=0;SDmotorInB=1;}} 00626 // fault reading 00627 if (SDmotorInA==1){SD_faultA=1;}else{SD_faultA=0;} 00628 if (SDmotorInB==1){SD_faultB=1;}else{SD_faultB=0;} 00629 } 00630 //******************************************************* 00631 void startDelay(){ 00632 int ritardo =0; 00633 ritardo = (int)((float)(dcActualDuty*800.0f)); 00634 timeout.attach_us(&DC_CheckCurrent,ritardo); 00635 } 00636 //******************************************************* 00637 void quinCalc(double parametro){ 00638 double riferimento=tempoTraBecchi_mS/parametro; 00639 // riceve l'impulso di sincro dal master e fa partire il timer di verifica dell'errore 00640 if ((quinconceIn==1)&&(quincStart==0)&&(oldQuincIn==0)){ 00641 #if defined(pcSerial) 00642 #if defined(Qnc) 00643 pc.printf(" quinconceIn: %d", masterSinc); 00644 #endif 00645 #endif 00646 oldQuincIn=1; 00647 quincTime.reset(); 00648 quincStart=1; 00649 } 00650 if( quinconceIn==0){ 00651 oldQuincIn=0; 00652 } 00653 // riceve l'impulso di passaggio del becco locale e determina il valore dell'errore 00654 if (quinconceActive==1){ 00655 if ((quincStart==1)&&(SDzeroDebounced==1)){ 00656 memoDuty=dcActualDuty; 00657 accelera=0; 00658 decelera=0; 00659 quincStop=0; 00660 quincStart=0; 00661 percento=0.0f; 00662 scostamento = (double)quincTime.read_ms(); 00663 quincError = 0.0f; 00664 if ((scostamento > riferimento)||(scostamento==0.0f)){ 00665 decelera=1; 00666 accelera=0; 00667 quincError = scostamento - riferimento; 00668 percento = 1.0f - (abs(quincError)/riferimento); 00669 } 00670 if ((scostamento < riferimento)&&(scostamento >0.0f)){ 00671 decelera=0; 00672 accelera=1; 00673 quincError = riferimento - scostamento; 00674 percento = 1.0f + (abs(quincError)/riferimento); 00675 } 00676 if ((decelera==1)&&(quincStop==0)){ 00677 quincStopTimer.reset(); 00678 quincStopTimer.start(); 00679 quincStop=1; 00680 propoQuinc = (speedOfSeedWheel/1.25f)*quincError; 00681 } 00682 if (abs(percento) > 1.05f){percento=1.05f;} 00683 if (abs(percento) < 0.75f){percento=0.75f;} 00684 } 00685 dcActualDuty = memoDuty * abs(percento); 00686 } 00687 if ((quinconceActive==0)&&(quincEnable==1)){ 00688 // da encoder master 00689 if (quincVerify==1){ 00690 memoDuty=dcActualDuty; 00691 quincVerify=0; 00692 accelera=0; 00693 decelera=0; 00694 percento=0.0f; 00695 quincError = 0.0f; 00696 if (masterSinc > prePosSD){ 00697 decelera=0; 00698 accelera=1; 00699 quincError = masterSinc - prePosSD; 00700 percento = 1.0f + (abs(quincError)/SDsectorStep); 00701 } 00702 if (masterSinc < prePosSD){ 00703 decelera=1; 00704 accelera=0; 00705 quincError = prePosSD -masterSinc; 00706 percento = 1.0f - (abs(quincError)/riferimento); 00707 } 00708 // se quinconce richiesto deve andare al valore di riferimento 00709 if (abs(percento) > 1.55f){percento=1.55f;} 00710 if (abs(percento) < 0.55f){percento=0.55f;} 00711 #if defined(pcSerial) 00712 #if defined(Qnc) 00713 pc.printf(" masterSinc: %d", masterSinc); 00714 pc.printf(" prePosSD: %d",prePosSD); 00715 pc.printf(" percento: %f\n",percento); 00716 #endif 00717 #endif 00718 dcActualDuty = memoDuty * abs(percento); 00719 } 00720 } 00721 } 00722 // ------------------------------------------------------------------------------------------------------------------------------------------------------------------ 00723 // MAIN SECTION 00724 // ------------------------------------------------------------------------------------------------------------------------------------------------------------------ 00725 00726 int main() 00727 { 00728 for (int a=0; a<5;a++){ 00729 mediaSpeed[a]=0; 00730 } 00731 00732 // Stepper driver init and set 00733 TBmotorRst=0; // reset stepper driver 00734 TBmotorDirecti=0; // reset stepper direction 00735 // M1 M2 M3 RESOLUTION 00736 // 0 0 0 1/2 00737 // 1 0 0 1/8 00738 // 0 1 0 1/16 00739 // 1 1 0 1/32 00740 // 0 0 1 1/64 00741 // 1 0 1 1/128 00742 // 0 1 1 1/10 00743 // 1 1 1 1/20 00744 if (TBmotorSteps==400){ 00745 TBmotor_M1=0; 00746 TBmotor_M2=0; 00747 TBmotor_M3=0; 00748 }else if (TBmotorSteps==1600){ 00749 TBmotor_M1=1; 00750 TBmotor_M2=0; 00751 TBmotor_M3=0; 00752 }else if (TBmotorSteps==3200){ 00753 TBmotor_M1=0; 00754 TBmotor_M2=1; 00755 TBmotor_M3=0; 00756 }else if (TBmotorSteps==6400){ 00757 TBmotor_M1=1; 00758 TBmotor_M2=1; 00759 TBmotor_M3=0; 00760 }else if (TBmotorSteps==12800){ 00761 TBmotor_M1=0; 00762 TBmotor_M2=0; 00763 TBmotor_M3=1; 00764 }else if (TBmotorSteps==25600){ 00765 TBmotor_M1=1; 00766 TBmotor_M2=0; 00767 TBmotor_M3=1; 00768 }else if (TBmotorSteps==2000){ 00769 TBmotor_M1=0; 00770 TBmotor_M2=1; 00771 TBmotor_M3=1; 00772 }else if (TBmotorSteps==4000){ 00773 TBmotor_M1=1; 00774 TBmotor_M2=1; 00775 TBmotor_M3=1; 00776 } 00777 TBmotorRst=1; 00778 00779 // DC reset ad set 00780 int decima = 100; 00781 wait_ms(200); 00782 SD_CurrentStart=floor(SDcurrent.read()*decima)/decima; 00783 wait_ms(2); 00784 SD_CurrentStart+=floor(SDcurrent.read()*decima)/decima; 00785 wait_ms(1); 00786 SD_CurrentStart+=floor(SDcurrent.read()*decima)/decima; 00787 wait_ms(3); 00788 SD_CurrentStart+=floor(SDcurrent.read()*decima)/decima; 00789 SD_CurrentStart=(floor((SD_CurrentStart/4.0f)*decima)/decima)-0.01f; 00790 wait_ms(100); 00791 DC_prepare(); 00792 00793 speedTimer.start(); // speed pulse timer 00794 intraPickTimer.start(); 00795 speedTimeOut.start(); 00796 speedFilter.start(); 00797 seedFilter.start(); 00798 TBfilter.start(); 00799 sincroTimer.start(); 00800 rotationTimeOut.start(); 00801 metalTimer.start(); 00802 quincTime.start(); 00803 00804 //******************************************************* 00805 // controls for check DC motor current 00806 //currentCheck.attach(&DC_CheckCurrent, 0.10f); 00807 pwmCheck.rise(&startDelay); 00808 //tbCorrection.attach(&correction,0.1f; 00809 00810 if (inProva==0){ 00811 tractorSpeedRead.rise(&tractorReadSpeed); 00812 tftUpdate.attach(&videoUpdate,0.50f); 00813 seedCorrection.attach(&seedCorrect,0.005f); // con 16 becchi a 4,5Kmh ci sono 37mS tra un becco e l'altro, quindi 8 correzioni di tb 00814 if (speedFromPick==0){ 00815 ElementPosition.rise(&sd25Fall); 00816 } 00817 }else{ 00818 tftUpdate.attach(&videoUpdate,0.125f); 00819 } 00820 00821 aggiornaParametri(); 00822 00823 SDmotorPWM.period_us(periodoSD); // frequency 1KHz pilotaggio motore DC 00824 SDmotorPWM.write(0.0f); // duty cycle = stop 00825 TBmotorDirecti=0; // tb motor direction set 00826 00827 #if defined(provaStepper) 00828 TBmotorRst=1; 00829 TBticker.attach_us(&step_TBPulseOut,3000.0f); // clock time are seconds and attach seed motor stepper controls 00830 #endif // end prova stepper 00831 00832 #if defined(canbusActive) 00833 can1.attach(&leggiCAN, CAN::RxIrq); 00834 #endif 00835 00836 //************************************************************************************************************** 00837 // MAIN LOOP 00838 //************************************************************************************************************** 00839 while (true){ 00840 00841 // ripetitore segnale di velocità 00842 if (tractorSpeedRead==0){speedClock=0;} 00843 00844 // inversione segnali ingressi 00845 TBzeroPinInput = !TBzeroPinInputRev; 00846 seedWheelZeroPinInput = !seedWheelZeroPinInputRev; 00847 00848 // se quinconce attivo ed unita' master invia segnale di sincro 00849 #if defined(speedMaster) 00850 if (seedWheelZeroPinInput==1){ 00851 quinconceOut=1; 00852 }else{ 00853 quinconceOut=0; 00854 } 00855 #endif 00856 00857 #if defined(overWriteCanSimulation) 00858 leggiCAN(); 00859 #endif 00860 00861 // simulazione velocita 00862 if (enableSimula==1){ 00863 double TMT = 0.0f; 00864 if (speedSimula > 0){ 00865 TMT = (double)(speedSimula) * 100.0f /3600.0f; 00866 pulseSpeedInterval = pulseDistance / TMT; 00867 }else{ 00868 pulseSpeedInterval = 10000.0f; 00869 } 00870 if (avviaSimula==1){ 00871 if(oldSimulaSpeed!=pulseSpeedInterval){ 00872 spedSimclock.attach_us(&speedSimulationClock,pulseSpeedInterval); 00873 oldSimulaSpeed=pulseSpeedInterval; 00874 } 00875 }else{ 00876 oldSimulaSpeed=10000.0f; 00877 spedSimclock.detach(); 00878 } 00879 }else{ 00880 spedSimclock.detach(); 00881 } 00882 00883 //******************************************************* 00884 // determina se sono in bassa velocità per il controllo di TB 00885 if (speedOfSeedWheel<=minSeedSpeed){ 00886 if (lowSpeedRequired==0){ 00887 ritardaLowSpeed.reset(); 00888 ritardaLowSpeed.start(); 00889 } 00890 lowSpeedRequired=1; 00891 }else{ 00892 if (lowSpeedRequired==1){ 00893 lowSpeedRequired=0; 00894 ritardaLowSpeed.reset(); 00895 ritardaLowSpeed.stop(); 00896 } 00897 } 00898 00899 if (ritardaLowSpeed.read_ms()> 2000){ 00900 lowSpeed=1; 00901 }else{ 00902 lowSpeed=0; 00903 } 00904 00905 00906 //************************************************************************************************************** 00907 //************************************************************************************************************** 00908 // LOGICAL CONTROLS 00909 //************************************************************************************************************** 00910 //************************************************************************************************************** 00911 00912 if (inProva==0){ 00913 if ((startCycleSimulation==0)&&(enableSimula==0)){zeroRequestBuf=1;runRequestBuf=0;enableCycle=1; 00914 }else{zeroRequestBuf=1;runRequestBuf=0;enableCycle=1;} 00915 if ((tractorSpeedRead==0)&&(speedFilter.read_ms()>3)){oldTractorSpeedRead=0;} 00916 // ---------------------------------------- 00917 // filtra il segnale dei becchi per misurare il tempo di intervallo tra loro 00918 // ---------------------------------------- 00919 if ((seedWheelZeroPinInput==0)&&(seedFilter.read_ms()>=4)){ 00920 oldSeedWheelZeroPinInput=0; 00921 SDzeroDebounced=0; 00922 } 00923 if ((seedWheelZeroPinInput==1)&&(oldSeedWheelZeroPinInput==0)){ 00924 timeIntraPick = (double)intraPickTimer.read_ms(); 00925 prePosSD=500; // preposizionamento SD 00926 intraPickTimer.reset(); 00927 rotationTimeOut.reset(); 00928 seedFilter.reset(); 00929 sincroTimer.reset(); 00930 oldSeedWheelZeroPinInput=1; 00931 SDzeroDebounced=1; 00932 SDwheelTimer.reset(); 00933 if (quincCnt<10){ 00934 quincCnt++; 00935 } 00936 if (quincCnt >3){ 00937 quincEnable=1; 00938 }else{ 00939 quincEnable=0; 00940 } 00941 if ((aspettaStart==0)&&(lowSpeed==1)){ 00942 beccoPronto=1; 00943 } 00944 SDzeroCyclePulse=1; 00945 lockStart=0; 00946 double fase1=0.0f; 00947 forzaFase=0; 00948 double limite=fixedStepGiroSD/pickNumber; 00949 if (pickCounterLow< 0xFF){ 00950 pickCounterLow++; 00951 }else{ 00952 pickCounterHig++; 00953 pickCounterLow=0; 00954 } 00955 if (tamburoStandard==0){ 00956 fase1=TBdeltaStep; 00957 }else{ 00958 if(speedForCorrection >= speedOfSeedWheel){ 00959 fase1=TBdeltaStep; 00960 }else{ 00961 //fase1=(TBdeltaStep)-(((speedOfSeedWheel-speedForCorrection)/1.25f)*(TBfaseStep)); 00962 fase1=(TBdeltaStep)-(((speedOfSeedWheel)/1.25f)*(TBfaseStep)); 00963 } 00964 if (fase1 > limite){ 00965 fase1 -= limite; // se la fase calcolata supera gli step del settore riporta il valore nell'arco precedente (es. fase 1 800, limite 750, risulta 50) 00966 //forzaFase=1; 00967 } 00968 if ((fase1 > (limite -20.0f))&&(fase1<(limite +5.0f))){ 00969 fase1 = limite -20.0f; // se la fase è molto vicina al limite interpone uno spazio minimo di lavoro del sincronismo 00970 forzaFase=1; 00971 } 00972 trigRepos=1; 00973 } 00974 fase = (uint32_t)fase1+500; 00975 #if defined(pcSerial) 00976 #if defined(inCorre) 00977 pc.printf(" limite %f", limite); 00978 pc.printf(" delta %f", TBdeltaStep); 00979 pc.printf(" faseStep %f", TBfaseStep); 00980 pc.printf(" fase %d",fase); 00981 pc.printf(" forzaFase %d",forzaFase); 00982 pc.printf(" trigRepos %d", trigRepos); 00983 pc.printf(" ActualSD: %d",SDactualPosition); 00984 pc.printf(" SpeedWheel: %f",speedOfSeedWheel); 00985 pc.printf(" SPEED: %f \n",tractorSpeed_MtS_timed); 00986 #endif 00987 #endif 00988 if (timeIntraPick >= (memoIntraPick*2)){ 00989 if ((aspettaStart==0)&&(zeroRequestBuf==0)){ 00990 if (firstStart==0){ 00991 all_pickSignal=1; 00992 } 00993 } 00994 } 00995 memoIntraPick = timeIntraPick; 00996 if (speedFromPick==1){ 00997 speedOfSeedWheel=((seedPerimeter / pickNumber)/timeIntraPick)*1000.0f; 00998 } 00999 if ((tractorSpeed_MtS_timed==0.0f)&&(zeroCycle==0)&&(zeroCycleEnd==1)){ 01000 if ((firstStart==0)&&(simOk==0)&&(enableSimula==0)&&(zeroCycleEnd==1)){ 01001 all_noSpeedSen=1; 01002 } 01003 #if defined(pcSerial) 01004 #if defined(canDataReceived) 01005 pc.printf("allarme no speed sensor"); 01006 #endif 01007 #endif 01008 } 01009 pulseRised2=1; 01010 #if defined(speedMaster) 01011 double oldLastPr = (double)oldLastPulseRead*1.2f; 01012 if((double)speedTimeOut.read_us()> (oldLastPr)){ 01013 if ((firstStart==0)&&(simOk==0)&&(enableSimula==0)&&(zeroCycleEnd==1)){ 01014 all_speedError =1; 01015 } 01016 #if defined(pcSerial) 01017 #if defined(canDataReceived) 01018 pc.printf("allarme errore speed sensor"); 01019 #endif 01020 #endif 01021 01022 } 01023 #endif 01024 //******************************************* 01025 // segue calcolo clock per la generazione della posizione teorica 01026 // la realtà in base al segnale di presenza del becco 01027 realSpeed = speedOfSeedWheel; 01028 realGiroSD = seedPerimeter / realSpeed; 01029 tempoBecco = (realGiroSD/360.0f)*16000.0f; 01030 frequenzaReale = fixedStepGiroSD/realGiroSD; 01031 semiPeriodoReale = (1000000.0f/frequenzaReale); 01032 tempoTraBecchi_mS = 0.0f; 01033 seedWheelRPM = (speedOfSeedWheel)*K_WheelRPM ; // calcola i giri al minuto della ruota di semina 7.37 31,75 01034 rapportoRuote = pickNumber/cellsNumber; // 0.67 calcola il rapporto tra il numero di becchi ed il numero di celle 0.8 1,00 01035 TBrpm = seedWheelRPM*rapportoRuote; // 5.896 31,75 01036 TBgiroStep = TBmotorSteps*TBreductionRatio; 01037 K_TBfrequency = TBgiroStep/60.0f; // 1600 * 1.65625f /60 = 44 44,00 01038 TBfrequency = (TBrpm*K_TBfrequency); // 130Hz a 0,29Mts 1397,00 a 1,25mt/s con 15 becchi e 15 celle 01039 TBperiod=1000000.0f/TBfrequency; // 715uS 01040 } 01041 // ---------------------------------------- 01042 // check SD fase 01043 if ((prePosSD >= fase)||(forzaFase==1)){//&&(prePosSD < (fase +30))){ 01044 forzaFase=0; 01045 if (trigRepos==1){ 01046 SDactualPosition=0; 01047 if ((countCicli<30)&&(trigCicli==0)){countCicli++;trigCicli=1;} 01048 if(countCicli>=cicliAspettaStart){aspettaStart=0;} 01049 if ((lowSpeed==0)&&(aspettaStart==0)&&(lockStart==0)){syncroCheck=1;beccoPronto=0;} 01050 if (trigTB==0){inhibit=1;trigSD=1;}else{inhibit=0;trigTB=0;trigSD=0;} 01051 trigRepos=0; 01052 } 01053 }else{ 01054 trigCicli=0; 01055 } 01056 /* 01057 if ((SDactualPosition >= fase) && (SDactualPosition < (fase +(uint32_t)30))){ 01058 if ((countCicli<30)&&(trigCicli==0)){countCicli++;trigCicli=1;} 01059 if(countCicli>=cicliAspettaStart){aspettaStart=0;} 01060 if ((lowSpeed==0)&&(aspettaStart==0)&&(lockStart==0)){syncroCheck=1;beccoPronto=0;} 01061 }else{ 01062 trigCicli=0; 01063 } 01064 */ 01065 // ---------------------------------------- 01066 // filtra il segnale del tamburo per lo stop in fase del tamburo stesso 01067 if (TBzeroPinInput==0){if (TBfilter.read_ms()>=5){oldTBzeroPinInput=0;}} 01068 if ((TBzeroPinInput==1)&&(oldTBzeroPinInput==0)){ 01069 oldTBzeroPinInput=1; 01070 if (loadDaCanInCorso==0){ 01071 stopCicloTB=1; 01072 startCicloTB=0; 01073 } 01074 TBfilter.reset(); 01075 TBzeroCyclePulse=1; 01076 TBactualPosition=0; 01077 if (cellsCounterLow < 0xFF){ 01078 cellsCounterLow++; 01079 }else{ 01080 cellsCounterHig++; 01081 cellsCounterLow=0; 01082 } 01083 if (loadDaCanInCorso==1){ 01084 cntCellsForLoad++; 01085 if (cntCellsForLoad >= 5){ 01086 stopCicloTB=1; 01087 cntCellsForLoad=0; 01088 } 01089 }else{ 01090 cntCellsForLoad=0; 01091 } 01092 if (trigSD==0){inhibit=1;trigTB=1;}else{inhibit=0;trigTB=0;trigSD=0;} 01093 // torna indietro per sbloccare il tamburo 01094 if ((TBmotorDirecti==1)&&(erroreTamburo==1)){ 01095 cntCellsForReload++; 01096 if (cntCellsForReload > 3){ 01097 TBmotorDirecti=0; 01098 erroreTamburo=0; 01099 } 01100 } 01101 } 01102 if ((double)TBactualPosition > ((TBgiroStep/cellsNumber)*1.2f)){ 01103 if (firstStart==0){ 01104 all_cellSignal=1; 01105 } 01106 if (erroreTamburo==0){ 01107 erroreTamburo=1; 01108 TBmotorDirecti=1; 01109 cntCellsForReload=0; 01110 cntTbError++; 01111 } 01112 #if defined(pcSerial) 01113 #if defined(canDataReceived) 01114 pc.printf("allarme error cells"); 01115 #endif 01116 #endif 01117 } 01118 if (((double)TBactualPosition > ((TBgiroStep/cellsNumber)*3.0f))||(cntTbError>2)){ 01119 if (firstStart==0){ 01120 all_noStepRota=1; 01121 } 01122 cntTbError=0; 01123 #if defined(pcSerial) 01124 #if defined(canDataReceived) 01125 pc.printf("allarme no cells sensor"); 01126 #endif 01127 #endif 01128 } 01129 // ---------------------------------------- 01130 // read and manage joystick 01131 // WARNING - ENABLE CYCLE IS SOFTWARE ALWAYS ACTIVE 01132 if (enableCycle==1){ 01133 if(runRequestBuf==1){ 01134 if (OldStartCycle!=runRequestBuf){ 01135 if((startCycle==0)&&(zeroCycleEnd==1)){ 01136 startCycle=1; 01137 OldStartCycle = runRequestBuf; 01138 oldZeroCycle=0; 01139 } 01140 } 01141 }else{ 01142 startCycle=0; 01143 pntMedia=0; 01144 } 01145 if (azzeraDaCan==1){ 01146 if (tractorSpeed_MtS_timed==0.0f){ 01147 zeroRequestBuf=1; 01148 oldZeroCycle=0; 01149 } 01150 azzeraDaCan=0; 01151 } 01152 if (loadDaCan==1){ 01153 if (tractorSpeed_MtS_timed==0.0f){ 01154 ciclaTB(); 01155 } 01156 } 01157 if ((zeroRequestBuf==1)){ 01158 if (oldZeroCycle!=zeroRequestBuf){ 01159 zeroCycle=1; 01160 zeroCycleEnd=0; 01161 SDzeroed=0; 01162 TBzeroed=0; 01163 zeroTrigger=0; 01164 oldZeroCycle = zeroRequestBuf; 01165 } 01166 } 01167 }else{ 01168 startCycle=0; 01169 zeroCycle=0; 01170 } 01171 01172 //*************************************************************************************************** 01173 // pulseRised define the event of speed wheel pulse occurs 01174 // 01175 double maxInterval = pulseDistance/minWorkSpeed; 01176 double minIntervalPulse = pulseDistance/maxWorkSpeed; 01177 if (pulseRised==1){ 01178 if (enableSpeed<10){enableSpeed++;} 01179 pulseRised=0;pulseRised1=1;speedMediaCalc(); 01180 // calcola velocità trattore 01181 if(enableSpeed>=2){ 01182 if ((pulseSpeedInterval>=0.0f)){ //minIntervalPulse)&&(pulseSpeedInterval<maxInterval)){ 01183 tractorSpeed_MtS_timed = ((pulseDistance / pulseSpeedInterval)); // tractor speed (unit= Mt/S) from pulse time interval 01184 if (checkSDrotation==0){ 01185 checkSDrotation=1; 01186 SDwheelTimer.start(); 01187 } 01188 } 01189 } 01190 if ((pulseSpeedInterval>=minIntervalPulse)&&(pulseSpeedInterval<maxInterval)){ 01191 enableSDcontrol =1; 01192 } 01193 speedTimeOut.reset(); 01194 }else{ 01195 double oldLastPr = (double)oldLastPulseRead*1.7f; 01196 if((double)speedTimeOut.read_us()> (oldLastPr)){ 01197 tractorSpeed_MtS_timed = 0.0f; 01198 enableSDcontrol =0; 01199 pntMedia=0; 01200 speedTimeOut.reset(); 01201 enableSpeed=0; 01202 quincCnt=0; 01203 } 01204 } 01205 // esegue il controllo di velocità minima 01206 if ((double)speedTimer.read_ms()>=maxInterval){tractorSpeed_MtS_timed = 0.0f; 01207 enableSDcontrol =0; 01208 enableSpeed=0; 01209 } 01210 //*************************************************************************************************************** 01211 // cycle logic control section 01212 //*************************************************************************************************************** 01213 if (enableSimula==1){if(simOk==0){tractorSpeed_MtS_timed=0.0f;}} 01214 if ((tractorSpeed_MtS_timed>0.01f)){ 01215 cycleStopRequest=1; 01216 tempoGiroSD = seedPerimeter / tractorSpeed_MtS_timed; // tempo Teorico impiegato dalla ruota di semina per fare un giro completo (a 4,5Km/h impiega 1,89 secondi) 01217 if (speedFromPick==1) { 01218 tempoTraBecchi_mS = (tempoGiroSD / pickNumber)*1000.0f; // tempo tra due impulsi dai becchi in millisecondi ( circa 157mS a 4,5Km/h) 01219 }else{ 01220 tempoTraBecchi_mS = (tempoGiroSD / 25.0f)*1000.0f; // tempo tra due impulsi dai becchi in millisecondi ( circa 157mS a 4,5Km/h) 01221 } 01222 seedWheelPeriodTeory = semiPeriodoReale; 01223 seedWheelPeriod=seedWheelPeriodTeory; 01224 if (seedWheelPeriod < 180.0f){seedWheelPeriod = 180.0f;} 01225 if ((oldSeedWheelPeriod!=seedWheelPeriod)&&(seedWheelPeriod >=180.0f )){ 01226 SDticker.attach_us(&step_SDPulseOut,seedWheelPeriod); // clock time are milliseconds and attach seed motor stepper controls 01227 oldSeedWheelPeriod=seedWheelPeriod; 01228 } 01229 //******************************************* 01230 // segue calcolo duty cycle comando motore DC per allinearsi con la velocità del trattore 01231 double dutyTeorico = 0.00; 01232 if ((tractorSpeed_MtS_timed>0.0)&&(tractorSpeed_MtS_timed<tabSpeed[0])){dutyTeorico = tabComan[0];} 01233 for (int ii = 0;ii<16;ii++){ 01234 if ((tractorSpeed_MtS_timed>=tabSpeed[ii])&&(tractorSpeed_MtS_timed<tabSpeed[ii+1])){ 01235 dutyTeorico = tabComan[ii+1]; 01236 } 01237 } 01238 if (tractorSpeed_MtS_timed > tabSpeed[16]){dutyTeorico=tractorSpeed_MtS_timed/1.25f;} 01239 if ((pulseRised1==1)&&(enableSpeed>3)&&(pulseRised2==1)){ 01240 double erroreTempo = 0.0f; 01241 if(speedFromPick==1){ 01242 erroreTempo = (double)timeIntraPick - tempoTraBecchi_mS; // errore tra il tempo previsto ed il tempo reale ( >0 se sto andando più piano del previsto) 01243 }else{ 01244 erroreTempo = (double)memoTimeHole - tempoTraBecchi_mS; // errore tra il tempo previsto ed il tempo reale ( >0 se sto andando più piano del previsto) 01245 } 01246 double errorePercentuale = erroreTempo / tempoTraBecchi_mS; 01247 double k1=errorePercentuale*0.065f;double k3=0.0f;double k4=0.0f;double k5=0.0f; 01248 if (tractorSpeed_MtS_timed <= minSeedSpeed){k3=0.105;k4=0.207;k5=0.310; 01249 }else{k3=0.1075f;k4=0.211f;k5=0.315f;} 01250 if (errorePercentuale > 25.0f){k1=errorePercentuale*k5;} 01251 if ((errorePercentuale >= 10.0f)&&(errorePercentuale<=25.0f)){k1=errorePercentuale*k4;} 01252 if (errorePercentuale < 10.0f){k1=errorePercentuale*k3;} 01253 double memoCorrezione = (dutyTeorico *k1); 01254 correzione = correzione + memoCorrezione; 01255 pulseRised1=0; 01256 pulseRised2=0; 01257 } 01258 if (correzione > (dutyTeorico*0.20)){correzione = dutyTeorico*0.20;} 01259 if((enableSDcontrol==1)){ 01260 if (correzioneAttiva==1){ 01261 dcActualDuty = dutyTeorico + correzione; 01262 } 01263 }else{ 01264 dcActualDuty = dutyTeorico; 01265 } 01266 // prova a mantenere il quinconce quando attivo 01267 #if defined(speedMaster) 01268 dcActualDuty=dcActualDuty; 01269 DC_brake=0; 01270 DC_forward=1; 01271 DC_prepare(); 01272 #else 01273 #if defined(mezzo) 01274 if (quincEnable==1){ 01275 if (quinconceActive==1){ 01276 quinCalc(2.0f); 01277 }else{ 01278 quinCalc(1.0f); 01279 } 01280 }else{ 01281 DC_brake=0; 01282 DC_forward=1; 01283 DC_prepare(); 01284 } 01285 #else 01286 if (quincEnable==1){ 01287 quinCalc(1.0f); 01288 }else{ 01289 DC_brake=0; 01290 DC_forward=1; 01291 DC_prepare(); 01292 } 01293 #endif 01294 #endif 01295 if (dcActualDuty <0.0f){dcActualDuty=0.0f;} 01296 if (dcActualDuty > 1.0f){dcActualDuty = dcMaxSpeed;} 01297 SDmotorPWM.write(dcActualDuty); 01298 // allarme 01299 if (SDwheelTimer.read_ms()>4000){ 01300 if (firstStart==0){ 01301 all_noDcRotati=1; 01302 } 01303 #if defined(pcSerial) 01304 #if defined(canDataReceived) 01305 pc.printf("allarme no DC rotation"); 01306 #endif 01307 #endif 01308 01309 } 01310 01311 //*************************************************************************************************************** 01312 // CONTROLLA TAMBURO 01313 //*************************************************************************************************************** 01314 if(lowSpeed==0){ 01315 if (syncroCheck==1){ 01316 syncroCheck=0; 01317 lockStart=1; 01318 periodo = TBperiod; 01319 if (aspettaStart==0){cambiaTB(periodo);} 01320 } 01321 // controllo di stop 01322 double memoIntraP = (double)memoIntraPick*1.8f; 01323 if ((double)rotationTimeOut.read_ms()> (memoIntraP)){ 01324 syncroCheck=0; 01325 aspettaStart=1; 01326 countCicli=0; 01327 if (TBzeroCyclePulse==1){TBticker.detach();} 01328 } 01329 }else{ // fine ciclo fuori da low speed 01330 syncroCheck=0; 01331 lockStart=0; 01332 if (beccoPronto==1){ 01333 if (tamburoStandard==1){ 01334 double ritardoMassimo = 0.0f; 01335 if(speedFromPick==1){ 01336 ritardoMassimo = (double)timeIntraPick; 01337 }else{ 01338 ritardoMassimo = (double)memoTimeHole; 01339 } 01340 int tempoDiSincro = (int)((double)(ritardoMassimo - ((tempoBecco/2.0f)+((speedOfSeedWheel/1.25f)*ritardoMassimo)))); // 01341 if (tempoDiSincro <= 1){tempoDiSincro=1;} 01342 if ((sincroTimer.read_ms()>= tempoDiSincro)){ 01343 if (tractorSpeed_MtS_timed >= minWorkSpeed){startCicloTB=1;} 01344 beccoPronto=0; 01345 } 01346 }else{ 01347 // tamburo per zucca 01348 if (speedOfSeedWheel >= minWorkSpeed){startCicloTB=1;} 01349 beccoPronto=0; 01350 } 01351 } 01352 ciclaTB(); 01353 } 01354 //************************************************************* 01355 }else{ // fine ciclo con velocita maggiore di 0 01356 SDwheelTimer.stop(); 01357 SDwheelTimer.reset(); 01358 checkSDrotation=0; 01359 oldFaseLavoro=0; 01360 aspettaStart=1; 01361 countCicli=0; 01362 startCycle=0; 01363 oldSeedWheelPeriod=0.0f; 01364 oldPeriodoTB=0.0f; 01365 correzione=0.0f; 01366 OLDpulseSpeedInterval=1000.01f; 01367 cicloTbinCorso=0; 01368 cntTbError=0; 01369 if (cycleStopRequest==1){ 01370 zeroDelay.reset(); 01371 zeroDelay.start(); 01372 runRequestBuf=0; 01373 zeroRequestBuf=1; 01374 cycleStopRequest=0; 01375 SDzeroCyclePulse=0; 01376 TBzeroCyclePulse=0; 01377 zeroCycleEnd=0; 01378 zeroCycle=1; 01379 zeroTrigger=0; 01380 } 01381 } 01382 //************************************************************************************************* 01383 // ciclo di azzeramento motori 01384 if ((zeroCycleEnd==0)&&(zeroCycle==1)){//&&(zeroDelay.read_ms()>10000)){ 01385 if (zeroTrigger==0){ 01386 TBticker.attach_us(&step_TBPulseOut,1000.0f); // clock time are seconds and attach seed motor stepper controls 01387 DC_forward=1; 01388 DC_brake=1; 01389 DC_prepare(); 01390 frenata=0; 01391 zeroTrigger=1; 01392 ritardoStop.reset(); 01393 ritardoComandoStop.reset(); 01394 ritardoComandoStop.start(); 01395 timeOutZeroSD.start(); 01396 quincTimeSet.reset(); 01397 } 01398 int tempoFrenata=300; 01399 if (((ritardoStop.read_ms()>tempoFrenata)&&(SDzeroDebounced==0))||(accensione==1)||(ritardoComandoStop.read_ms()>400)){ 01400 accensione=0; 01401 avvicinamento=1; 01402 avvicinamentoOn.reset(); 01403 avvicinamentoOn.start(); 01404 SDmotorPWM.write(0.32f); // duty cycle = 60% of power 01405 DC_forward=1; 01406 DC_brake=0; 01407 DC_prepare(); 01408 ritardoComandoStop.reset(); 01409 ritardoComandoStop.stop(); 01410 } 01411 if (avvicinamento==1){ 01412 if(avvicinamentoOn.read_ms()>300){ 01413 SDmotorPWM.write(0.7f); 01414 avvicinamentoOn.reset(); 01415 avvicinamentoOff.reset(); 01416 avvicinamentoOff.start(); 01417 } 01418 if(avvicinamentoOff.read_ms()>100){ 01419 SDmotorPWM.write(0.32f); 01420 avvicinamentoOff.reset(); 01421 avvicinamentoOff.stop(); 01422 avvicinamentoOn.start(); 01423 } 01424 }else{ 01425 avvicinamentoOn.stop(); 01426 avvicinamentoOff.stop(); 01427 avvicinamentoOn.reset(); 01428 avvicinamentoOff.reset(); 01429 } 01430 if (frenata==0){ 01431 if (SDzeroCyclePulse==1){ 01432 SDticker.detach(); 01433 frenata=1; 01434 quincTimeSet.reset(); 01435 quincTimeSet.start(); 01436 ritardoStop.start(); 01437 //ritardoComandoStop.reset(); 01438 //ritardoComandoStop.stop(); 01439 } 01440 }else{ 01441 if (quinconceActive==0){ 01442 if (SDzeroCyclePulse==1){ 01443 avvicinamento=0; 01444 DC_brake=1; 01445 DC_prepare(); 01446 SDzeroed=1; 01447 ritardoStop.reset(); 01448 ritardoStop.stop(); 01449 } 01450 }else{ 01451 if (quincTimeSet.read_ms()>500){ 01452 avvicinamento=0; 01453 DC_brake=1; 01454 DC_prepare(); 01455 SDzeroed=1; 01456 ritardoStop.reset(); 01457 ritardoStop.stop(); 01458 quincTimeSet.stop(); 01459 } 01460 } 01461 } 01462 // azzera tutto in time out 01463 if (timeOutZeroSD.read_ms()>=10000){ 01464 #if defined(pcSerial) 01465 #if defined(canDataReceived) 01466 pc.printf("allarme no zero"); 01467 #endif 01468 #endif 01469 if (firstStart==0){ 01470 all_no_Zeroing=1; 01471 } 01472 avvicinamento=0; 01473 DC_brake=1; 01474 DC_prepare(); 01475 SDzeroed=1; 01476 ritardoStop.reset(); 01477 ritardoStop.stop(); 01478 avvicinamentoOn.stop(); 01479 avvicinamentoOff.stop(); 01480 avvicinamentoOn.reset(); 01481 avvicinamentoOff.reset(); 01482 ritardoComandoStop.reset(); 01483 ritardoComandoStop.stop(); 01484 timeOutZeroSD.stop(); 01485 timeOutZeroSD.reset(); 01486 } 01487 if (TBzeroCyclePulse==1){ 01488 TBticker.detach(); 01489 TBzeroed=1; 01490 } 01491 if ((SDzeroed==1)&&(TBzeroed==1)){ 01492 avvicinamentoOn.stop(); 01493 avvicinamentoOff.stop(); 01494 ritardoComandoStop.stop(); 01495 ritardoStop.stop(); 01496 zeroCycleEnd=1; 01497 zeroCycle=0; 01498 zeroTrigger=0; 01499 runRequestBuf=1; 01500 zeroRequestBuf=0; 01501 cycleStopRequest=0; 01502 SDzeroed=0; 01503 TBzeroed=0; 01504 timeOutZeroSD.stop(); 01505 timeOutZeroSD.reset(); 01506 } 01507 } 01508 01509 //************************************************************************************************* 01510 if (enableCycle==0){ 01511 zeroTrigger=0; 01512 SDmotorPWM=0; 01513 SDmotorInA=0; 01514 SDmotorInB=0; 01515 } 01516 SDzeroCyclePulse=0; 01517 TBzeroCyclePulse=0; 01518 //************************************************************************************************* 01519 }else{//end ciclo normale 01520 //************************************************************************************************* 01521 // task di prova della scheda 01522 //************************************************************************************************* 01523 #if defined(provaScheda) 01524 clocca++; 01525 //led = !led; 01526 //txMsg.clear(); 01527 //txMsg << clocca; 01528 //test.printf("aogs \n"); 01529 //if(can1.write(txMsg)){ 01530 #if defined(pcSerial) 01531 pc.printf("Can write OK \n"); 01532 #endif 01533 //} 01534 switch (clocca){ 01535 case 1: 01536 TBmotorStepOut=1; // define step command for up down motor driver 01537 break; 01538 case 2: 01539 SDmotorPWM=1; // define step command for seeding whell motor driver 01540 break; 01541 case 3: 01542 speedClock=1; // define input of 01543 break; 01544 case 4: 01545 break; 01546 case 5: 01547 SDmotorInA=1; 01548 SDmotorInB=0; 01549 break; 01550 case 6: 01551 break; 01552 case 7: 01553 break; 01554 case 8: 01555 break; 01556 case 9: 01557 break; 01558 case 10: 01559 break; 01560 case 11: 01561 break; 01562 case 12: 01563 break; 01564 case 13: 01565 break; 01566 case 14: 01567 SDmotorPWM=1; // power mosfet 2 command out 01568 break; 01569 case 15: 01570 break; 01571 case 16: 01572 case 17: 01573 break; 01574 case 18: 01575 TBmotorStepOut=0; // define step command for up down motor driver 01576 SDmotorPWM=0; // define step command for seeding whell motor driver 01577 speedClock=0; // define input of 01578 SDmotorInA=0; 01579 SDmotorInB=0; 01580 SDmotorPWM=0; // power mosfet 2 command out 01581 break; 01582 default: 01583 clocca=0; 01584 break; 01585 } 01586 wait_ms(100); 01587 #endif // end prova scheda 01588 01589 #if defined(provaDC) 01590 int rampa=1000; 01591 int pausa=3000; 01592 switch (clocca){ 01593 case 0: 01594 DC_brake=0; 01595 DC_forward=1; 01596 duty_DC+=0.01f; 01597 if (duty_DC>=0.2f){ 01598 duty_DC=0.2f; 01599 clocca=1; 01600 } 01601 wait_ms(rampa); 01602 break; 01603 case 1: 01604 wait_ms(pausa*4); 01605 clocca=2; 01606 break; 01607 case 2: 01608 DC_brake=0; 01609 DC_forward=1; 01610 duty_DC-=0.01f; 01611 if (duty_DC<=0.0f){ 01612 duty_DC=0.0f; 01613 clocca=3; 01614 } 01615 wait_ms(rampa); 01616 break; 01617 case 3: 01618 wait_ms(pausa); 01619 clocca=4; 01620 break; 01621 case 4: 01622 DC_brake=0; 01623 DC_forward=1; 01624 duty_DC+=0.01f; 01625 if (duty_DC>=1.0f){ 01626 duty_DC=1.0f; 01627 clocca=5; 01628 } 01629 wait_ms(rampa); 01630 break; 01631 case 5: 01632 wait_ms(pausa); 01633 clocca=6; 01634 break; 01635 case 6: 01636 DC_brake=0; 01637 DC_forward=1; 01638 duty_DC-=0.01f; 01639 if (duty_DC<=0.0f){ 01640 duty_DC=0.0f; 01641 clocca=7; 01642 } 01643 wait_ms(rampa); 01644 break; 01645 case 7: 01646 wait_ms(pausa); 01647 clocca=0; 01648 break; 01649 default: 01650 break; 01651 } 01652 if (oldDuty_DC != duty_DC){ 01653 SDmotorPWM.write(duty_DC); // duty cycle = stop 01654 oldDuty_DC=duty_DC; 01655 DC_prepare(); 01656 } 01657 #endif 01658 }//end in prova 01659 } // end while 01660 }// end main 01661
Generated on Fri Jul 15 2022 20:43:43 by
1.7.2