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