new

Dependencies:   mbed CANMsg

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

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