Francesco Pistone
tr
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 00002 //******************************************************************************************************************** 00003 //******************************************************************************************************************** 00004 // FIRMWARE SEMINATRICE MODULA 00005 // VERSIONE PER SCHEDA DI CONTROLLO CON DRIVER INTEGRATI 00006 // V4 - ATTENZIONE - LA VERSIONE V4 HA IL DRIVER STEPPER LV8727 00007 // IL MOTORE DC E' GESTITO CON IL DRIVER VNH3SP30-E E CON LA LETTURA 00008 // DELLA CORRENTE ASSORBITA TRAMITE IL CONVERTITORE MLX91210-CAS102 CON 50A FONDOSCALA 00009 // CHE FORNISCE UNA TENSIONE DI USCITA PARI A 40mV/A 00010 // FIRST RELEASE OF BOARD DEC 2017 00011 // FIRST RELEASE OF FIRMWARE JAN 2018 00012 // 00013 // THIS RELEASE: 09 JULY 2018 00014 // 00015 // APPLICATION: MODULA CON DISTRIBUTORE ZUCCA OPPURE RISO E PUO' FUNZIONARE ANCHE CON SENSORE A 25 FORI SUL DISCO O 00016 // ENCODER MOTORE SETTANDO GLI APPOSITI FLAGS 00017 // 00018 // 29 05 2018 - INSERITO SECONDO ENCODER VIRTUALE PER LA GESTIONE DEL SINCRONISMO TRA TAMBURO E RUOTA DI SEMINA 00019 // IN PRATICA IL PRIMO ENCODER è SINCRONO CON IL SEGNALE DEI BECCHI E VIENE AZZERATO DA QUESTI, MENTRE 00020 // IL SECONDO E' INCREMENTATO IN SINCRONO CON IL PRIMO MA AZZERATO DALLA FASE. IL SUO VALORE E' POI DIVISO 00021 // PER IL RAPPORTO RUOTE E LA CORREZIONE AGISCE SULLA VELOCITA' DEL TAMBURO PER MANTENERE LA FASE DEL SECONDO 00022 // ENCODER 00023 // 05 06 2018 - INSERITO IL CONTROLLO DI GESTIONE DEL QUINCONCE SENZA ENCODER 00024 // 09 06 2018 - INSERITO CONTROLLO DI FASE CON ENCODER MASTER PER QUINCONCE - DATO SCAMBIATO IN CAN 00025 // 00026 /******************** 00027 IL FIRMWARE SI COMPONE DI 7 FILES: 00028 - main.cpp 00029 - main.hpp 00030 - iodefinition.hpp 00031 - canbus.hpp 00032 - parameters.hpp 00033 - timeandtick.hpp 00034 - variables.hpp 00035 ED UTILIZZA LE LIBRERIE STANDARD MBED PIU' UNA LIBRERIA MODIFICATA E DEDICATA PER IL CAN 00036 ********************* 00037 LA MACCHINA UTILIZZA SEMPRE 2 SOLI SENSORI; UNO PER SENTIRE LE CELLE DI CARICO SEME ED UNO PER SENTIRE I BECCHI DI SEMINA. 00038 GLI AZIONAMENTI SONO COMPOSTI DA DUE MOTORI; UN DC PER IL CONTROLLO DELLA RUOTA DI SEMINA ED UNO STEPPER PER IL CONTROLLO DEL TAMBURO 00039 UN SENSORE AGGIUNTIVO SULL'ELEMENTO MASTER RILEVA LA VELOCITA' DI AVANZAMENTO 00040 UN SENSORE AGGIUNTIVO SULLA RUOTA DI SEMINA RILEVA LA ROTAZIONE DELLA RUOTA STESSA ATTRAVERSO FORI PRESENTI SUL DISCO DI SEMINA 00041 ********************* 00042 LA LOGICA GENERALE PREVEDE CHE IL DC DELLA RUOTA DI SEMINA VENGA COMANDATO IN FUNZIONE DELLA VELOCITA' LETTA DAL SENSORE DI AVANZAMAENTO DEL MASTER 00043 IL PROBLEMA PRINCIPALE E' CHE QUANDO I BECCHI SONO INSERITI NEL TERRENO NON VI E' RETROAZIONE REALE SULLA VELOCITA' DI ROTAZIONE DELLA RUOTA STESSA 00044 PROPRIO PERCHE' L'AVANZAMANETO NEL TERRENO IMPRIME UNA VELOCITA' PROPRIA AL BECCO E QUINDI ANCHE ALLA RUOTA. 00045 PER OVVIARE A QUESTO PROBLEMA SI E' INSERITO UN CONTROLLO DI CORRENTE ASSORBITA DAL DC; SE E' BASSA DEVO ACCELERARE, SE E' ALTA DEVO RALLENTARE 00046 IL VALORE DI RIFERIMENTO DELL'ANALOGICA DI INGRESSO VIENE AGGIORNATO OGNI VOLTA CHE LA RUOTA DI SEMINA E' FERMA 00047 IL TAMBURO SEGUE LA RUOTA DI SEMINA RILEVANDONE LA VELOCITA' E RICALCOLANDO LA PROPRIA IN FUNZIONE DELLA REALE VELOCITA' DI ROTAZIONE DELLA RUOTA DI SEMINA 00048 LA FASE VIENE DETERMINATA DAL PASSAGGIO DEI BECCHI SUL SENSORE RELATIVO. 00049 IL PROBLEMA PRINCIPALE NEL MANTENERE LA FASE DEL TAMBURO E' DATO DAL FATTO CHE LA SINCRONIZZAZIONE DELLA FASE SOLO SULL'IMPULSO DEL BECCO NON E' SUFFICIENTE 00050 SOPRATUTTO QUANDO I BECCHI SONO MOLTO DISTANZIATI. 00051 PER OVVIARE A QUESTO PROBLEMA SI SONO INSERITI DUE ENCODER VIRTUALI CHE SEZIONANO LA RUOTA DI SEMINA IN 9000 PARTI. ENTRAMBI VENGONO GESTITI DA UN GENERATORE DINAMICO DI CLOCK INTERNO 00052 TARATO SULLA REALE VELOCITA' DI ROTAZIONE DELLA RUOTA DI SEMINA. 00053 IL PRIMO ENCODER VIRTUALE SI OCCUPA DI DETERMINARE LA POSIZIONE FISICA DELLA RUOTA DI SEMINA E SI AZZERA AL PASSAGGIO DI OGNI BECCO. 00054 IL SECONDO VIENE AZZERATO DALL'IMPULSO DI FASE DEL PRIMO ENCODER DETERMINATO DAI VALORI IMPOSTI SUL TERMINALE TRITECNICA 00055 IL SECONDO ENCODER VIENE CONFRONTATO CON LA POSIZIONE ASSOLUTA DEL TAMBURO (DETERMINATA DAL NUMERO DI STEP EMESSI DAL CONTROLLO), RAPPORTATA TRA CELLE E BECCHI. 00056 IL CONFRONTO DETERMINA LA POSIZIONE RELATIVA DELLA SINGOLA CELLA RISPETTO AL SINGOLO BECCO. IL MANTENIMENTO DELLA SINCRONIZZAZIONE DI FASE, DETERMINA IL SINCRO CELLA/BECCO. 00057 LA SINCRONIZZAZIONE VIENE PERO' E' A SUA VOLTA RICALCOLATA SHIFTANDO LA POSIZIONE DI AZZERAMENTO DEL SECONDO ENCODER IN FUNZIONE DELLA VELOCITA' DI ROTAZIONE GENERALE AL FINE 00058 DI CAMBIARE L'ANGOLO DI ANTICIPO DI RILASCIO DEL SEME IN FUNZIONE DELLA VELOCITA' E RECUPERARE COSI' IL TEMPO DI VOLO DEL SEME. 00059 IL TAMBURO HA DUE TIPI DI FUNZIONAMENTO: CONTINUO E AD IMPULSI. E' SELEZIONABILE IN FUNZIONE DELLA VELOCITA' E DEL TIPO DI DISTRIBUTORE MONTATO. 00060 ********************** 00061 TUTTI I VALORI, CELLE, BECCHI, IMPULSI VELOCITA', ANCGOLO DI AVVIO, FASE DI SEMINA, ECC.. SONO IMPOSTABILI DA PANNELLO OPERATORE 00062 I DATI SONO SCAMBIATI CON IL PANNELLO OPERATORE E CON GLI ALTRI MODULI ATTRAVERSO RETE CAN CON PROTOCOLLO FREESTYLE ATTRAVERSO INDIRIZZAMENTI DEDICATI 00063 AL MOMENTO NON E' POSSIBILE ATTRIBUIRE L'INIDIRIZZO BASE DELL'ELEMENTO DA TERMINALE OPERATORE MA SOLO IN FASE DI COMPILAZIONE DEL FIRMWARE. 00064 ********************** 00065 ALTRE SEZIONI RIGUARDANO LA GENERAZIONE DEGLI ALLARMI, LA COMUNICAZIONE CAN, LA SIMULAZIONE DI LAVORO, LA GESTIONE DELLA DIAGNOSI ECC.. 00066 IL MOTORE DC E' CONTROLLATO DA DIVERSE ROUTINE; LE PRIORITA' SONO (DALLA PIU' BASSA ALLA PIU' ALTA): CALCOLO TEORICO, RICALCOLO REALE, CONTROLLO DI FASE QUINCONCE, CONTROLLO DI CORRENTE. 00067 LO STEPPER SEGUE IL DC. 00068 ********************** 00069 IN FASE DI ACCENSIONE ED OGNI QUALVOLTA SI ARRIVA A VELOCITA' ZERO, LA MACCHINA ESEGUE UN CICLO DI AZZERAMENTO 00070 NON ESISTE PULSANTE DI MARCIA/STOP; E' SEMPRE ATTIVA. 00071 ********************** 00072 NEL PROGRAMMA E' PRESENTE UNA SEZIONE DI TEST FISICO DELLA SCHEDA ATTIVABILE SOLO IN FASE DI COMPILAZIONE 00073 ********************** 00074 ALTRE FUNZIONI: PRECARICAMENTO DEL TAMBURO 00075 AZZERAMENTO MANUALE 00076 STATISTICA DI SEMINA (CONTA LE CELLE) 00077 */ 00078 //******************************************************************************************************************** 00079 //******************************************************************************************************************** 00080 00081 #include "main.hpp" 00082 #include "timeandtick.hpp" 00083 #include "canbus.hpp" 00084 #include "watchdog.h" 00085 #include "iodefinition.hpp" 00086 #include "parameters.hpp" 00087 #include "variables.hpp" 00088 //******************************************************************************************************************** 00089 //******************************************************************************************************************** 00090 // ------------------------------------------------------------------------------------------------------------------------------------------------------------------ 00091 // TASK SECTION 00092 // ------------------------------------------------------------------------------------------------------------------------------------------------------------------ 00093 //************************************************************************ 00094 //************************************************************************ 00095 // rise of seed speed 25 pulse sensor 00096 void sd25Fall(){ 00097 timeHole=metalTimer.read_ms(); 00098 int memo_TimeHole= (memoTimeHole + timeHole)/ (int)2; 00099 memoTimeHole = timeHole; 00100 metalTimer.reset(); 00101 if (speedFromPick==0){ 00102 speedOfSeedWheel=((seedPerimeter/25.0f)/(double)memo_TimeHole)*1000.0f; //mtS 00103 } 00104 } 00105 // rise of seed speed motor encoder 00106 void encoRise(){ 00107 timeHole=metalTimer.read_us(); 00108 int memo_TimeHole= (memoTimeHole + timeHole)/ (int)2; 00109 memoTimeHole = timeHole; 00110 metalTimer.reset(); 00111 if (encoder==true){ 00112 speedOfSeedWheel=((seedPerimeter/((SDreductionRatio*25.5f)))/(double)memo_TimeHole)*1000000.0f; //mtS 00113 pulseRised2=1; 00114 } 00115 } 00116 // rise of seed presence sensor 00117 void seedSensorTask(){ 00118 seedSee=1; 00119 } 00120 //************************************************** 00121 // generate speed clock when speed is simulated from Tritecnica display 00122 void speedSimulationClock(){ 00123 lastPulseRead=speedTimer.read_us(); 00124 oldLastPulseRead=lastPulseRead; 00125 speedTimer.reset(); 00126 pulseRised=1; 00127 speedFilter.reset(); 00128 } 00129 //******************************************************* 00130 // interrupt task for tractor speed reading 00131 //******************************************************* 00132 void tractorReadSpeed(){ 00133 if ((oldTractorSpeedRead==0)){ 00134 lastPulseRead=speedTimer.read_us(); 00135 oldLastPulseRead=lastPulseRead; 00136 speedTimer.reset(); 00137 pulseRised=1; 00138 oldTractorSpeedRead=1; 00139 spazioCoperto+= pulseDistance; 00140 } 00141 speedFilter.reset(); 00142 speedClock=1; 00143 } 00144 //******************************************************* 00145 void speedMediaCalc(){ 00146 double lastPd=(double) lastPulseRead/1000.0f; 00147 pulseSpeedInterval = (mediaSpeed[0]+lastPd)/2.0f; 00148 if (enableSimula==1){ 00149 double TMT = (double)(speedSimula) * 100.0f /3600.0f; 00150 pulseSpeedInterval = pulseDistance / TMT; 00151 } 00152 mediaSpeed[0]=lastPd; 00153 OLDpulseSpeedInterval=pulseSpeedInterval; 00154 } 00155 00156 //******************************************************* 00157 // clocked task for manage virtual encoder of seed wheel i/o 00158 //******************************************************* 00159 //******************************************************* 00160 void step_SDPulseOut(){ 00161 SDactualPosition++; 00162 prePosSD++; 00163 #if defined(speedMaster) 00164 posForQuinc++; 00165 #endif 00166 } 00167 //******************************************************* 00168 void step_TBPulseOut(){ 00169 TBmotorStepOut=!TBmotorStepOut; 00170 if (TBmotorStepOut==0){ 00171 if (TBmotorDirecti==1){ 00172 TBactualPosition++; 00173 }else{ 00174 TBactualPosition--; 00175 } 00176 } 00177 } 00178 //******************************************************* 00179 // aggiornamento parametri di lavoro fissi e da Tritecnica 00180 void aggiornaParametri(){ 00181 speedPerimeter = Pi * speedWheelDiameter ; // perimeter of speed wheel 00182 pulseDistance = (speedPerimeter / speedWheelPulse)*1000.0f; // linear space between speed wheel pulse 00183 seedPerimeter = Pi * (seedWheelDiameter-(deepOfSeed*2.0f)); // perimeter of seed wheel 00184 intraPickDistance = seedPerimeter/pickNumber; 00185 K_WheelRPM = 60.0f/seedPerimeter; // calcola il K per i giri al minuto della ruota di semina 00186 K_WhellFrequency = (seedWheelMotorSteps*SDreductionRatio)/60.0f; // calcola il K per la frequenza di comando del motore di semina 00187 rapportoRuote = pickNumber/cellsNumber; // calcola il rapporto tra il numero di becchi ed il numero di celle 00188 K_percentuale = TBmotorSteps*TBreductionRatio; 00189 SDsectorStep = (double)fixedStepGiroSD / (double)pickNumber; 00190 TBsectorStep = (TBmotorSteps*TBreductionRatio)/cellsNumber; 00191 KcorT = (SDsectorStep/TBsectorStep);///2.0f; 00192 angoloFase=angoloPh; 00193 avvioGradi=angoloAv; 00194 stepGrado=fixedStepGiroSD/360.0f; 00195 TBdeltaStep=(fixedStepGiroSD/pickNumber)+(stepGrado*avvioGradi); 00196 TBfaseStep = (stepGrado*angoloFase); 00197 K_WheelRPM = 60.0f/seedPerimeter; // calcola il K per i giri al minuto della ruota di semina 25.4 25,40 00198 TBgiroStep = TBmotorSteps*TBreductionRatio; 00199 K_TBfrequency = TBgiroStep/60.0f; // 1600 * 1.65625f /60 = 44 44,00 00200 if (speedFromPick==1) { 00201 intraPickDistance = seedPerimeter/pickNumber; 00202 }else{ 00203 intraPickDistance = seedPerimeter/25.0f; // 25 è il numero di fori presenti nel disco di semina 00204 } 00205 } 00206 //******************************************************* 00207 void cambiaTB(double perio){ 00208 // update TB frequency 00209 double TBper=0.0f; 00210 if (aspettaStart==0){ 00211 if (perio<250.0f){perio=500.0f;} 00212 double scala =0.0f; 00213 if (lowSpeed==1){ 00214 scala =2.0f; 00215 }else{ 00216 scala =1.8f; 00217 } 00218 TBper=perio/scala; 00219 if (oldPeriodoTB!=TBper){ 00220 if (TBper >= 250.0f){ 00221 TBticker.attach_us(&step_TBPulseOut,TBper); // clock time are milliseconds and attach seed motor stepper controls 00222 }else{ 00223 TBticker.detach(); 00224 } 00225 oldPeriodoTB=TBper; 00226 } 00227 } 00228 } 00229 //******************************************************* 00230 void seedCorrect(){ 00231 /* 00232 posError determina la posizione relativa di TB rispetto ad SD 00233 la reale posizione di SD viene modificata in funzione della velocità per 00234 traslare la posizione relativa di TB. All'aumentare della velocità la posizione 00235 di SD viene incrementata così che TB acceleri per raggiungerla in modo da rilasciare il seme prima 00236 La taratura del sistema avviene determinando prima il valore di angoloFase alla minima velocità, 00237 poi, alla massima velocità, dovrebbe spostarsi la posizione relativa con una variabile proporzionale alla velocità, ma c'è un però. 00238 Il problema è che il momento di avvio determina una correzione dell'angolo di partenza del tamburo 00239 angolo che viene rideterminato ogni volta che il sensore becchi legge un transito. 00240 Di fatto c'è una concorrenza tra l'angolo di avvio determinato e la correzione di posizione relativa 00241 del tamburo. E' molto probabile che convenga modificare solo la posizione relativa e non anche l'angolo di avvio 00242 Ancora di più se viene eliminata la parte gestita da ciclata. 00243 In questo modo dovrebbe esserci solo un andamento in accelerazione di TB che viene poi eventualmente decelerato 00244 dal passaggio sul sensore di TB. Funzione corretta perchè il sincronismo tra i sensori genera l'inibizione della correzione 00245 di fase di TB. In pratica il ciclo viene resettato al passaggio sul sensore di SD che riporta a 0 la posizione di SD. 00246 Appena il sensore di TB viene impegnato allora viene abilitato il controllo di fase del tamburo. 00247 Questo si traduce nel fatto che il controllo di posizione viene gestito solo all'interno di uno slot di semina in modo che 00248 il tamburo non risenta della condizione di reset della posizione di SD mentre lui è ancora nella fase precedente. Si fermerebbe. 00249 00250 // La considerazione finale è che mantenendo l'angolo di avvio fisso e regolato sulla bassa velocità, intervenendo solo sulla correzione 00251 // di posizione in questa routine, dovrebbe essere possibile seminare correttamente a tutte le velocità regolando solo 2 parametri. 00252 */ 00253 /* 00254 SDsectorStep = (double)fixedStepGiroSD / (double)pickNumber; 00255 TBsectorStep = (TBmotorSteps*TBreductionRatio)/cellsNumber; 00256 KcorT = (SDsectorStep/TBsectorStep); 00257 angoloFase=angoloPh; 00258 stepGrado=fixedStepGiroSD/360.0f; 00259 avvioGradi = costante da terminale tritecnica 00260 TBdeltaStep=(fixedStepGiroSD/pickNumber)-(stepGrado*avvioGradi); 00261 TBfaseStep = (stepGrado*angoloFase); 00262 */ 00263 00264 if ((tractorSpeed_MtS_timed>0.01f)){ 00265 if (inhibit==0){ 00266 double posError =0.0f; 00267 double posSD=((double)SDactualPosition)/KcorT; 00268 posError = posSD - (double)TBactualPosition; 00269 // interviene sulla velocità di TB per raggiungere la corretta posizione relativa 00270 if((lowSpeed==0)&&(aspettaStart==0)){ 00271 if (posError>50.0f){posError=50.0f;} 00272 if (posError<-50.0f){posError=-50.0f;} 00273 if ((posError >=1.0f)||(posError<=-1.0f)){ 00274 ePpos = periodo /(1.0f+ ((posError/100.0f))); 00275 cambiaTB(ePpos); 00276 } 00277 } 00278 } 00279 } 00280 } 00281 //******************************************************* 00282 void videoUpdate(){ 00283 for(int aa=0;aa<4;aa++){speedForDisplay[aa]=speedForDisplay[aa+1];} 00284 speedForDisplay[4]=tractorSpeed_MtS_timed; 00285 totalSpeed=0.0f; 00286 for (int aa=0; aa<5; aa++){totalSpeed += speedForDisplay[aa];} 00287 totalSpeed = totalSpeed / 5.0f; 00288 #if defined(pcSerial) 00289 #if defined(SDreset) 00290 pc.printf("Fase: %d",fase); 00291 pc.printf(" PrePosSD: %d",prePosSD); 00292 pc.printf(" PosSD: %d",SDactualPosition); 00293 pc.printf(" speed: %f",tractorSpeed_MtS_timed); 00294 pc.printf(" Trigger: %d \n", trigRepos); 00295 #endif 00296 #endif 00297 } 00298 //******************************************************* 00299 void ciclaTB(){ 00300 if ((startCicloTB==1)&&(cicloTbinCorso==0)){ 00301 TBticker.attach_us(&step_TBPulseOut,TBperiod/2.5f); // clock time are milliseconds and attach seed motor stepper controls 00302 cicloTbinCorso = 1; 00303 startCicloTB=0; 00304 } 00305 if ((loadDaCan==1)&&(loadDaCanInCorso==0)){ 00306 TBticker.attach_us(&step_TBPulseOut,1000.0f); // clock time are milliseconds and attach seed motor stepper controls 00307 loadDaCanInCorso=1; 00308 stopCicloTB=0; 00309 } 00310 if ((stopCicloTB==1)&&(TBactualPosition>5)){ 00311 TBticker.detach(); 00312 cicloTbinCorso = 0; 00313 stopCicloTB=0; 00314 loadDaCanInCorso=0; 00315 loadDaCan=0; 00316 } 00317 } 00318 // ------------------------------------------------------------------------------------------------------------------------------------------------------------------ 00319 void stepSetting(){ 00320 // Stepper driver init and set 00321 TBmotorRst=1; // reset stepper driver 00322 if (tractorSpeed_MtS_timed==0.0f){ 00323 TBmotorDirecti=1; // reset stepper direction 00324 } 00325 // M1 M2 M3 RESOLUTION 00326 // 0 0 0 1/2 00327 // 1 0 0 1/8 00328 // 0 1 0 1/16 00329 // 1 1 0 1/32 00330 // 0 0 1 1/64 00331 // 1 0 1 1/128 00332 // 0 1 1 1/10 00333 // 1 1 1 1/20 00334 if (TBmotorSteps==400){ 00335 TBmotor_M1=1; 00336 TBmotor_M2=1; 00337 TBmotor_M3=1; 00338 }else if (TBmotorSteps==1600){ 00339 TBmotor_M1=0; 00340 TBmotor_M2=1; 00341 TBmotor_M3=1; 00342 }else if (TBmotorSteps==3200){ 00343 TBmotor_M1=1; 00344 TBmotor_M2=0; 00345 TBmotor_M3=1; 00346 }else if (TBmotorSteps==6400){ 00347 TBmotor_M1=0; 00348 TBmotor_M2=0; 00349 TBmotor_M3=1; 00350 }else if (TBmotorSteps==12800){ 00351 TBmotor_M1=1; 00352 TBmotor_M2=1; 00353 TBmotor_M3=0; 00354 }else if (TBmotorSteps==25600){ 00355 TBmotor_M1=0; 00356 TBmotor_M2=1; 00357 TBmotor_M3=0; 00358 }else if (TBmotorSteps==2000){ 00359 TBmotor_M1=1; 00360 TBmotor_M2=0; 00361 TBmotor_M3=0; 00362 }else if (TBmotorSteps==4000){ 00363 TBmotor_M1=0; 00364 TBmotor_M2=0; 00365 TBmotor_M3=0; 00366 } 00367 TBmotorRst=0; 00368 } 00369 //**************************************** 00370 void dcSetting(){ 00371 if ((speedFromPick==0)&&(encoder==false)){ 00372 DcEncoder.rise(&sd25Fall); 00373 } 00374 if (encoder==true){ 00375 DcEncoder.rise(&encoRise); 00376 //ElementPosition.fall(&encoRise); 00377 } 00378 } 00379 //******************************************************* 00380 void allarmi(){ 00381 uint8_t alarmLowRegister1=0x00; 00382 alarmLowRegister=0x00; 00383 alarmHighRegister=0x80; 00384 00385 //alarmLowRegister=alarmLowRegister+(all_semiFiniti*0x01); // manca il sensore 00386 alarmLowRegister=alarmLowRegister+(all_pickSignal*0x02); // fatto 00387 alarmLowRegister=alarmLowRegister+(all_cellSignal*0x04); // fatto 00388 alarmLowRegister=alarmLowRegister+(all_lowBattery*0x08); // fatto 00389 alarmLowRegister=alarmLowRegister+(all_overCurrDC*0x10); // fatto 00390 alarmLowRegister=alarmLowRegister+(all_stopSistem*0x20); // verificarne la necessità 00391 //alarmLowRegister=alarmLowRegister+(all_upElements*0x40); // manca il sensore 00392 if (seedSensorEnable==true){ 00393 alarmLowRegister=alarmLowRegister+(all_noSeedOnCe*0x80); // manca il sensore 00394 } 00395 00396 //alarmLowRegister1=alarmLowRegister1+(all_cfgnErrors*0x01); // da scrivere 00397 alarmLowRegister1=alarmLowRegister1+(all_noDcRotati*0x02); // fatto 00398 alarmLowRegister1=alarmLowRegister1+(all_noStepRota*0x04); // fatto 00399 alarmLowRegister1=alarmLowRegister1+(all_speedError*0x08); // fatto 00400 alarmLowRegister1=alarmLowRegister1+(all_noSpeedSen*0x10); // fatto 00401 alarmLowRegister1=alarmLowRegister1+(all_no_Zeroing*0x20); // fatto 00402 alarmLowRegister1=alarmLowRegister1+(all_genericals*0x40); 00403 if (alarmLowRegister1 > 0){ 00404 alarmHighRegister = 0x81; 00405 alarmLowRegister = alarmLowRegister1; 00406 } 00407 00408 #if defined(pcSerial) 00409 #if defined(VediAllarmi) 00410 if (all_pickSignal==1){pc.printf("AllarmeBecchi\n");} 00411 if (all_cellSignal==1){pc.printf("AllarmeCelle\n");} 00412 if (all_lowBattery==1){pc.printf("AllarmeBassaCorrente\n");} 00413 if (all_overCurrDC==1){pc.printf("AllarmeAltaCorrente\n");} 00414 if (all_stopSistem==1){pc.printf("AllarmeStop\n");} 00415 if (all_noDcRotati==1){pc.printf("AllarmeDCnoRotation\n");} 00416 if (all_noStepRota==1){pc.printf("AllarmeNoStepRotation\n");} 00417 if (all_speedError==1){pc.printf("AllarmeSpeedError\n");} 00418 if (all_noSpeedSen==1){pc.printf("AllarmeNoSpeedSensor\n");} 00419 if (all_no_Zeroing==1){pc.printf("AllarmeNoZero\n");} 00420 if (all_genericals==1){pc.printf("AllarmeGenerico\n");} 00421 pc.printf("Code: 0x%x%x\n",alarmHighRegister,alarmLowRegister); 00422 #endif 00423 #endif 00424 all_semiFiniti=0; 00425 all_pickSignal=0; 00426 all_cellSignal=0; 00427 all_lowBattery=0; 00428 all_overCurrDC=0; 00429 all_stopSistem=0; 00430 all_upElements=0; 00431 all_noSeedOnCe=0; 00432 all_cfgnErrors=0; 00433 all_noDcRotati=0; 00434 all_noStepRota=0; 00435 all_speedError=0; 00436 all_noSpeedSen=0; 00437 all_no_Zeroing=0; 00438 all_genericals=0; 00439 } 00440 //******************************************************* 00441 #if defined(speedMaster) 00442 void upDateSincro(){ 00443 char val1[8]={0,0,0,0,0,0,0,0}; 00444 val1[3]=(posForQuinc /0x00FF0000)&0x000000FF; 00445 val1[2]=(posForQuinc /0x0000FF00)&0x000000FF; 00446 val1[1]=(posForQuinc /0x000000FF)&0x000000FF; 00447 val1[0]=posForQuinc & 0x000000FF; 00448 //double pass = tractorSpeed_MtS_timed*100.0f; 00449 double pass = speedOfSeedWheel*100.0f; 00450 val1[4]=(uint8_t)(pass)&0x000000FF; 00451 val1[5]=(prePosSD /0x0000FF00)&0x000000FF; 00452 val1[6]=(prePosSD /0x000000FF)&0x000000FF; 00453 val1[7]=prePosSD & 0x000000FF; 00454 #if defined(canbusActive) 00455 #if defined(speedMaster) 00456 if(can1.write(CANMessage(TX_SI, *&val1,8))){ 00457 checkState=0; 00458 } 00459 #endif 00460 #endif 00461 } 00462 #endif 00463 //******************************************************* 00464 void upDateSpeed(){ 00465 /* 00466 aggiorna dati OPUSA3 00467 val1[0] contiene il dato di velocità 00468 val1[1] contiene il byte basso della tabella allarmi 00469 uint8_t all_semiFiniti = 0; // semi finiti (generato dal relativo sensore) 00470 uint8_t all_pickSignal = 0; // errore segnale becchi (generato dal tempo tra un becco ed il successivo) 00471 uint8_t all_cellSignal = 0; // errore segnale celle (generato dal sensore tamburo ) 00472 uint8_t all_lowBattery = 0; // allarme batteria (valore interno di tritecnica) 00473 uint8_t all_overCurrDC = 0; // sovracorrente motore DC (generato dal sensore di corrente) 00474 uint8_t all_stopSistem = 0; // sistema in stop (a tempo con ruota ferma) 00475 uint8_t all_upElements = 0; // elementi sollevati (generato dal relativo sensore) 00476 uint8_t all_noSeedOnCe = 0; // fallanza di semina (manca il seme nella cella) 00477 uint8_t all_cfgnErrors = 0; // errore di configurazione (incongruenza dei parametri impostati) 00478 uint8_t all_noDcRotati = 0; // ruota di semina bloccata (arriva la velocità ma non i becchi) 00479 uint8_t all_noStepRota = 0; // tamburo di semina bloccato (comando il tamburo ma non ricevo il sensore) 00480 uint8_t all_speedError = 0; // errore sensore velocità (tempo impulsi non costante) 00481 uint8_t all_noSpeedSen = 0; // mancanza segnale di velocità (sento i becchi ma non la velocita) 00482 uint8_t all_no_Zeroing = 0; // mancato azzeramento sistema (generato dal timeout del comando motore DC) 00483 uint8_t all_genericals = 0; // allarme generico 00484 val1[2] contiene il byte alto della tabella di allarmi 00485 val1[3] contiene i segnali per la diagnostica 00486 bit 0= sensore ruota fonica 00487 bit 1= sensore celle 00488 bit 2= sensore posizione 00489 bit 3= sensore becchi 00490 bit 4= motore DC 00491 bit 5= controller 00492 bit 6= motore stepper 00493 */ 00494 char val1[8]={0,0,0,0,0,0,0,0}; 00495 00496 val1[3]=0; 00497 val1[3]=val1[3]+(tractorSpeedRead*0x01); 00498 val1[3]=val1[3]+(TBzeroPinInput*0x02); 00499 val1[3]=val1[3]+(DcEncoder*0x04); 00500 val1[3]=val1[3]+(seedWheelZeroPinInput*0x08); 00501 #if defined(simulaPerCan) 00502 if (buttonUser==0){ 00503 val1[1]=0x02; 00504 }else{ 00505 val1[1]=0x00; 00506 } 00507 val1[3]=valore; 00508 valore+=1; 00509 if(valore>50){ 00510 valore=0; 00511 } 00512 tractorSpeed_MtS_timed=valore; 00513 #endif 00514 allarmi(); 00515 valore = totalSpeed*36.0f; // tractorSpeed_MtS_timed*36.0f; 00516 val1[0]=valore; 00517 val1[1]=alarmHighRegister; // registro alto allarmi. Parte sempre da 0x80 00518 val1[2]=alarmLowRegister; // registro basso allarmi 00519 //val1[3]=val1[3];// registro di stato degli ingressi 00520 val1[4]=resetComandi; 00521 val1[5]=cellsCounterLow; 00522 val1[6]=cellsCounterHig; 00523 #if defined(canbusActive) 00524 if(can1.write(CANMessage(TX_ID, *&val1,8))){ 00525 checkState=0; 00526 } 00527 #endif 00528 } 00529 //******************************************************* 00530 void leggiCAN(){ 00531 #if defined(canbusActive) 00532 if(can1.read(rxMsg)) { 00533 if (firstStart==1){ 00534 #if defined(speedMaster) 00535 sincUpdate.attach(&upDateSincro,0.009f); 00536 canUpdate.attach(&upDateSpeed,0.21f); 00537 #else 00538 canUpdate.attach(&upDateSpeed,0.407f); 00539 #endif 00540 firstStart=0; 00541 } 00542 00543 if (rxMsg.id==RX_ID){ 00544 #if defined(pcSerial) 00545 #if defined(canDataReceiveda) 00546 pc.printf("Messaggio ricevuto\n"); 00547 #endif 00548 #endif 00549 } 00550 if (rxMsg.id==RX_Broadcast){ 00551 #if defined(pcSerial) 00552 #if defined(canDataReceivedb) 00553 pc.printf("BroadCast ricevuto\n"); 00554 #endif 00555 #endif 00556 enableSimula= rxMsg.data[0]; 00557 speedSimula = rxMsg.data[1]; 00558 avviaSimula = rxMsg.data[2]; 00559 selezionato = rxMsg.data[3]; 00560 comandiDaCan = rxMsg.data[4]; 00561 #if defined(pcSerial) 00562 #if defined(canDataReceived) 00563 pc.printf("Speed simula %d \n",speedSimula); 00564 #endif 00565 #endif 00566 switch (comandiDaCan){ 00567 case 1: 00568 case 3: 00569 azzeraDaCan=1; 00570 resetComandi=0x01; 00571 comandiDaCan=0; 00572 break; 00573 case 2: 00574 loadDaCan=1; 00575 resetComandi=0x02; 00576 comandiDaCan=0; 00577 break; 00578 default: 00579 comandiDaCan=0; 00580 resetComandi=0xFF; 00581 break; 00582 } 00583 #if defined(pcSerial) 00584 #if defined(canDataReceivedR) 00585 pc.printf("Comandi: %x\n",resetComandi); 00586 #endif 00587 #endif 00588 if (speedSimula>45){ 00589 speedSimula=45; 00590 } 00591 // modulo 1 00592 if (RX_ID==0x100){ 00593 if ((selezionato&0x01)==0x01){ 00594 simOk=1; 00595 }else{ 00596 simOk=0; 00597 } 00598 } 00599 // modulo 2 00600 if (RX_ID==0x102){ 00601 if ((selezionato&0x02)==0x02){ 00602 simOk=1; 00603 }else{ 00604 simOk=0; 00605 } 00606 } 00607 // modulo 3 00608 if (RX_ID==0x104){ 00609 if ((selezionato&0x04)==0x04){ 00610 simOk=1; 00611 }else{ 00612 simOk=0; 00613 } 00614 } 00615 // modulo 4 00616 if (RX_ID==0x106){ 00617 if ((selezionato&0x08)==0x08){ 00618 simOk=1; 00619 }else{ 00620 simOk=0; 00621 } 00622 } 00623 // modulo 5 00624 if (RX_ID==0x108){ 00625 if ((selezionato&0x10)==0x10){ 00626 simOk=1; 00627 }else{ 00628 simOk=0; 00629 } 00630 } 00631 00632 } 00633 if (rxMsg.id==RX_Settings){ 00634 if (tractorSpeed_MtS_timed==0.0f){ 00635 pickNumber = rxMsg.data[0]; // numero di becchi installati sulla ruota di semina 00636 cellsNumber = rxMsg.data[1]; // numero di celle del tamburo 00637 deepOfSeed=(rxMsg.data[2]/10000.0f); // deep of seeding 00638 seedWheelDiameter = ((rxMsg.data[4]*0xFF)+rxMsg.data[3])/10000.0f; // seed wheel diameter setting 00639 speedWheelDiameter = ((rxMsg.data[6]*0xFF)+rxMsg.data[5])/10000.0f; // variable for tractor speed calculation (need to be set from UI) ( Unit= meters ) 00640 speedWheelPulse = rxMsg.data[7]; // variable which define the number of pulse each turn of tractor speed wheel (need to be set from UI) 00641 aggiornaParametri(); 00642 } 00643 } 00644 if (rxMsg.id==RX_AngoloPh){ 00645 if (tractorSpeed_MtS_timed==0.0f){ 00646 #if defined(M1) 00647 angoloPh = (double) rxMsg.data[0] ; 00648 aggiornaParametri(); 00649 #endif 00650 #if defined(M2) 00651 angoloPh = (double) rxMsg.data[1] ; 00652 aggiornaParametri(); 00653 #endif 00654 #if defined(M3) 00655 angoloPh = (double) rxMsg.data[2] ; 00656 aggiornaParametri(); 00657 #endif 00658 #if defined(M4) 00659 angoloPh = (double) rxMsg.data[3] ; 00660 aggiornaParametri(); 00661 #endif 00662 #if defined(M5) 00663 angoloPh = (double) rxMsg.data[4] ; 00664 aggiornaParametri(); 00665 #endif 00666 #if defined(M6) 00667 angoloPh = (double) rxMsg.data[5] ; 00668 aggiornaParametri(); 00669 #endif 00670 } 00671 } 00672 if (rxMsg.id==RX_AngoloAv){ 00673 if (tractorSpeed_MtS_timed==0.0f){ 00674 #if defined(M1) 00675 angoloAv = (double) rxMsg.data[0] ; 00676 aggiornaParametri(); 00677 #endif 00678 #if defined(M2) 00679 angoloAv = (double) rxMsg.data[1] ; 00680 aggiornaParametri(); 00681 #endif 00682 #if defined(M3) 00683 angoloAv = (double) rxMsg.data[2] ; 00684 aggiornaParametri(); 00685 #endif 00686 #if defined(M4) 00687 angoloAv = (double) rxMsg.data[3] ; 00688 aggiornaParametri(); 00689 #endif 00690 #if defined(M5) 00691 angoloAv = (double) rxMsg.data[4] ; 00692 aggiornaParametri(); 00693 #endif 00694 #if defined(M6) 00695 angoloAv = (double) rxMsg.data[5] ; 00696 aggiornaParametri(); 00697 #endif 00698 } 00699 } 00700 if (rxMsg.id==RX_Quinconce){ 00701 if (tractorSpeed_MtS_timed==0.0f){ 00702 quinconceActive = (uint8_t) rxMsg.data[0]; 00703 quincPIDminus = (uint8_t) rxMsg.data[1]; 00704 quincPIDplus = (uint8_t) rxMsg.data[2]; 00705 quincLIMminus = (uint8_t) rxMsg.data[3]; 00706 quincLIMplus = (uint8_t) rxMsg.data[4]; 00707 quincSector = (uint8_t) rxMsg.data[5]; 00708 aggiornaParametri(); 00709 } 00710 } 00711 if (rxMsg.id==RX_QuincSinc){ 00712 masterSinc = (uint32_t) rxMsg.data[3] * 0x00FF0000; 00713 masterSinc = masterSinc + ((uint32_t) rxMsg.data[2] * 0x0000FF00); 00714 masterSinc = masterSinc + ((uint32_t) rxMsg.data[1] * 0x000000FF); 00715 masterSinc = masterSinc + ((uint32_t) rxMsg.data[0]); 00716 speedFromMaster = (double)rxMsg.data[4]/100.0f; 00717 mast2_Sinc = ((uint32_t) rxMsg.data[5] * 0x0000FF00); 00718 mast2_Sinc = mast2_Sinc + ((uint32_t) rxMsg.data[6] * 0x000000FF); 00719 mast2_Sinc = mast2_Sinc + ((uint32_t) rxMsg.data[7]); 00720 canDataCheck=1; 00721 } 00722 if (rxMsg.id==RX_Configure){ 00723 uint8_t flags = rxMsg.data[0]; 00724 uint16_t steps = (uint32_t) rxMsg.data[1]*0xFF00; 00725 steps = steps + ((uint32_t)rxMsg.data[2]); 00726 stepSetting(); 00727 cellsCountSet = rxMsg.data[3]; 00728 if ((flags&0x01)==0x01){ 00729 if (encoder==false){ 00730 encoder=true; 00731 DcEncoder.rise(NULL); 00732 dcSetting(); 00733 } 00734 }else{ 00735 if (encoder==true){ 00736 encoder=false; 00737 DcEncoder.rise(NULL); 00738 dcSetting(); 00739 } 00740 } 00741 if ((flags&0x02)==0x02){ 00742 tankLevelEnable=true; 00743 }else{ 00744 tankLevelEnable=false; 00745 } 00746 if ((flags&0x04)==0x04){ 00747 seedSensorEnable=true; 00748 }else{ 00749 seedSensorEnable=false; 00750 } 00751 if ((flags&0x08)==0x08){ 00752 stendiNylonEnable=true; 00753 }else{ 00754 stendiNylonEnable=false; 00755 } 00756 if ((flags&0x10)==0x10){ 00757 canDataCheckEnable=true; 00758 }else{ 00759 canDataCheckEnable=false; 00760 } 00761 if ((flags&0x20)==0x20){ 00762 tamburoStandard=1; 00763 }else{ 00764 tamburoStandard=0; 00765 } 00766 if ((flags&0x40)==0x40){ 00767 currentCheckEnable=true; 00768 }else{ 00769 currentCheckEnable=false; 00770 } 00771 } 00772 } 00773 #endif 00774 #if defined(overWriteCanSimulation) 00775 enableSimula=1; 00776 speedSimula=25; 00777 avviaSimula=1; 00778 simOk=1; 00779 #endif 00780 00781 } 00782 00783 //******************************************************* 00784 void DC_CheckCurrent(){ 00785 00786 // TODO: tabella di riferimento assorbimenti alle varie velocità al fine di gestire 00787 // gli allarmi e le correzioni di velocità 00788 00789 //float SD_analogMatrix[10]={0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f}; 00790 //int SD_analogIndex[10]={0,0,0,0,0,0,0,0,0,0}; 00791 // Analog reading 00792 //number = floor(number * 100) / 100; 00793 //if (pwmCheck==1){ 00794 timeout.detach(); 00795 for (int ii=1;ii<20;ii++){ 00796 SD_analogMatrix[ii]=SD_analogMatrix[ii+1]; 00797 } 00798 SD_CurrentAnalog = floor(SDcurrent.read()*100)/100; // valore in ingresso compreso tra 0.00 e 1.00 00799 SD_analogMatrix[20]=SD_CurrentAnalog; 00800 if (SDmotorPWM==0.0f){ 00801 SD_CurrentStart=SD_CurrentAnalog; 00802 } 00803 float sommaTutto=0.0f; 00804 for (int ii=1;ii<21;ii++){ 00805 sommaTutto=sommaTutto+SD_analogMatrix[ii]; 00806 } 00807 float SD_CurrentAnalogica=sommaTutto/20.0f; 00808 SD_CurrentScaled = floor(( (SD_CurrentAnalogica - SD_CurrentStart)*3.3f) / (SD_CurrentFactor/1000.0f)*10)/10; 00809 #if defined(pcSerial) 00810 #if defined(correnteAssorbita) 00811 pc.printf("CorrenteStart: %f ",SD_CurrentStart); 00812 pc.printf(" CorrenteScaled: %f ",SD_CurrentScaled); 00813 pc.printf(" CorrenteMedia: %f \n",SD_CurrentAnalogica); 00814 #endif 00815 #endif 00816 reduceCurrent=false; 00817 incrementCurrent=false; 00818 /* 00819 if (SD_CurrentScaled < 3.0f){ 00820 #if defined(canbusActive) 00821 all_lowBattery=1; 00822 #endif 00823 incrementCurrent=true; 00824 } 00825 */ 00826 if (SD_CurrentScaled > 10.0f){ 00827 timeCurr.start(); 00828 if (timeCurr.read() > 5.0f){ 00829 #if defined(canbusActive) 00830 all_overCurrDC=1; 00831 #endif 00832 reduceCurrent=true; 00833 timeCurr.reset(); 00834 } 00835 }else{ 00836 timeCurr.stop(); 00837 } 00838 //} 00839 } 00840 //******************************************************* 00841 void DC_prepare(){ 00842 // direction or brake preparation 00843 if (DC_brake==1){ 00844 SDmotorInA=1; 00845 SDmotorInB=1; 00846 }else{ 00847 if (DC_forward==0){ 00848 SDmotorInA=1; 00849 SDmotorInB=0; 00850 }else{ 00851 SDmotorInA=0; 00852 SDmotorInB=1; 00853 } 00854 } 00855 // fault reading 00856 if (SDmotorInA==1){SD_faultA=1;}else{SD_faultA=0;} 00857 if (SDmotorInB==1){SD_faultB=1;}else{SD_faultB=0;} 00858 } 00859 //******************************************************* 00860 void startDelay(){ 00861 int ritardo =0; 00862 ritardo = (int)((float)(dcActualDuty*800.0f)); 00863 timeout.attach_us(&DC_CheckCurrent,ritardo); 00864 } 00865 //******************************************************* 00866 void quincTrigon(){ 00867 quincClock=true; 00868 } 00869 void quincTrigof(){ 00870 quincClock=false; 00871 } 00872 //******************************************************* 00873 void quinCalc(){ 00874 // riceve l'impulso di sincro dal master e fa partire il timer di verifica dell'errore 00875 #if !defined(mezzo) 00876 if ((quincClock==true)&&(oldQuincIn==0)){ 00877 oldQuincIn=1; 00878 if (quincStart==0){ 00879 oldQuincTimeSD = (double) quincTimeSD.read_ms(); 00880 quincTime.reset(); 00881 quincTimeSD.reset(); 00882 quincStart=1; 00883 } 00884 } 00885 if(quincClock==false){ 00886 oldQuincIn=0; 00887 } 00888 #else 00889 if ((((quinconceActive==0)&&(quincClock==true))||((quinconceActive==1)&&(quincClock==false)))&&(oldQuincIn==0)){ 00890 oldQuincIn=1; 00891 if (quincStart==0){ 00892 oldQuincTimeSD = (double) quincTimeSD.read_ms(); 00893 quincTime.reset(); 00894 quincStart=1; 00895 } 00896 } 00897 if (quinconceActive==0){ 00898 if (quincClock==false){ 00899 oldQuincIn=0; 00900 } 00901 }else{ 00902 if (quincClock==true){ 00903 oldQuincIn=0; 00904 } 00905 } 00906 #endif 00907 //**************************************************************************************** 00908 if (quincCnt>=4){ 00909 if (countPicks==0){ 00910 if ((sincroQui==1)&&(quincStart==0)){ 00911 // decelera 00912 countPicks=1; 00913 } 00914 if ((sincroQui==0)&&(quincStart==1)){ 00915 // accelera 00916 countPicks=2; 00917 } 00918 } 00919 if ((sincroQui==1)&&(quincStart==1)){ 00920 if (countPicks==1){ //decelera 00921 scostamento = oldQuincTimeSD; 00922 if (scostamento < (tempoBecchiPerQuinc*0.75f)){ 00923 double scostPerc = (scostamento/tempoBecchiPerQuinc); 00924 percento -= ((double)quincPIDminus/100.0f)*(scostPerc); 00925 #if defined(pcSerial) 00926 #if defined(laq) 00927 pc.printf("RALL scos2: %f tbpq: %f percento: %f \n",scostamento,tempoBecchiPerQuinc,percento); 00928 #endif 00929 #endif 00930 } 00931 //if (scostamento <15.0f){percento=0.0f;} 00932 } 00933 if (countPicks==2){ //accelera 00934 scostamento = (double)quincTime.read_ms(); 00935 if (scostamento < (tempoBecchiPerQuinc*0.75f)){ 00936 double scostPerc = (scostamento/tempoBecchiPerQuinc); 00937 percento += ((double)quincPIDplus/100.0f)*(scostPerc); 00938 #if defined(pcSerial) 00939 #if defined(laq) 00940 pc.printf( 00941 "ACCE scos1: %f tbpq: %f percento: %f \n",scostamento,tempoBecchiPerQuinc,percento); 00942 #endif 00943 #endif 00944 } 00945 //if (scostamento <15.0f){percento=0.0f;} 00946 } 00947 sincroQui=0; 00948 quincStart=0; 00949 countPicks=0; 00950 #if !defined(speedMaster) 00951 if (quincCnt>=3){ 00952 if (speedFromMaster>0.0f){ 00953 if (enableSimula==0){ 00954 tractorSpeed_MtS_timed = speedFromMaster + percento; 00955 } 00956 } 00957 } 00958 #endif 00959 } 00960 00961 //******************************************************************* 00962 if (canDataCheckEnable==true){ 00963 if (canDataCheck==1){ // sincro da comunicazione can del valore di posizione del tamburo master 00964 canDataCheck=0; 00965 double parametro = SDsectorStep/3.0f; 00966 double differenza=0.0f; 00967 #if defined(mezzo) 00968 if (quinconceActive==1){ 00969 differenza = (double)masterSinc - (double)prePosSD; 00970 }else{ 00971 differenza = (double)mast2_Sinc - (double)prePosSD; 00972 } 00973 #else 00974 differenza = (double)mast2_Sinc - (double)prePosSD; 00975 #endif 00976 if ((differenza > 0.0f)&&(differenza < parametro)){ 00977 double diffPerc = differenza / parametro; 00978 percento += ((double)quincPIDplus/100.0f)*abs(diffPerc); 00979 #if defined(pcSerial) 00980 #if defined(quinca) 00981 pc.printf("m1 %d m2 %d prePo %d diffe %f perce %f parm %f %\n",masterSinc, mast2_Sinc,prePosSD,differenza,percento, parametro); 00982 #endif 00983 #endif 00984 } 00985 if ((differenza < 0.0f)&&(abs(differenza) < parametro)){ 00986 double diffPerc = (double)differenza / parametro; 00987 percento -= ((double)quincPIDminus/100.0f)*abs(diffPerc); 00988 #if defined(pcSerial) 00989 #if defined(quinca) 00990 pc.printf("m1 %d m2 %d prePo %d diffe %f perce %f parm %f %\n",masterSinc, mast2_Sinc,prePosSD,differenza,percento, parametro); 00991 #endif 00992 #endif 00993 } 00994 00995 #if !defined(speedMaster) 00996 if (quincCnt>=3){ 00997 if (speedFromMaster>0.0f){ 00998 if (enableSimula==0){ 00999 tractorSpeed_MtS_timed = speedFromMaster + percento; 01000 } 01001 } 01002 } 01003 #endif 01004 } 01005 } 01006 01007 } 01008 if ((percento) > ((double) quincLIMplus/100.0f)){ 01009 percento= (double)quincLIMplus/100.0f; 01010 } 01011 if ((percento) < (((double)quincLIMminus*-1.0f)/100.0f)){ 01012 percento=((double)quincLIMminus*-1.0f)/100.0f; 01013 } 01014 } 01015 // ---------------------------------------- 01016 #if defined(seedSensor) 01017 void resetDelay(){ 01018 delaySeedCheck.reset(); 01019 delaySeedCheck.stop(); 01020 } 01021 #endif 01022 // ------------------------------------------------------------------------------------------------------------------------------------------------------------------ 01023 // MAIN SECTION 01024 // --------------------------------------------------------------------------------------------------------------------------------------------------------------- 01025 01026 int main() 01027 { 01028 //wait(1.0f); 01029 wd.Configure(2); //watchdog set at xx seconds 01030 01031 stepSetting(); 01032 01033 for (int a=0; a<5;a++){ 01034 mediaSpeed[a]=0; 01035 } 01036 01037 01038 // DC reset ad set 01039 int decima = 100; 01040 wait_ms(200); 01041 SD_CurrentStart=floor(SDcurrent.read()*decima)/decima; 01042 wait_ms(2); 01043 SD_CurrentStart+=floor(SDcurrent.read()*decima)/decima; 01044 wait_ms(1); 01045 SD_CurrentStart+=floor(SDcurrent.read()*decima)/decima; 01046 wait_ms(3); 01047 SD_CurrentStart+=floor(SDcurrent.read()*decima)/decima; 01048 SD_CurrentStart=(floor((SD_CurrentStart/4.0f)*decima)/decima)-0.01f; 01049 wait_ms(100); 01050 DC_prepare(); 01051 01052 speedTimer.start(); // speed pulse timer 01053 intraPickTimer.start(); 01054 speedTimeOut.start(); 01055 speedFilter.start(); 01056 seedFilter.start(); 01057 TBfilter.start(); 01058 sincroTimer.start(); 01059 rotationTimeOut.start(); 01060 metalTimer.start(); 01061 quincTime.start(); 01062 quincTimeSD.start(); 01063 //intraEncoTimer.start(); 01064 01065 //******************************************************* 01066 // controls for check DC motor current 01067 pwmCheck.rise(&startDelay); 01068 01069 if (inProva==0){ 01070 tractorSpeedRead.rise(&tractorReadSpeed); 01071 #if !defined(speedMaster) 01072 quinconceIn.rise(&quincTrigon); 01073 quinconceIn.fall(&quincTrigof); 01074 #endif 01075 #if defined(speedMaster) 01076 tftUpdate.attach(&videoUpdate,0.50f); 01077 #endif 01078 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 01079 dcSetting(); 01080 #if defined(seedSensor) 01081 seedCheck.fall(&seedSensorTask); 01082 #endif 01083 }else{ 01084 tftUpdate.attach(&videoUpdate,0.125f); 01085 } 01086 01087 aggiornaParametri(); 01088 01089 SDmotorPWM.period_us(periodoSD); // frequency 1KHz pilotaggio motore DC 01090 SDmotorPWM.write(1.0f); // duty cycle = stop 01091 TBmotorDirecti=1; // tb motor direction set 01092 01093 #if defined(provaStepper) 01094 TBmotorRst=0; 01095 TBticker.attach_us(&step_TBPulseOut,1000.0f); // clock time are seconds and attach seed motor stepper controls 01096 #endif // end prova stepper 01097 01098 #if defined(canbusActive) 01099 can1.attach(&leggiCAN, CAN::RxIrq); 01100 #endif 01101 01102 //************************************************************************************************************** 01103 // MAIN LOOP 01104 //************************************************************************************************************** 01105 while (true){ 01106 // ripetitore segnale di velocità 01107 if (tractorSpeedRead==0){speedClock=0;} 01108 01109 // inversione segnali ingressi 01110 #if !defined(simulaBanco) 01111 seedWheelZeroPinInput = !seedWheelZeroPinInputRev; 01112 #else 01113 if ((prePosSD-500) >= SDsectorStep){ 01114 seedWheelZeroPinInput=1; 01115 } 01116 if ((prePosSD > 500)&&(prePosSD<580)){ 01117 seedWheelZeroPinInput=0; 01118 } 01119 #endif 01120 TBzeroPinInput = !TBzeroPinInputRev; 01121 01122 // se quinconce attivo ed unita' master invia segnale di sincro 01123 #if defined(speedMaster) 01124 if (seedWheelZeroPinInput==1){ 01125 quinconceOut=0; 01126 } 01127 if (((double)(prePosSD-500) >= (SDsectorStep/((double)quincSector)))&&(quinconceOut=1)){ 01128 quinconceOut=1; 01129 } 01130 if (quinconceActive==1){ 01131 if ((quinconceOut==1)&&(oldQuinconceOut==1)){ 01132 posForQuinc=500; 01133 oldQuinconceOut=0; 01134 } 01135 if (((double)posForQuinc-500.0f)> (SDsectorStep-200)){ 01136 oldQuinconceOut=1; 01137 } 01138 } 01139 #endif 01140 01141 #if defined(overWriteCanSimulation) 01142 leggiCAN(); 01143 #endif 01144 01145 // simulazione velocita 01146 if (enableSimula==1){ 01147 double TMT = 0.0f; 01148 if (speedSimula > 0){ 01149 TMT = (double)(speedSimula) * 100.0f /3600.0f; 01150 pulseSpeedInterval = pulseDistance / TMT; 01151 }else{ 01152 pulseSpeedInterval = 10000.0f; 01153 } 01154 if (avviaSimula==1){ 01155 if(oldSimulaSpeed!=pulseSpeedInterval){ 01156 #if defined(pcSerial) 01157 #if defined(canDataReceived) 01158 pc.printf("Pulseinterval %f \n",pulseSpeedInterval); 01159 #endif 01160 #endif 01161 spedSimclock.attach_us(&speedSimulationClock,pulseSpeedInterval); 01162 oldSimulaSpeed=pulseSpeedInterval; 01163 } 01164 }else{ 01165 oldSimulaSpeed=10000.0f; 01166 spedSimclock.detach(); 01167 } 01168 }else{ 01169 spedSimclock.detach(); 01170 } 01171 01172 //******************************************************* 01173 // determina se sono in bassa velocità per il controllo di TB 01174 if (speedOfSeedWheel<=minSeedSpeed){ 01175 if (lowSpeedRequired==0){ 01176 ritardaLowSpeed.reset(); 01177 ritardaLowSpeed.start(); 01178 } 01179 lowSpeedRequired=1; 01180 }else{ 01181 if (lowSpeedRequired==1){ 01182 lowSpeedRequired=0; 01183 ritardaLowSpeed.reset(); 01184 ritardaLowSpeed.stop(); 01185 } 01186 } 01187 01188 if (ritardaLowSpeed.read_ms()> 2000){ 01189 lowSpeed=1; 01190 }else{ 01191 lowSpeed=0; 01192 } 01193 01194 01195 //************************************************************************************************************** 01196 //************************************************************************************************************** 01197 // LOGICAL CONTROLS 01198 //************************************************************************************************************** 01199 //************************************************************************************************************** 01200 01201 if (inProva==0){ 01202 if ((startCycleSimulation==0)&&(enableSimula==0)){ 01203 zeroRequestBuf=1; 01204 runRequestBuf=0; 01205 enableCycle=1; 01206 }else{ 01207 zeroRequestBuf=1; 01208 runRequestBuf=0; 01209 enableCycle=1; 01210 } 01211 if ((tractorSpeedRead==0)&&(speedFilter.read_ms()>3)){ 01212 oldTractorSpeedRead=0; 01213 } 01214 // ---------------------------------------- 01215 // filtra il segnale dei becchi per misurare il tempo di intervallo tra loro 01216 // ---------------------------------------- 01217 if ((seedWheelZeroPinInput==0)&&(oldSeedWheelZeroPinInput==1)){ 01218 if(seedFilter.read_ms()>=4){ 01219 oldSeedWheelZeroPinInput=0; 01220 SDzeroDebounced=0; 01221 } 01222 } 01223 if ((seedWheelZeroPinInput==1)&&(oldSeedWheelZeroPinInput==0)){ 01224 timeIntraPick = (double)intraPickTimer.read_ms(); 01225 prePosSD=500; // preposizionamento SD 01226 intraPickTimer.reset(); 01227 rotationTimeOut.reset(); 01228 seedFilter.reset(); 01229 sincroTimer.reset(); 01230 oldSeedWheelZeroPinInput=1; 01231 quincTime.reset(); 01232 quincTimeSD.reset(); 01233 SDzeroDebounced=1; 01234 sincroQui=1; 01235 SDwheelTimer.reset(); 01236 #if defined(speedMaster) 01237 if (quinconceActive==0){ 01238 posForQuinc=500; 01239 } 01240 #endif 01241 if (quincCnt<10){ 01242 quincCnt++; 01243 } 01244 if ((aspettaStart==0)&&(lowSpeed==1)){ 01245 beccoPronto=1; 01246 } 01247 SDzeroCyclePulse=1; 01248 lockStart=0; 01249 double fase1=0.0f; 01250 forzaFase=0; 01251 double limite=fixedStepGiroSD/pickNumber; 01252 if (tamburoStandard==0){ 01253 fase1=TBdeltaStep; 01254 }else{ 01255 if(speedForCorrection >= speedOfSeedWheel){ 01256 fase1=TBdeltaStep; 01257 }else{ 01258 //fase1=(TBdeltaStep)-(((speedOfSeedWheel-speedForCorrection)/maxWorkSpeed)*(TBfaseStep)); 01259 fase1=(TBdeltaStep)-(((speedOfSeedWheel)/maxWorkSpeed)*(TBfaseStep)); 01260 } 01261 if (fase1 > limite){ 01262 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) 01263 //forzaFase=1; 01264 } 01265 if ((fase1 > (limite -20.0f))&&(fase1<(limite +5.0f))){ 01266 fase1 = limite -20.0f; // se la fase è molto vicina al limite interpone uno spazio minimo di lavoro del sincronismo 01267 forzaFase=1; 01268 } 01269 trigRepos=1; 01270 } 01271 fase = (uint32_t)fase1+500; 01272 #if defined(pcSerial) 01273 #if defined(inCorre) 01274 pc.printf(" limite %f", limite); 01275 pc.printf(" delta %f", TBdeltaStep); 01276 pc.printf(" faseStep %f", TBfaseStep); 01277 pc.printf(" fase %d",fase); 01278 pc.printf(" forzaFase %d",forzaFase); 01279 pc.printf(" trigRepos %d", trigRepos); 01280 pc.printf(" ActualSD: %d",SDactualPosition); 01281 pc.printf(" SpeedWheel: %f",speedOfSeedWheel); 01282 pc.printf(" SPEED: %f \n",tractorSpeed_MtS_timed); 01283 #endif 01284 #endif 01285 if (timeIntraPick >= (memoIntraPick*2)){ 01286 if ((aspettaStart==0)&&(zeroRequestBuf==0)){ 01287 if (firstStart==0){ 01288 all_pickSignal=1; 01289 } 01290 } 01291 } 01292 memoIntraPick = timeIntraPick; 01293 if ((speedFromPick==1)&&(encoder==false)){ 01294 speedOfSeedWheel=((seedPerimeter / pickNumber)/timeIntraPick)*1000.0f; 01295 #if defined(pcSerial) 01296 #if defined(Qnca) 01297 pc.printf("perim: %f pickN: %f sped: %f\n", seedPerimeter, pickNumber,speedOfSeedWheel); 01298 #endif 01299 #endif 01300 } 01301 if (encoder==false){ 01302 pulseRised2=1; 01303 } 01304 #if defined(speedMaster) 01305 if ((tractorSpeed_MtS_timed==0.0f)&&(zeroCycle==0)&&(zeroCycleEnd==1)){ 01306 if ((firstStart==0)&&(simOk==0)&&(enableSimula==0)&&(zeroCycleEnd==1)){ 01307 all_noSpeedSen=1; 01308 } 01309 } 01310 double oldLastPr = (double)oldLastPulseRead*1.5f; 01311 if((double)speedTimeOut.read_us()> (oldLastPr)){ 01312 if ((firstStart==0)&&(simOk==0)&&(enableSimula==0)&&(zeroCycleEnd==1)){ 01313 all_speedError =1; 01314 } 01315 } 01316 #endif 01317 //******************************************* 01318 // esegue calcolo clock per la generazione della posizione teorica 01319 // la realtà in base al segnale di presenza del becco 01320 realSpeed = speedOfSeedWheel; 01321 realGiroSD = seedPerimeter / realSpeed; 01322 tempoBecco = (realGiroSD/360.0f)*16000.0f; 01323 frequenzaReale = fixedStepGiroSD/realGiroSD; 01324 semiPeriodoReale = (1000000.0f/frequenzaReale); 01325 tempoTraBecchi_mS = 0.0f; 01326 seedWheelRPM = (speedOfSeedWheel)*K_WheelRPM ; // calcola i giri al minuto della ruota di semina 7.37 31,75 01327 TBrpm = seedWheelRPM*rapportoRuote; // 5.896 31,75 01328 TBfrequency = (TBrpm*K_TBfrequency); // 130Hz a 0,29Mts 1397,00 a 1,25mt/s con 15 becchi e 15 celle 01329 TBperiod=1000000.0f/TBfrequency; // 715uS 01330 } 01331 // ---------------------------------------- 01332 // check SD fase 01333 if ((prePosSD >= fase)||(forzaFase==1)){//&&(prePosSD < (fase +30))){ 01334 forzaFase=0; 01335 if (trigRepos==1){ 01336 SDactualPosition=0; 01337 if ((countCicli<30)&&(trigCicli==0)){countCicli++;trigCicli=1;} 01338 if(countCicli>=cicliAspettaStart){aspettaStart=0;} 01339 if ((lowSpeed==0)&&(aspettaStart==0)&&(lockStart==0)){syncroCheck=1;beccoPronto=0;} 01340 if (trigTB==0){ 01341 inhibit=1; 01342 trigSD=1; 01343 }else{ 01344 inhibit=0; 01345 trigTB=0; 01346 trigSD=0; 01347 } 01348 trigRepos=0; 01349 } 01350 }else{ 01351 trigCicli=0; 01352 } 01353 // ---------------------------------------- 01354 // filtra il segnale del tamburo per lo stop in fase del tamburo stesso 01355 if (TBzeroPinInput==0){if (TBfilter.read_ms()>=5){oldTBzeroPinInput=0;}} 01356 if ((TBzeroPinInput==1)&&(oldTBzeroPinInput==0)){ 01357 oldTBzeroPinInput=1; 01358 if (loadDaCanInCorso==0){ 01359 stopCicloTB=1; 01360 startCicloTB=0; 01361 } 01362 TBfilter.reset(); 01363 TBzeroCyclePulse=1; 01364 TBactualPosition=0; 01365 if (cntTbError>0){ 01366 cntCellsCorrect++; 01367 } 01368 if (cntCellsCorrect>3){ 01369 cntTbError=0; 01370 cntCellsCorrect=0; 01371 } 01372 // conteggio celle erogate 01373 if (cellsCounterLow < 0xFF){ 01374 cellsCounterLow++; 01375 }else{ 01376 cellsCounterHig++; 01377 cellsCounterLow=0; 01378 } 01379 // ciclo conteggio celle per carico manuale 01380 if (loadDaCanInCorso==1){ 01381 cntCellsForLoad++; 01382 if (cntCellsForLoad >= 5){ 01383 stopCicloTB=1; 01384 cntCellsForLoad=0; 01385 } 01386 }else{ 01387 cntCellsForLoad=0; 01388 } 01389 // inibizione controllo di sincro per fuori fase 01390 if (trigSD==0){ 01391 inhibit=1; 01392 trigTB=1; 01393 }else{ 01394 inhibit=0; 01395 trigTB=0; 01396 trigSD=0; 01397 } 01398 // conta le celle indietro per sbloccare il tamburo 01399 if ((TBmotorDirecti==0)&&(erroreTamburo==1)){ 01400 cntCellsForReload++; 01401 if (cntCellsForReload >= cellsCountSet){ 01402 TBmotorDirecti=1; // rotazione normale 01403 erroreTamburo=0; 01404 cntCellsCorrect=0; 01405 } 01406 } 01407 #if defined(seedSensor) 01408 resetDelay(); 01409 delaySeedCheck.start(); 01410 #endif 01411 } 01412 if ((double)TBactualPosition > ((TBgiroStep/cellsNumber)*1.2f)&&(erroreTamburo==0)){ 01413 if (firstStart==0){ 01414 if (cntTbError>2){ 01415 all_cellSignal=1; 01416 #if defined(seedSensor) 01417 resetDelay(); 01418 #endif 01419 } 01420 } 01421 if (erroreTamburo==0){ 01422 erroreTamburo=1; 01423 TBmotorDirecti=0; // rotazione inversa 01424 cntCellsForReload=0; 01425 cntTbError++; 01426 #if defined(seedSensor) 01427 resetDelay(); 01428 #endif 01429 } 01430 } 01431 if (((double)TBactualPosition > ((TBgiroStep/cellsNumber)*3.0f))||(cntTbError>4)){ 01432 if (firstStart==0){ 01433 all_noStepRota=1; 01434 #if defined(seedSensor) 01435 resetDelay(); 01436 #endif 01437 } 01438 cntTbError=0; 01439 } 01440 // ---------------------------------------- 01441 // read and manage joystick 01442 // WARNING - ENABLE CYCLE IS SOFTWARE ALWAYS ACTIVE 01443 if (enableCycle==1){ 01444 if(runRequestBuf==1){ 01445 if (OldStartCycle!=runRequestBuf){ 01446 if((startCycle==0)&&(zeroCycleEnd==1)){ 01447 startCycle=1; 01448 OldStartCycle = runRequestBuf; 01449 oldZeroCycle=0; 01450 } 01451 } 01452 }else{ 01453 startCycle=0; 01454 pntMedia=0; 01455 } 01456 if (azzeraDaCan==1){ 01457 if (tractorSpeed_MtS_timed==0.0f){ 01458 zeroRequestBuf=1; 01459 oldZeroCycle=0; 01460 } 01461 azzeraDaCan=0; 01462 } 01463 if (loadDaCan==1){ 01464 if (tractorSpeed_MtS_timed==0.0f){ 01465 ciclaTB(); 01466 } 01467 } 01468 if ((zeroRequestBuf==1)){ 01469 if (oldZeroCycle!=zeroRequestBuf){ 01470 zeroCycle=1; 01471 zeroCycleEnd=0; 01472 SDzeroed=0; 01473 TBzeroed=0; 01474 zeroTrigger=0; 01475 oldZeroCycle = zeroRequestBuf; 01476 } 01477 } 01478 }else{ 01479 startCycle=0; 01480 zeroCycle=0; 01481 } 01482 01483 //*************************************************************************************************** 01484 // pulseRised define the event of speed wheel pulse occurs 01485 // 01486 double maxInterval = pulseDistance/minWorkSpeed; 01487 double minIntervalPulse = pulseDistance/maxWorkSpeed; 01488 if (pulseRised==1){ 01489 if (enableSpeed<10){enableSpeed++;} 01490 pulseRised=0; 01491 pulseRised1=1; 01492 speedMediaCalc(); 01493 // calcola velocità trattore 01494 if(enableSpeed>=2){ 01495 if ((pulseSpeedInterval>=0.0f)){ //minIntervalPulse)&&(pulseSpeedInterval<maxInterval)){ 01496 if((quincCnt<3)||(speedFromMaster==0.0f)||(enableSimula==1)){ 01497 tractorSpeed_MtS_timed = ((pulseDistance / pulseSpeedInterval)); // tractor speed (unit= Mt/S) from pulse time interval 01498 } 01499 /*#if !defined(speedMaster) 01500 if (quincCnt>=5){ 01501 if (speedFromMaster>0.0f){ 01502 tractorSpeed_MtS_timed = speedFromMaster + percento; 01503 } 01504 } 01505 #endif*/ 01506 if (checkSDrotation==0){ 01507 checkSDrotation=1; 01508 SDwheelTimer.start(); 01509 } 01510 } 01511 } 01512 speedTimeOut.reset(); 01513 }else{ 01514 double oldLastPr = (double)oldLastPulseRead*1.7f; 01515 if((double)speedTimeOut.read_us()> (oldLastPr)){ 01516 tractorSpeed_MtS_timed = 0.0f; 01517 #if defined(seedSensor) 01518 resetDelay(); 01519 #endif 01520 pntMedia=0; 01521 speedTimeOut.reset(); 01522 enableSpeed=0; 01523 quincCnt=0; 01524 } 01525 } 01526 01527 #if defined(seedSensor) 01528 if (seedSensorEnable==true){ 01529 if (delaySeedCheck.read_ms()>100){ 01530 if (seedSee==0){ 01531 all_noSeedOnCe=1; 01532 } 01533 resetDelay(); 01534 } 01535 } 01536 #endif 01537 // esegue il controllo di velocità minima 01538 /*if ((double)speedTimer.read_ms()>=maxInterval){ 01539 tractorSpeed_MtS_timed = 0.0f; 01540 enableSpeed=0; 01541 }*/ 01542 // esegue il controllo di velocità massima 01543 /*if ((double)speedTimer.read_ms()<=minIntervalPulse){ 01544 tractorSpeed_MtS_timed = 4.5f; 01545 }*/ 01546 //*************************************************************************************************************** 01547 // cycle logic control section 01548 //*************************************************************************************************************** 01549 if (enableSimula==1){if(simOk==0){tractorSpeed_MtS_timed=0.0f;}} 01550 if ((tractorSpeed_MtS_timed>0.01f)){ 01551 cycleStopRequest=1; 01552 // calcola il tempo teorico di passaggio becchi sulla base della velocità del trattore 01553 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) 01554 if (encoder==false){ 01555 if (speedFromPick==1) { 01556 tempoTraBecchi_mS = (tempoGiroSD / pickNumber)*1000.0f; // tempo tra due impulsi dai becchi in millisecondi ( circa 157mS a 4,5Km/h) 01557 }else{ 01558 tempoTraBecchi_mS = (tempoGiroSD / 25.0f)*1000.0f; // tempo tra due impulsi dai becchi in millisecondi ( circa 157mS a 4,5Km/h) 01559 } 01560 }else{ 01561 tempoTraBecchi_mS = (tempoGiroSD / (SDreductionRatio*25.5f))*1000.0f; // tempo tra due impulsi dai becchi in millisecondi ( circa 157mS a 4,5Km/h) 01562 double pippo=0.0f; 01563 #if !defined(speedMaster) 01564 pippo = seedPerimeter / speedFromMaster; 01565 #endif 01566 tempoBecchiPerQuinc = (pippo / pickNumber)*1000.0f; 01567 } 01568 //******************************************* 01569 // segue calcolo duty cycle comando motore DC per allinearsi con la velocità del trattore 01570 double dutyTeorico = 0.00; 01571 if ((tractorSpeed_MtS_timed>0.0)&&(tractorSpeed_MtS_timed<tabSpeed[0])){dutyTeorico = tabComan[0];} 01572 for (int ii = 0;ii<16;ii++){ 01573 if ((tractorSpeed_MtS_timed>=tabSpeed[ii])&&(tractorSpeed_MtS_timed<tabSpeed[ii+1])){ 01574 dutyTeorico = tabComan[ii+1]; 01575 } 01576 } 01577 if (tractorSpeed_MtS_timed > tabSpeed[16]){dutyTeorico=tractorSpeed_MtS_timed/maxWorkSpeed;} 01578 #if !defined(speedMaster) 01579 quinCalc(); 01580 #endif 01581 if ((enableSpeed>3)&&(pulseRised2==1)&&(quincCnt>=2)){ 01582 double erroreTempo = 0.0f; 01583 if(encoder==false){ 01584 if(speedFromPick==1){ 01585 erroreTempo = (double)timeIntraPick - tempoTraBecchi_mS; 01586 }else{ 01587 erroreTempo = (double)memoTimeHole - tempoTraBecchi_mS; // errore tra il tempo previsto ed il tempo reale ( >0 se sto andando più piano del previsto) 01588 } 01589 }else{ 01590 erroreTempo = ((double)memoTimeHole/1000.0f) - tempoTraBecchi_mS; // errore tra il tempo previsto ed il tempo reale ( >0 se sto andando più piano del previsto) 01591 } 01592 double errorePercentuale = erroreTempo / tempoTraBecchi_mS; 01593 double k3=0.0f; 01594 double k4=0.0f; 01595 double k5=0.0f; 01596 double k6=0.0f; 01597 /*if (tractorSpeed_MtS_timed <= minSeedSpeed){ 01598 k3=1.030f; 01599 k4=5.103f; 01600 k5=10.00f; 01601 k6=20.50f; 01602 }else{*/ 01603 #if defined(speedMaster) 01604 k3=0.010f; 01605 #else 01606 k3=0.050f; 01607 #endif 01608 k4=1.103f; 01609 k5=10.00f; 01610 k6=20.50f; 01611 //} 01612 double L1 = 0.045f; 01613 double L_1=-0.045f; 01614 double L2 = 0.150f; 01615 double L_2=-0.150f; 01616 double L3 = 0.301f; 01617 double L_3=-0.301f; 01618 double k1=0.0f; 01619 if ((errorePercentuale > L3)||(errorePercentuale < L_3)){ 01620 k1=errorePercentuale*k6; 01621 } 01622 if (((errorePercentuale >= L2)&&(errorePercentuale<=L3))||((errorePercentuale <= L_2)&&(errorePercentuale>=L_3))){ 01623 k1=errorePercentuale*k5; 01624 } 01625 if (((errorePercentuale < L2)&&(errorePercentuale>L1))||((errorePercentuale > L_2)&&(errorePercentuale<L_1))){ 01626 k1=errorePercentuale*k4; 01627 } 01628 if ((errorePercentuale < L1)||(errorePercentuale > L_1)){ 01629 k1=errorePercentuale*k3; 01630 } 01631 double memoCorrezione = k1; 01632 if (quincCnt >= 2){ 01633 correzione = correzione + memoCorrezione; 01634 if (correzione > (1.0f - dutyTeorico)){correzione = (1.0f - dutyTeorico);} 01635 if ((correzione < 0.0f)&&(dutyTeorico+correzione<0.0f)){correzione = -1.0f*dutyTeorico;} 01636 } 01637 pulseRised1=0; 01638 pulseRised2=0; 01639 #if defined(pcSerial) 01640 #if defined(Qnca) 01641 pc.printf("ErTem: %f K1: %f Corr: %f MemoCorr:%f DutyTeo: %f \n",erroreTempo, k1,correzione, memoCorrezione, dutyTeorico); 01642 pc.printf("TsP: %f SpW: %f InPic: %f TBec: %f EPerc: %f Duty:%f \n",tractorSpeed_MtS_timed,speedOfSeedWheel,timeIntraPick, tempoTraBecchi_mS,errorePercentuale, dcActualDuty); 01643 #endif 01644 #endif 01645 #if defined(pcSerial) 01646 #if defined(Qncb) 01647 pc.printf("TsP: %f SpW: %f InPic: %f EPerc: %f Duty:%f \n",tractorSpeed_MtS_timed,speedOfSeedWheel,timeIntraPick, errorePercentuale, dcActualDuty); 01648 #endif 01649 #endif 01650 } 01651 // introduce il controllo di corrente 01652 if (currentCheckEnable==true){ 01653 if (incrementCurrent){ 01654 boostDcOut +=0.005f; 01655 } 01656 if (reduceCurrent){ 01657 boostDcOut -=0.005f; 01658 } 01659 if (boostDcOut >= 0.2f){ 01660 boostDcOut=0.2f; 01661 all_genericals=1; 01662 } 01663 if (boostDcOut <=-0.2f){ 01664 boostDcOut=-0.2f; 01665 all_genericals=1; 01666 } 01667 correzione += boostDcOut; 01668 } 01669 DC_brake=0; 01670 DC_forward=1; 01671 DC_prepare(); 01672 01673 // il semiperiodoreale è calcolato sulla lettura del passaggio becchi reale 01674 seedWheelPeriod = semiPeriodoReale; 01675 if (seedWheelPeriod < 180.0f){seedWheelPeriod = 180.0f;} 01676 if ((oldSeedWheelPeriod!=seedWheelPeriod)&&(seedWheelPeriod >=180.0f )){ 01677 SDticker.attach_us(&step_SDPulseOut,seedWheelPeriod); // clock time are milliseconds and attach seed motor stepper controls 01678 oldSeedWheelPeriod=seedWheelPeriod; 01679 } 01680 01681 if((quincCnt>=3)){ 01682 if (correzioneAttiva==1){ 01683 dcActualDuty = dutyTeorico + correzione; 01684 }else{ 01685 dcActualDuty = dutyTeorico; 01686 } 01687 }else{ 01688 dcActualDuty = dutyTeorico; 01689 } 01690 if (dcActualDuty <=0.0f){dcActualDuty=0.05f;} 01691 if (dcActualDuty > 0.95f){dcActualDuty = 0.95f;}//dcMaxSpeed;} 01692 if (olddcActualDuty!=dcActualDuty){ 01693 SDmotorPWM.write(1.0f-dcActualDuty); 01694 olddcActualDuty=dcActualDuty; 01695 } 01696 // allarme 01697 if (SDwheelTimer.read_ms()>4000){ 01698 if (firstStart==0){ 01699 all_noDcRotati=1; 01700 } 01701 #if defined(pcSerial) 01702 #if defined(VediAllarmi) 01703 pc.printf("allarme no DC rotation"); 01704 #endif 01705 #endif 01706 01707 } 01708 01709 //*************************************************************************************************************** 01710 // CONTROLLA TAMBURO 01711 //*************************************************************************************************************** 01712 if(lowSpeed==0){ 01713 if (syncroCheck==1){ 01714 syncroCheck=0; 01715 lockStart=1; 01716 periodo = TBperiod; 01717 if (aspettaStart==0){cambiaTB(periodo);} 01718 } 01719 // controllo di stop 01720 double memoIntraP = (double)memoIntraPick*1.8f; 01721 if ((double)rotationTimeOut.read_ms()> (memoIntraP)){ 01722 syncroCheck=0; 01723 aspettaStart=1; 01724 countCicli=0; 01725 if (TBzeroCyclePulse==1){TBticker.detach();} 01726 } 01727 }else{ // fine ciclo fuori da low speed 01728 syncroCheck=0; 01729 lockStart=0; 01730 if (beccoPronto==1){ 01731 if (tamburoStandard==1){ 01732 double ritardoMassimo = 0.0f; 01733 if (encoder==false){ 01734 if(speedFromPick==1){ 01735 ritardoMassimo = (double)timeIntraPick; 01736 }else{ 01737 ritardoMassimo = (double)memoTimeHole; 01738 } 01739 }else{ 01740 ritardoMassimo = (double)timeIntraPick; 01741 } 01742 int tempoDiSincro = (int)((double)(ritardoMassimo - ((tempoBecco/2.0f)+((speedOfSeedWheel/maxWorkSpeed)*ritardoMassimo)))); // 01743 if (tempoDiSincro <= 1){tempoDiSincro=1;} 01744 if ((sincroTimer.read_ms()>= tempoDiSincro)){ 01745 if (tractorSpeed_MtS_timed >= minWorkSpeed){startCicloTB=1;} 01746 beccoPronto=0; 01747 } 01748 }else{ 01749 // tamburo per zucca 01750 if (speedOfSeedWheel >= minWorkSpeed){startCicloTB=1;} 01751 beccoPronto=0; 01752 } 01753 } 01754 ciclaTB(); 01755 } 01756 //************************************************************* 01757 }else{ // fine ciclo con velocita maggiore di 0 01758 SDwheelTimer.stop(); 01759 SDwheelTimer.reset(); 01760 #if defined(seedSensor) 01761 resetDelay(); 01762 #endif 01763 checkSDrotation=0; 01764 oldFaseLavoro=0; 01765 aspettaStart=1; 01766 countCicli=0; 01767 startCycle=0; 01768 oldSeedWheelPeriod=0.0f; 01769 oldPeriodoTB=0.0f; 01770 correzione=0.0f; 01771 OLDpulseSpeedInterval=1000.01f; 01772 cicloTbinCorso=0; 01773 cntTbError=0; 01774 if (cycleStopRequest==1){ 01775 zeroDelay.reset(); 01776 zeroDelay.start(); 01777 runRequestBuf=0; 01778 zeroRequestBuf=1; 01779 cycleStopRequest=0; 01780 SDzeroCyclePulse=0; 01781 TBzeroCyclePulse=0; 01782 zeroCycleEnd=0; 01783 zeroCycle=1; 01784 zeroTrigger=0; 01785 noSDzeroRequest=1; 01786 } 01787 } 01788 //************************************************************************************************* 01789 // ciclo di azzeramento motori 01790 if ((zeroCycleEnd==0)&&(zeroCycle==1)){//&&(zeroDelay.read_ms()>10000)){ 01791 if (zeroTrigger==0){ 01792 TBticker.attach_us(&step_TBPulseOut,1000.0f); // clock time are seconds and attach seed motor stepper controls 01793 DC_forward=1; 01794 DC_brake=1; 01795 DC_prepare(); 01796 frenata=0; 01797 zeroTrigger=1; 01798 ritardoStop.reset(); 01799 ritardoComandoStop.reset(); 01800 ritardoComandoStop.start(); 01801 timeOutZeroSD.start(); 01802 quincTimeSet.reset(); 01803 } 01804 int tempoFrenata=300; 01805 if (((ritardoStop.read_ms()>tempoFrenata)&&(SDzeroDebounced==0))||(accensione==1)||(ritardoComandoStop.read_ms()>400)){ 01806 accensione=0; 01807 avvicinamento=1; 01808 avvicinamentoOn.reset(); 01809 avvicinamentoOn.start(); 01810 SDmotorPWM.write(1.0f-0.32f); // duty cycle = 60% of power 01811 DC_forward=1; 01812 DC_brake=0; 01813 DC_prepare(); 01814 ritardoComandoStop.reset(); 01815 ritardoComandoStop.stop(); 01816 } 01817 if (avvicinamento==1){ 01818 if(avvicinamentoOn.read_ms()>300){ 01819 SDmotorPWM.write(1.0f-0.7f); 01820 avvicinamentoOn.reset(); 01821 avvicinamentoOff.reset(); 01822 avvicinamentoOff.start(); 01823 } 01824 if(avvicinamentoOff.read_ms()>100){ 01825 SDmotorPWM.write(1.0f-0.32f); 01826 avvicinamentoOff.reset(); 01827 avvicinamentoOff.stop(); 01828 avvicinamentoOn.start(); 01829 } 01830 }else{ 01831 avvicinamentoOn.stop(); 01832 avvicinamentoOff.stop(); 01833 avvicinamentoOn.reset(); 01834 avvicinamentoOff.reset(); 01835 } 01836 if (frenata==0){ 01837 if (SDzeroCyclePulse==1){ 01838 SDticker.detach(); 01839 frenata=1; 01840 quincTimeSet.reset(); 01841 quincTimeSet.start(); 01842 ritardoStop.start(); 01843 //ritardoComandoStop.reset(); 01844 //ritardoComandoStop.stop(); 01845 } 01846 }else{ 01847 #if defined(mezzo) 01848 if (quinconceActive==0){ 01849 if (SDzeroCyclePulse==1){ 01850 avvicinamento=0; 01851 DC_brake=1; 01852 DC_prepare(); 01853 SDzeroed=1; 01854 ritardoStop.reset(); 01855 ritardoStop.stop(); 01856 } 01857 }else{ 01858 if (quincTimeSet.read_ms()>700){ 01859 avvicinamento=0; 01860 DC_brake=1; 01861 DC_prepare(); 01862 SDzeroed=1; 01863 ritardoStop.reset(); 01864 ritardoStop.stop(); 01865 quincTimeSet.stop(); 01866 } 01867 } 01868 #else 01869 if (SDzeroCyclePulse==1){ 01870 avvicinamento=0; 01871 DC_brake=1; 01872 DC_prepare(); 01873 SDzeroed=1; 01874 ritardoStop.reset(); 01875 ritardoStop.stop(); 01876 } 01877 #endif 01878 } 01879 // azzera tutto in time out 01880 if ((timeOutZeroSD.read_ms()>=10000)||(noSDzeroRequest==1)){ 01881 if ((firstStart==0)&&(noSDzeroRequest==0)){ 01882 all_no_Zeroing=1; 01883 } 01884 avvicinamento=0; 01885 DC_brake=1; 01886 DC_prepare(); 01887 SDzeroed=1; 01888 ritardoStop.reset(); 01889 ritardoStop.stop(); 01890 avvicinamentoOn.stop(); 01891 avvicinamentoOff.stop(); 01892 avvicinamentoOn.reset(); 01893 avvicinamentoOff.reset(); 01894 ritardoComandoStop.reset(); 01895 ritardoComandoStop.stop(); 01896 timeOutZeroSD.stop(); 01897 timeOutZeroSD.reset(); 01898 noSDzeroRequest=0; 01899 } 01900 if (TBzeroCyclePulse==1){ 01901 TBticker.detach(); 01902 TBzeroed=1; 01903 } 01904 if ((SDzeroed==1)&&(TBzeroed==1)){ 01905 avvicinamentoOn.stop(); 01906 avvicinamentoOff.stop(); 01907 ritardoComandoStop.stop(); 01908 ritardoStop.stop(); 01909 zeroCycleEnd=1; 01910 zeroCycle=0; 01911 zeroTrigger=0; 01912 runRequestBuf=1; 01913 zeroRequestBuf=0; 01914 cycleStopRequest=0; 01915 SDzeroed=0; 01916 TBzeroed=0; 01917 timeOutZeroSD.stop(); 01918 timeOutZeroSD.reset(); 01919 } 01920 } 01921 01922 //************************************************************************************************* 01923 if (enableCycle==0){ 01924 zeroTrigger=0; 01925 SDmotorPWM=0; 01926 SDmotorInA=0; 01927 SDmotorInB=0; 01928 } 01929 SDzeroCyclePulse=0; 01930 TBzeroCyclePulse=0; 01931 //************************************************************************************************* 01932 }else{//end ciclo normale 01933 //************************************************************************************************* 01934 // task di prova della scheda 01935 //************************************************************************************************* 01936 #if defined(provaScheda) 01937 clocca++; 01938 //led = !led; 01939 //txMsg.clear(); 01940 //txMsg << clocca; 01941 //test.printf("aogs \n"); 01942 //if(can1.write(txMsg)){ 01943 #if defined(pcSerial) 01944 pc.printf("Can write OK \n"); 01945 #endif 01946 //} 01947 switch (clocca){ 01948 case 1: 01949 TBmotorStepOut=1; // define step command for up down motor driver 01950 break; 01951 case 2: 01952 SDmotorPWM=1; // define step command for seeding whell motor driver 01953 break; 01954 case 3: 01955 speedClock=1; // define input of 01956 break; 01957 case 4: 01958 break; 01959 case 5: 01960 SDmotorInA=1; 01961 SDmotorInB=0; 01962 break; 01963 case 6: 01964 break; 01965 case 7: 01966 break; 01967 case 8: 01968 break; 01969 case 9: 01970 break; 01971 case 10: 01972 break; 01973 case 11: 01974 break; 01975 case 12: 01976 break; 01977 case 13: 01978 break; 01979 case 14: 01980 SDmotorPWM=1; // power mosfet 2 command out 01981 break; 01982 case 15: 01983 break; 01984 case 16: 01985 case 17: 01986 break; 01987 case 18: 01988 TBmotorStepOut=0; // define step command for up down motor driver 01989 SDmotorPWM=0; // define step command for seeding whell motor driver 01990 speedClock=0; // define input of 01991 SDmotorInA=0; 01992 SDmotorInB=0; 01993 SDmotorPWM=0; // power mosfet 2 command out 01994 break; 01995 default: 01996 clocca=0; 01997 break; 01998 } 01999 wait_ms(100); 02000 #endif // end prova scheda 02001 02002 #if defined(provaDC) 02003 int rampa=1000; 02004 int pausa=3000; 02005 switch (clocca){ 02006 case 0: 02007 DC_brake=0; 02008 DC_forward=1; 02009 duty_DC+=0.01f; 02010 if (duty_DC>=1.0f){ 02011 duty_DC=1.0f; 02012 clocca=1; 02013 } 02014 wait_ms(rampa); 02015 break; 02016 case 1: 02017 wait_ms(pausa*4); 02018 clocca=2; 02019 break; 02020 case 2: 02021 DC_brake=0; 02022 DC_forward=1; 02023 duty_DC-=0.01f; 02024 if (duty_DC<=0.0f){ 02025 duty_DC=0.0f; 02026 clocca=3; 02027 } 02028 wait_ms(rampa); 02029 break; 02030 case 3: 02031 wait_ms(pausa); 02032 clocca=4; 02033 break; 02034 case 4: 02035 DC_brake=0; 02036 DC_forward=1; 02037 duty_DC+=0.01f; 02038 if (duty_DC>=1.0f){ 02039 duty_DC=1.0f; 02040 clocca=5; 02041 } 02042 wait_ms(rampa); 02043 break; 02044 case 5: 02045 wait_ms(pausa); 02046 clocca=6; 02047 break; 02048 case 6: 02049 DC_brake=0; 02050 DC_forward=1; 02051 duty_DC-=0.01f; 02052 if (duty_DC<=0.0f){ 02053 duty_DC=0.0f; 02054 clocca=7; 02055 } 02056 wait_ms(rampa); 02057 break; 02058 case 7: 02059 wait_ms(pausa); 02060 clocca=0; 02061 break; 02062 default: 02063 break; 02064 } 02065 if (oldDuty_DC != duty_DC){ 02066 SDmotorPWM.write(1.0f-duty_DC); // duty cycle = stop 02067 oldDuty_DC=duty_DC; 02068 DC_prepare(); 02069 } 02070 #endif 02071 }//end in prova 02072 wd.Service(); // kick the dog before the timeout 02073 } // end while 02074 }// end main 02075
Generated on Wed Jul 13 2022 15:21:40 by
1.7.2