new_2019

Dependencies:   mbed CANMsg

main.cpp

Committer:
francescopistone
Date:
20 months ago
Branch:
BOX_ARGENTO_2019
Revision:
4:8c64ea251890
Parent:
3:36da019e6bb6

File content as of revision 4:8c64ea251890:

//********************************************************************************************************************
//********************************************************************************************************************
// FIRMWARE SEMINATRICE MODULA
// VERSIONE PER SCHEDA DI CONTROLLO CON DRIVER INTEGRATI
// V4 - ATTENZIONE - LA VERSIONE V4 HA IL DRIVER STEPPER LV8727
// IL MOTORE DC E' GESTITO CON IL DRIVER VNH3SP30-E E CON LA LETTURA
// DELLA CORRENTE ASSORBITA TRAMITE IL CONVERTITORE MLX91210-CAS102 CON 50A FONDOSCALA
// CHE FORNISCE UNA TENSIONE DI USCITA PARI A 40mV/A
// FIRST RELEASE OF BOARD DEC 2017
// FIRST RELEASE OF FIRMWARE JAN 2018
//
// THIS RELEASE: 29 MAJ 2018
//
// APPLICATION: MODULA CON DISTRIBUTORE ZUCCA OPPURE RISO E PUO' FUNZIONARE ANCHE CON SENSORE A 25 FORI SUL DISCO O 
//              ENCODER MOTORE SETTANDO GLI APPOSITI FLAGS
//
// 29 05 2018 - INSERITO SECONDO ENCODER VIRTUALE PER LA GESTIONE DEL SINCRONISMO TRA TAMBURO E RUOTA DI SEMINA
//              IN PRATICA IL PRIMO ENCODER è SINCRONO CON IL SEGNALE DEI BECCHI E VIENE AZZERATO DA QUESTI, MENTRE
//              IL SECONDO E' INCREMENTATO IN SINCRONO CON IL PRIMO MA AZZERATO DALLA FASE. IL SUO VALORE E' POI DIVISO
//              PER IL RAPPORTO RUOTE E LA CORREZIONE AGISCE SULLA VELOCITA' DEL TAMBURO PER MANTENERE LA FASE DEL SECONDO
//              ENCODER
// 05 06 2018 - INSERITO IL CONTROLLO DI GESTIONE DEL QUINCONCE SENZA ENCODER
//
//********************************************************************************************************************
//********************************************************************************************************************

#include "main.hpp"
#include "timeandtick.hpp"
#include "canbus.hpp"
#include "iodefinition.hpp"
#include "parameters.hpp"
#include "variables.hpp"
//********************************************************************************************************************
//********************************************************************************************************************
// ------------------------------------------------------------------------------------------------------------------------------------------------------------------
// TASK SECTION
// ------------------------------------------------------------------------------------------------------------------------------------------------------------------
//************************************************************************
// rise of seed speed 25 pulse sensor
void sd25Fall(){
    timeHole=metalTimer.read_ms();
    int memo_TimeHole= (memoTimeHole + timeHole)/ (int)2;
    memoTimeHole = timeHole;
    metalTimer.reset();
    if (speedFromPick==0){
        speedOfSeedWheel=((seedPerimeter/25.0f)/(double)memo_TimeHole)*1000.0f;    //mtS
    }
}
//**************************************************
// generate speed clock when speed is simulated from Tritecnica display
void speedSimulationClock(){        
    lastPulseRead=speedTimer.read_us();
    oldLastPulseRead=lastPulseRead;
    speedTimer.reset();
    pulseRised=1;
}
//*******************************************************
//    interrupt task for tractor speed reading
//*******************************************************
void tractorReadSpeed(){
    if ((oldTractorSpeedRead==0)){
        lastPulseRead=speedTimer.read_us();
        oldLastPulseRead=lastPulseRead;
        speedTimer.reset();
        pulseRised=1;
        oldTractorSpeedRead=1;
    }
    speedFilter.reset();
    speedClock=1;
}
//*******************************************************
void speedMediaCalc(){
    double lastPd=(double) lastPulseRead/1000.0f;
    pulseSpeedInterval = (mediaSpeed[0]+lastPd)/2.0f;
    if (enableSimula==1){
        double TMT = (double)(speedSimula) * 100.0f /3600.0f;
        pulseSpeedInterval = pulseDistance / TMT;
    }    
    mediaSpeed[0]=lastPd;
    OLDpulseSpeedInterval=pulseSpeedInterval;
}    

//*******************************************************
//    clocked task for manage virtual encoder of seed wheel i/o
//*******************************************************
//*******************************************************
void step_SDPulseOut(){
    SDactualPosition++;
    prePosSD++;
}
//*******************************************************
void step_TBPulseOut(){
    TBmotorStepOut=!TBmotorStepOut;
    if (TBmotorStepOut==0){
        TBactualPosition++;
    }
}
//*******************************************************
// aggiornamento parametri di lavoro fissi e da Tritecnica
void aggiornaParametri(){
    speedPerimeter = Pi * speedWheelDiameter ;                          // perimeter of speed wheel
    pulseDistance = (speedPerimeter / speedWheelPulse)*1000.0f;         // linear space between speed wheel pulse
    seedPerimeter = Pi * (seedWheelDiameter-(deepOfSeed*2.0f));         // perimeter of seed wheel
    intraPickDistance = seedPerimeter/pickNumber;
    K_WheelRPM = 60.0f/seedPerimeter;                                   // calcola il K per i giri al minuto della ruota di semina
    K_WhellFrequency = (seedWheelMotorSteps*SDreductionRatio)/60.0f;    // calcola il K per la frequenza di comando del motore di semina
    rapportoRuote = pickNumber/cellsNumber;                             // calcola il rapporto tra il numero di becchi ed il numero di celle
    K_TBfrequency = (TBmotorSteps*TBreductionRatio)/240.0f;
    K_percentuale = TBmotorSteps*TBreductionRatio;
    SDsectorStep = (double)fixedStepGiroSD / (double)pickNumber;
    TBsectorStep = (TBmotorSteps*TBreductionRatio)/cellsNumber;
    KcorT = (SDsectorStep/TBsectorStep);///2.0f;
    angoloFase=angoloPh;
    avvioGradi=angoloAv;
    stepGrado=fixedStepGiroSD/360.0f;
    TBdeltaStep=(fixedStepGiroSD/pickNumber)+(stepGrado*avvioGradi);
    TBfaseStep = (stepGrado*angoloFase); 
    K_WheelRPM = 60.0f/seedPerimeter;                                   // calcola il K per i giri al minuto della ruota di semina  25.4                25,40
    if (speedFromPick==1) {
        intraPickDistance = seedPerimeter/pickNumber;
    }else{
        intraPickDistance = seedPerimeter/25.0f;        // 25 è il numero di fori presenti nel disco di semina
    }
    if (anticipoMax > ((360.0f/pickNumber)-(avvioGradi+16.0f))){
        anticipoMax=(360.0f/pickNumber)-(avvioGradi+16.0f);
    }
}
//*******************************************************
void cambiaTB(double perio){
    // update TB frequency
    double TBper=0.0f;
    if (aspettaStart==0){
        if (perio<250.0f){perio=500.0f;}
        double scala =0.0f;
        if (lowSpeed==1){
            scala =2.0f;
        }else{
            scala =1.8f;
        }
        TBper=perio/scala;
        if (oldPeriodoTB!=TBper){
            if (TBper >= 250.0f){
                TBticker.attach_us(&step_TBPulseOut,TBper);  // clock time are milliseconds and attach seed motor stepper controls
            }else{
                TBticker.detach();
            }
            oldPeriodoTB=TBper;
        }
    }
}
//*******************************************************
void seedCorrect(){
    /*
    posError determina la posizione relativa di TB rispetto ad SD
    la reale posizione di SD viene modificata in funzione della velocità per 
    traslare la posizione relativa di TB. All'aumentare della velocità la posizione
    di SD viene incrementata così che TB acceleri per raggiungerla in modo da rilasciare il seme prima
    La taratura del sistema avviene determinando prima il valore di angoloFase alla minima velocità,
    poi, alla massima velocità, dovrebbe spostarsi la posizione relativa con una variabile proporzionale alla velocità, ma c'è un però.
    Il problema è che il momento di avvio determina una correzione dell'angolo di partenza del tamburo
    angolo che viene rideterminato ogni volta che il sensore becchi legge un transito.
    Di fatto c'è una concorrenza tra l'angolo di avvio determinato e la correzione di posizione relativa
    del tamburo. E' molto probabile che convenga modificare solo la posizione relativa e non anche l'angolo di avvio
    Ancora di più se viene eliminata la parte gestita da ciclata.
    In questo modo dovrebbe esserci solo un andamento in accelerazione di TB che viene poi eventualmente decelerato
    dal passaggio sul sensore di TB. Funzione corretta perchè il sincronismo tra i sensori genera l'inibizione della correzione
    di fase di TB. In pratica il ciclo viene resettato al passaggio sul sensore di SD che riporta a 0 la posizione di SD.
    Appena il sensore di TB viene impegnato allora viene abilitato il controllo di fase del tamburo.
    Questo si traduce nel fatto che il controllo di posizione viene gestito solo all'interno di uno slot di semina in modo che
    il tamburo non risenta della condizione di reset della posizione di SD mentre lui è ancora nella fase precedente. Si fermerebbe.
    
    // La considerazione finale è che mantenendo l'angolo di avvio fisso e regolato sulla bassa velocità, intervenendo solo sulla correzione
    // di posizione in questa routine, dovrebbe essere possibile seminare correttamente a tutte le velocità regolando solo 2 parametri.
    */            
    /*
    SDsectorStep = (double)fixedStepGiroSD / (double)pickNumber;
    TBsectorStep = (TBmotorSteps*TBreductionRatio)/cellsNumber;
    KcorT = (SDsectorStep/TBsectorStep);
    angoloFase=angoloPh;
    stepGrado=fixedStepGiroSD/360.0f;
    avvioGradi = costante da terminale tritecnica
    TBdeltaStep=(fixedStepGiroSD/pickNumber)-(stepGrado*avvioGradi);
    TBfaseStep = (stepGrado*angoloFase); 
    anticipoMax = valore in gradi del massimo anticipo proporzionale alla velocità della ruota di semina
    */

    if ((tractorSpeed_MtS_timed>0.01f)){
        if (inhibit==0){
            double posError =0.0f;
            double posSD=((double)SDactualPosition)/KcorT;
            posError = posSD - (double)TBactualPosition;    
            // interviene sulla velocità di TB per raggiungere la corretta posizione relativa
            if((lowSpeed==0)&&(aspettaStart==0)){
                if (posError>50.0f){posError=50.0f;}
                if (posError<-50.0f){posError=-50.0f;}
                if ((posError >=1.0f)||(posError<=-1.0f)){   
                    ePpos = periodo /(1.0f+ ((posError/100.0f)));
                    cambiaTB(ePpos);
                }
            }
        }
        #if defined(pcSerial)
            #if defined(SDreset)
                pc.printf("Fase: %d",fase);
                pc.printf(" PrePosSD: %d",prePosSD);
                pc.printf(" PosSD: %d",SDactualPosition);
                pc.printf(" speed: %f",tractorSpeed_MtS_timed);
                pc.printf(" Trigger: %d \n", trigRepos);
            #endif
        #endif
    }
}
//*******************************************************
void videoUpdate(){
    for(int aa=0;aa<4;aa++){speedForDisplay[aa]=speedForDisplay[aa+1];}
    speedForDisplay[4]=tractorSpeed_MtS_timed;
    totalSpeed=0.0f;
    for (int aa=0; aa<5; aa++){totalSpeed += speedForDisplay[aa];}
    totalSpeed = totalSpeed / 5.0f;
}
//*******************************************************
void ciclaTB(){
    if ((startCicloTB==1)&&(cicloTbinCorso==0)){
        TBticker.attach_us(&step_TBPulseOut,TBperiod/2.5f);  // clock time are milliseconds and attach seed motor stepper controls
        cicloTbinCorso = 1;
        startCicloTB=0;
    }
    if ((loadDaCan==1)&&(loadDaCanInCorso==0)){
        TBticker.attach_us(&step_TBPulseOut,1000.0f);  // clock time are milliseconds and attach seed motor stepper controls
        loadDaCanInCorso=1;
        stopCicloTB=0;
    }
    if ((stopCicloTB==1)&&(TBactualPosition>5)){
        TBticker.detach();
        cicloTbinCorso = 0;
        stopCicloTB=0;
        loadDaCanInCorso=0;
        loadDaCan=0;
    }
}
//*******************************************************
void allarmi(){
        uint8_t alarmLowRegister1=0x00;
        alarmLowRegister=0x00;
        alarmHighRegister=0x80;

        //alarmLowRegister=alarmLowRegister+(all_semiFiniti*0x01);    // manca il sensore
        alarmLowRegister=alarmLowRegister+(all_pickSignal*0x02);    // fatto
        alarmLowRegister=alarmLowRegister+(all_cellSignal*0x04);    // fatto
        //alarmLowRegister=alarmLowRegister+(all_lowBattery*0x08);    // non gestito
        //alarmLowRegister=alarmLowRegister+(all_overCurrDC*0x10);    
        alarmLowRegister=alarmLowRegister+(all_stopSistem*0x20);    // verificarne la necessità
        //alarmLowRegister=alarmLowRegister+(all_upElements*0x40);    // manca il sensore
        //alarmLowRegister=alarmLowRegister+(all_noSeedOnCe*0x80);    // manca il sensore

        //alarmLowRegister1=alarmLowRegister1+(all_cfgnErrors*0x01);    // da scrivere
        alarmLowRegister1=alarmLowRegister1+(all_noDcRotati*0x02);    // fatto
        alarmLowRegister1=alarmLowRegister1+(all_noStepRota*0x04);    // fatto
        alarmLowRegister1=alarmLowRegister1+(all_speedError*0x08);    // fatto
        alarmLowRegister1=alarmLowRegister1+(all_noSpeedSen*0x10);    // fatto
        alarmLowRegister1=alarmLowRegister1+(all_no_Zeroing*0x20);    // fatto
        alarmLowRegister1=alarmLowRegister1+(all_genericals*0x40);
        if (alarmLowRegister1 > 0){
            alarmHighRegister = 0x81;
            alarmLowRegister = alarmLowRegister1;
        }

        #if defined(pcSerial)
            #if defined(VediAllarmi)
                if (all_pickSignal==1){pc.printf("AllarmeBecchi\n");}
                if (all_cellSignal==1){pc.printf("AllarmeCelle\n");}
                if (all_stopSistem==1){pc.printf("AllarmeStop\n");}
                if (all_noDcRotati==1){pc.printf("AllarmeDCnoRotation\n");}
                if (all_noStepRota==1){pc.printf("AllarmeNoStepRotation\n");}
                if (all_speedError==1){pc.printf("AllarmeSpeedError\n");}
                if (all_noSpeedSen==1){pc.printf("AllarmeNoSpeedSensor\n");}
                if (all_no_Zeroing==1){pc.printf("AllarmeNoZero\n");}
                if (all_genericals==1){pc.printf("AllarmeGenerico\n");}
                pc.printf("Code: 0x%x%x\n",alarmHighRegister,alarmLowRegister);
            #endif
        #endif
        all_semiFiniti=0;
        all_pickSignal=0;
        all_cellSignal=0;
        all_lowBattery=0;
        all_overCurrDC=0;
        all_stopSistem=0;
        all_upElements=0;
        all_noSeedOnCe=0;
        all_cfgnErrors=0;
        all_noDcRotati=0;
        all_noStepRota=0;
        all_speedError=0;
        all_noSpeedSen=0;
        all_no_Zeroing=0;
        all_genericals=0;
}
//*******************************************************
void upDateSincro(){
    char val1[8]={0,0,0,0,0,0,0,0};
    val1[0]=(prePosSD /0x00FF0000)&0x000000FF;
    val1[1]=(prePosSD /0x0000FF00)&0x000000FF;
    val1[2]=(prePosSD /0x000000FF)&0x000000FF;
    val1[3]=prePosSD & 0x000000FF;
    #if defined(canbusActive)
        #if defined(speedMaster)
            if(can1.write(CANMessage(TX_SI, *&val1,8))){
                checkState=0;
            }
        #endif
    #endif
}
//*******************************************************
void upDateSpeed(){
    /*
    aggiorna dati OPUSA3
    val1[0] contiene il dato di velocità
    val1[1] contiene il byte basso della tabella allarmi
        uint8_t all_semiFiniti = 0;     // semi finiti (generato dal relativo sensore)
        uint8_t all_pickSignal = 0;     // errore segnale becchi (generato dal tempo tra un becco ed il successivo)
        uint8_t all_cellSignal = 0;     // errore segnale celle (generato dal sensore tamburo )
        uint8_t all_lowBattery = 0;     // allarme batteria (valore interno di tritecnica)
        uint8_t all_overCurrDC = 0;     // sovracorrente motore DC  (generato dal sensore di corrente)
        uint8_t all_stopSistem = 0;     // sistema in stop (a tempo con ruota ferma)
        uint8_t all_upElements = 0;     // elementi sollevati   (generato dal relativo sensore)
        uint8_t all_noSeedOnCe = 0;     // fallanza di semina (manca il seme nella cella)
        uint8_t all_cfgnErrors = 0;     // errore di configurazione     (incongruenza dei parametri impostati)
        uint8_t all_noDcRotati = 0;     // ruota di semina bloccata (arriva la velocità ma non i becchi)
        uint8_t all_noStepRota = 0;     // tamburo di semina bloccato   (comando il tamburo ma non ricevo il sensore)
        uint8_t all_speedError = 0;     // errore sensore velocità  (tempo impulsi non costante)
        uint8_t all_noSpeedSen = 0;     // mancanza segnale di velocità (sento i becchi ma non la velocita)
        uint8_t all_no_Zeroing = 0;     // mancato azzeramento sistema (generato dal timeout del comando motore DC)
        uint8_t all_genericals = 0;     // allarme generico
    val1[2] contiene il byte alto della tabella di allarmi
    val1[3] contiene i segnali per la diagnostica
        bit 0= sensore ruota fonica
        bit 1= sensore celle
        bit 2= sensore posizione
        bit 3= sensore becchi
        bit 4= motore DC
        bit 5= controller
        bit 6= motore stepper
    */
        char val1[8]={0,0,0,0,0,0,0,0};

        val1[3]=0;
        val1[3]=val1[3]+(tractorSpeedRead*0x01);
        val1[3]=val1[3]+(TBzeroPinInputRev*0x02);
        val1[3]=val1[3]+(ElementPosition*0x04);
        val1[3]=val1[3]+(seedWheelZeroPinInputRev*0x08);
    #if defined(simulaPerCan)
        if (buttonUser==0){
            val1[1]=0x02;
        }else{
            val1[1]=0x00;
        }
        val1[3]=valore;
        valore+=1;
        if(valore>50){
            valore=0;
        }
        tractorSpeed_MtS_timed=valore;
    #endif
    allarmi();
    valore = totalSpeed*36.0f; // tractorSpeed_MtS_timed*36.0f;
    val1[0]=valore;
    val1[1]=alarmHighRegister;   // registro alto allarmi. Parte sempre da 0x80
    val1[2]=alarmLowRegister;   // registro basso allarmi
    //val1[3]=val1[3];// registro di stato degli ingressi
    val1[4]=resetComandi;
    val1[5]=cellsCounterLow;
    val1[6]=cellsCounterHig;
    #if defined(canbusActive)
        if(can1.write(CANMessage(TX_ID, *&val1,8))){
            checkState=0;
        }
    #endif
}
//*******************************************************
void leggiCAN(){
    #if defined(canbusActive)
        if(can1.read(rxMsg)) {
            if (firstStart==1){
                #if defined(speedMaster)
                    sincUpdate.attach(&upDateSincro,0.01f);
                #endif
                canUpdate.attach(&upDateSpeed,0.11f);
                firstStart=0;
            }
            
            if (rxMsg.id==RX_ID){
                #if defined(pcSerial)
                    #if defined(canDataReceived)
                        pc.printf("Messaggio ricevuto\n");
                    #endif
                #endif
            }
            if (rxMsg.id==RX_Broadcast){
                #if defined(pcSerial)
                    #if defined(canDataReceived)
                        pc.printf("BroadCast ricevuto\n");
                    #endif
                #endif
                enableSimula= rxMsg.data[0];
                speedSimula = rxMsg.data[1];
                avviaSimula = rxMsg.data[2];
                selezionato = rxMsg.data[3];
                comandiDaCan = rxMsg.data[4];
                #if defined(pcSerial)
                    #if defined(canDataReceived)
                        pc.printf("Comandi: %x\n",comandiDaCan);
                    #endif
                #endif
                switch (comandiDaCan){
                    case 1:
                    case 3:
                        azzeraDaCan=1;
                        resetComandi=0x01;
                        comandiDaCan=0;
                        break;
                    case 2:
                        loadDaCan=1;
                        resetComandi=0x02;
                        comandiDaCan=0;
                        break;
                    default:
                        comandiDaCan=0;
                        resetComandi=0xFF;
                        break;
                }
                #if defined(pcSerial)
                    #if defined(canDataReceived)
                        pc.printf("Comandi: %x\n",resetComandi);
                    #endif
                #endif
                if (speedSimula>45){
                    speedSimula=45;
                }
                // modulo 1
                if (RX_ID==0x100){
                    if ((selezionato&0x01)==0x01){
                        simOk=1;
                    }else{
                        simOk=0;
                    }
                }
                // modulo 2
                if (RX_ID==0x102){
                    if ((selezionato&0x02)==0x02){
                        simOk=1;
                    }else{
                        simOk=0;
                    }
                }
                // modulo 3
                if (RX_ID==0x104){
                    if ((selezionato&0x04)==0x04){
                        simOk=1;
                    }else{
                        simOk=0;
                    }
                }
                // modulo 4
                if (RX_ID==0x106){
                    if ((selezionato&0x08)==0x08){
                        simOk=1;
                    }else{
                        simOk=0;
                    }
                }
                // modulo 5
                if (RX_ID==0x108){
                    if ((selezionato&0x10)==0x10){
                        simOk=1;
                    }else{
                        simOk=0;
                    }
                }
                
            }
            if (rxMsg.id==RX_Settings){
                if (tractorSpeed_MtS_timed==0.0f){
                    pickNumber = rxMsg.data[0];               // numero di becchi installati sulla ruota di semina
                    cellsNumber = rxMsg.data[1];             // numero di celle del tamburo
                    deepOfSeed=(rxMsg.data[2]/10000.0f);                // deep of seeding 
                    seedWheelDiameter = ((rxMsg.data[4]*0xFF)+rxMsg.data[3])/10000.0f;      // seed wheel diameter setting
                    speedWheelDiameter = ((rxMsg.data[6]*0xFF)+rxMsg.data[5])/10000.0f;     // variable for tractor speed calculation (need to be set from UI) ( Unit= meters )
                    speedWheelPulse = rxMsg.data[7];          // variable which define the number of pulse each turn of tractor speed wheel (need to be set from UI)
                    aggiornaParametri();
                    #if defined(pcSerial)
                        #if defined(canDataReceived)
                            pc.printf("Seed wheel diameter %f \n",seedWheelDiameter);
                            pc.printf("Speed wheel diameter %f \n",speedWheelDiameter);
                        #endif
                    #endif
                }
            }
            if (rxMsg.id==RX_AngoloPh){
                if (tractorSpeed_MtS_timed==0.0f){
                    #if defined(M1)
                        angoloPh = (double) rxMsg.data[0] ;
                        aggiornaParametri();
                    #endif
                    #if defined(M2)
                        angoloPh = (double) rxMsg.data[1] ;
                        aggiornaParametri();
                    #endif
                    #if defined(M3)
                        angoloPh = (double) rxMsg.data[2] ;
                        aggiornaParametri();
                    #endif
                    #if defined(M4)
                        angoloPh = (double) rxMsg.data[3] ;
                        aggiornaParametri();
                    #endif
                    #if defined(M5)
                        angoloPh = (double) rxMsg.data[4] ;
                        aggiornaParametri();
                    #endif
                    #if defined(M6)
                        angoloPh = (double) rxMsg.data[5] ;
                        aggiornaParametri();
                    #endif
                }
            }
            if (rxMsg.id==RX_AngoloAv){
                if (tractorSpeed_MtS_timed==0.0f){
                    #if defined(M1)
                        angoloAv = (double) rxMsg.data[0] ;
                        aggiornaParametri();
                    #endif
                    #if defined(M2)
                        angoloAv = (double) rxMsg.data[1] ;
                        aggiornaParametri();
                    #endif
                    #if defined(M3)
                        angoloAv = (double) rxMsg.data[2] ;
                        aggiornaParametri();
                    #endif
                    #if defined(M4)
                        angoloAv = (double) rxMsg.data[3] ;
                        aggiornaParametri();
                    #endif
                    #if defined(M5)
                        angoloAv = (double) rxMsg.data[4] ;
                        aggiornaParametri();
                    #endif
                    #if defined(M6)
                        angoloAv = (double) rxMsg.data[5] ;
                        aggiornaParametri();
                    #endif
                }
            }
            if (rxMsg.id==RX_Quinconce){
                if (tractorSpeed_MtS_timed==0.0f){
                    quinconceActive = (uint8_t) rxMsg.data[0];              
                    aggiornaParametri();
                }
            }
            if (rxMsg.id==RX_QuincSinc){
                masterSinc = (uint32_t) rxMsg.data[0] * 0x00FF0000;              
                masterSinc = masterSinc + ((uint32_t) rxMsg.data[1] * 0x0000FF00);              
                masterSinc = masterSinc + ((uint32_t) rxMsg.data[2] * 0x000000FF);              
                masterSinc = masterSinc + ((uint32_t) rxMsg.data[3]);
                quincVerify=1;
                //pc.printf("pippo");
            }
        } 
    #endif
    #if defined(overWriteCanSimulation)
        enableSimula=1;
        speedSimula=25;
        avviaSimula=1;
        simOk=1;
    #endif
    
}

//*******************************************************
void DC_CheckCurrent(){
    
    // TODO: tabella di riferimento assorbimenti alle varie velocità al fine di gestire
    //       gli allarmi e le correzioni di velocità
    
    //float SD_analogMatrix[10]={0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f};
    //int SD_analogIndex[10]={0,0,0,0,0,0,0,0,0,0};
    // Analog reading
    //number = floor(number * 100) / 100;
    //if (pwmCheck==1){
        timeout.detach();
        for (int ii=1;ii<20;ii++){
            SD_analogMatrix[ii]=SD_analogMatrix[ii+1];
        }
        SD_CurrentAnalog = floor(SDcurrent.read()*100)/100;              // valore in ingresso compreso tra 0.0 e 1.0
        SD_analogMatrix[20]=SD_CurrentAnalog;
        if (SDmotorPWM==0.0f){
            SD_CurrentStart=SD_CurrentAnalog;
        }
        float sommaTutto=0.0f;
        for (int ii=1;ii<21;ii++){
            sommaTutto=sommaTutto+SD_analogMatrix[ii];
        }
        float SD_CurrentAnalogica=sommaTutto/20.0f;
        SD_CurrentScaled = floor((((SD_CurrentAnalogica) - (SD_CurrentStart))*3.3f)/(SD_CurrentFactor/1000.0f)*10)/10;
        #if defined(pcSerial)
            #if defined(correnteAssorbita)
                pc.printf("Corrente: %f \n",SD_CurrentAnalogica);
            #endif
        #endif   
        if (SD_CurrentAnalogica < -10.0f){
            #if defined(canbusActive)
                int SD_AllarmeUndervoltage=1;   // da implementare nel CAN
            #endif
        }
        if (SD_CurrentAnalogica > 10.0f){
            #if defined(canbusActive)
                all_overCurrDC=1;
            #endif
        }
    //}
}
//*******************************************************
void DC_prepare(){
    // direction or brake preparation
    if (DC_brake==1){SDmotorInA=1;SDmotorInB=1;}else{if (DC_forward==1){SDmotorInA=1;SDmotorInB=0;}else{SDmotorInA=0;SDmotorInB=1;}}
    // fault reading
    if (SDmotorInA==1){SD_faultA=1;}else{SD_faultA=0;}
    if (SDmotorInB==1){SD_faultB=1;}else{SD_faultB=0;}
}
//*******************************************************
void startDelay(){
    int ritardo =0;
    ritardo = (int)((float)(dcActualDuty*800.0f));
    timeout.attach_us(&DC_CheckCurrent,ritardo);
}
//*******************************************************
void quinCalc(double parametro){
    double riferimento=tempoTraBecchi_mS/parametro;
    // riceve l'impulso di sincro dal master e fa partire il timer di verifica dell'errore
    if ((quinconceIn==1)&&(quincStart==0)&&(oldQuincIn==0)){
            #if defined(pcSerial)
                #if defined(Qnc)
                    pc.printf(" quinconceIn: %d", masterSinc);
                #endif
            #endif
        oldQuincIn=1;
        quincTime.reset();
        quincStart=1;
    }
    if( quinconceIn==0){
        oldQuincIn=0;
    }
    // riceve l'impulso di passaggio del becco locale e determina il valore dell'errore
    if (quinconceActive==1){
        if ((quincStart==1)&&(SDzeroDebounced==1)){
            memoDuty=dcActualDuty;
            accelera=0;
            decelera=0;
            quincStop=0;
            quincStart=0;
            percento=0.0f;
            scostamento = (double)quincTime.read_ms();
            quincError = 0.0f;
            if ((scostamento > riferimento)||(scostamento==0.0f)){
                decelera=1;
                accelera=0;
                quincError = scostamento - riferimento;
                percento = 1.0f - (abs(quincError)/riferimento);
            }
            if ((scostamento < riferimento)&&(scostamento >0.0f)){
                decelera=0;
                accelera=1;
                quincError = riferimento - scostamento;
                percento = 1.0f + (abs(quincError)/riferimento);
            }
            if ((decelera==1)&&(quincStop==0)){
                quincStopTimer.reset();
                quincStopTimer.start();
                quincStop=1;
                propoQuinc = (speedOfSeedWheel/1.25f)*quincError;
            }
            if (abs(percento) > 1.05f){percento=1.05f;}
            if (abs(percento) < 0.75f){percento=0.75f;}
        }
        dcActualDuty = memoDuty * abs(percento);
    }
    if ((quinconceActive==0)&&(quincEnable==1)){
        // da encoder master
        if (quincVerify==1){
            memoDuty=dcActualDuty;
            quincVerify=0;
            accelera=0;
            decelera=0;
            percento=0.0f;
            quincError = 0.0f;
            if (masterSinc > prePosSD){
                decelera=0;
                accelera=1;
                quincError = masterSinc - prePosSD;
                percento = 1.0f + (abs(quincError)/SDsectorStep);
            }
            if (masterSinc < prePosSD){
                decelera=1;
                accelera=0;
                quincError = prePosSD -masterSinc;
                percento = 1.0f - (abs(quincError)/riferimento);
            }
            // se quinconce richiesto deve andare al valore di riferimento
            if (abs(percento) > 1.55f){percento=1.55f;}
            if (abs(percento) < 0.55f){percento=0.55f;}
            #if defined(pcSerial)
                #if defined(Qnc)
                    pc.printf(" masterSinc: %d", masterSinc);
                    pc.printf(" prePosSD: %d",prePosSD);
                    pc.printf(" percento: %f\n",percento);
                #endif
            #endif
            dcActualDuty = memoDuty * abs(percento);
        }
    }
}
// ------------------------------------------------------------------------------------------------------------------------------------------------------------------
// MAIN SECTION
// ------------------------------------------------------------------------------------------------------------------------------------------------------------------

int main()
{
    for (int a=0; a<5;a++){
        mediaSpeed[a]=0;
    }
    
    // Stepper driver init and set
    TBmotorRst=0;                               // reset stepper driver
    TBmotorDirecti=0;                           // reset stepper direction
    // M1   M2  M3  RESOLUTION
    // 0    0   0       1/2
    // 1    0   0       1/8
    // 0    1   0       1/16
    // 1    1   0       1/32
    // 0    0   1       1/64
    // 1    0   1       1/128
    // 0    1   1       1/10
    // 1    1   1       1/20
    if (TBmotorSteps==400){
        TBmotor_M1=0;
        TBmotor_M2=0;
        TBmotor_M3=0;
    }else if (TBmotorSteps==1600){
        TBmotor_M1=1;
        TBmotor_M2=0;
        TBmotor_M3=0;
    }else if (TBmotorSteps==3200){
        TBmotor_M1=0;
        TBmotor_M2=1;
        TBmotor_M3=0;
    }else if (TBmotorSteps==6400){
        TBmotor_M1=1;
        TBmotor_M2=1;
        TBmotor_M3=0;
    }else if (TBmotorSteps==12800){
        TBmotor_M1=0;
        TBmotor_M2=0;
        TBmotor_M3=1;
    }else if (TBmotorSteps==25600){
        TBmotor_M1=1;
        TBmotor_M2=0;
        TBmotor_M3=1;
    }else if (TBmotorSteps==2000){
        TBmotor_M1=0;
        TBmotor_M2=1;
        TBmotor_M3=1;
    }else if (TBmotorSteps==4000){
        TBmotor_M1=1;
        TBmotor_M2=1;
        TBmotor_M3=1;
    }
    TBmotorRst=1;
    
    // DC reset ad set
    int decima = 100;
    wait_ms(200);
    SD_CurrentStart=floor(SDcurrent.read()*decima)/decima;
    wait_ms(2);
    SD_CurrentStart+=floor(SDcurrent.read()*decima)/decima;
    wait_ms(1);
    SD_CurrentStart+=floor(SDcurrent.read()*decima)/decima;
    wait_ms(3);
    SD_CurrentStart+=floor(SDcurrent.read()*decima)/decima;
    SD_CurrentStart=(floor((SD_CurrentStart/4.0f)*decima)/decima)-0.01f; 
    wait_ms(100);
    DC_prepare();
    
    speedTimer.start();                        // speed pulse timer 
    intraPickTimer.start();
    speedTimeOut.start();               
    speedFilter.start();
    seedFilter.start();
    TBfilter.start();
    sincroTimer.start();
    rotationTimeOut.start();
    metalTimer.start();
    quincTime.start();
    
    //*******************************************************
    // controls for check DC motor current
    //currentCheck.attach(&DC_CheckCurrent, 0.10f);
    pwmCheck.rise(&startDelay);
    //tbCorrection.attach(&correction,0.1f;
    
    if (inProva==0){
        tractorSpeedRead.rise(&tractorReadSpeed);
        tftUpdate.attach(&videoUpdate,0.50f);
        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
        if (speedFromPick==0){
            ElementPosition.rise(&sd25Fall);
        }
    }else{
      tftUpdate.attach(&videoUpdate,0.125f);
    }        
    
    aggiornaParametri();

    SDmotorPWM.period_us(periodoSD);      // frequency 1KHz pilotaggio motore DC
    SDmotorPWM.write(0.0f);         // duty cycle = stop
    TBmotorDirecti=0;               // tb motor direction set

    #if defined(provaStepper)
       TBmotorRst=1;
       TBticker.attach_us(&step_TBPulseOut,3000.0f);  // clock time are seconds and attach seed motor stepper controls
    #endif // end prova stepper
    
    #if defined(canbusActive)
        can1.attach(&leggiCAN, CAN::RxIrq);
    #endif
    
//**************************************************************************************************************
// MAIN LOOP
//**************************************************************************************************************
    while (true){

        // ripetitore segnale di velocità
        if (tractorSpeedRead==0){speedClock=0;}
        
        // inversione segnali ingressi
        TBzeroPinInput = !TBzeroPinInputRev;
        seedWheelZeroPinInput = !seedWheelZeroPinInputRev;
        
        // se quinconce attivo ed unita' master invia segnale di sincro
        #if defined(speedMaster)
            if (seedWheelZeroPinInput==1){
                quinconceOut=1;
            }else{
                quinconceOut=0;
            }
        #endif

        #if defined(overWriteCanSimulation)
            leggiCAN();
        #endif

        // simulazione velocita
        if (enableSimula==1){
            double TMT = 0.0f;
            if (speedSimula > 0){
                TMT = (double)(speedSimula) * 100.0f /3600.0f;
                pulseSpeedInterval = pulseDistance / TMT;
            }else{
                pulseSpeedInterval = 10000.0f;
            }                
            if (avviaSimula==1){
                if(oldSimulaSpeed!=pulseSpeedInterval){
                    spedSimclock.attach_us(&speedSimulationClock,pulseSpeedInterval);
                    oldSimulaSpeed=pulseSpeedInterval;
                }
            }else{
                oldSimulaSpeed=10000.0f;
                spedSimclock.detach();
            }
        }else{
            spedSimclock.detach();
        }
        
        //*******************************************************
        // determina se sono in bassa velocità per il controllo di TB
        if (speedOfSeedWheel<=minSeedSpeed){
            if (lowSpeedRequired==0){
                ritardaLowSpeed.reset();
                ritardaLowSpeed.start();
            }
            lowSpeedRequired=1;
        }else{
            if (lowSpeedRequired==1){
                lowSpeedRequired=0;
                ritardaLowSpeed.reset();
                ritardaLowSpeed.stop();
            }
        }

        if (ritardaLowSpeed.read_ms()> 2000){
            lowSpeed=1;
        }else{
            lowSpeed=0;
        }
        

//**************************************************************************************************************
//**************************************************************************************************************
// LOGICAL CONTROLS
//**************************************************************************************************************
//**************************************************************************************************************
        
        if (inProva==0){
            if ((startCycleSimulation==0)&&(enableSimula==0)){zeroRequestBuf=1;runRequestBuf=0;enableCycle=1;  
            }else{zeroRequestBuf=1;runRequestBuf=0;enableCycle=1;}
            if ((tractorSpeedRead==0)&&(speedFilter.read_ms()>3)){oldTractorSpeedRead=0;}
            // ----------------------------------------
            // filtra il segnale dei becchi per misurare il tempo di intervallo tra loro
            // ----------------------------------------
            if ((seedWheelZeroPinInput==0)&&(seedFilter.read_ms()>=4)){
                oldSeedWheelZeroPinInput=0;
                SDzeroDebounced=0;
            }
            if ((seedWheelZeroPinInput==1)&&(oldSeedWheelZeroPinInput==0)){
                timeIntraPick = (double)intraPickTimer.read_ms();
                prePosSD=500; // preposizionamento SD
                intraPickTimer.reset();
                rotationTimeOut.reset();
                seedFilter.reset();          
                sincroTimer.reset();
                oldSeedWheelZeroPinInput=1;
                SDzeroDebounced=1;
                SDwheelTimer.reset();
                if (quincCnt<10){
                    quincCnt++;
                }
                if (quincCnt >3){
                    quincEnable=1;
                }else{
                    quincEnable=0;
                }
                if ((aspettaStart==0)&&(lowSpeed==1)){
                    beccoPronto=1;
                }
                SDzeroCyclePulse=1;
                lockStart=0;
                double fase1=0.0f;
                forzaFase=0;
                double limite=fixedStepGiroSD/pickNumber;
                if (pickCounterLow< 0xFF){
                    pickCounterLow++;
                }else{
                    pickCounterHig++;
                    pickCounterLow=0;
                }
                if (tamburoStandard==0){
                    fase1=TBdeltaStep;  
                }else{
                    if(speedForCorrection >= speedOfSeedWheel){
                        fase1=TBdeltaStep;
                    }else{
                        //fase1=(TBdeltaStep)-(((speedOfSeedWheel-speedForCorrection)/1.25f)*(TBfaseStep));
                        fase1=(TBdeltaStep)-(((speedOfSeedWheel)/1.25f)*(TBfaseStep));
                    }
                    if (fase1 > limite){
                        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)
                        //forzaFase=1;
                    }
                    if ((fase1 > (limite -20.0f))&&(fase1<(limite +5.0f))){
                        fase1 = limite -20.0f;  // se la fase è molto vicina al limite interpone uno spazio minimo di lavoro del sincronismo
                        forzaFase=1;
                    }
                    trigRepos=1;
                }
                fase = (uint32_t)fase1+500;
                #if defined(pcSerial)
                    #if defined(inCorre)
                        pc.printf(" limite %f", limite);
                        pc.printf(" delta %f", TBdeltaStep);
                        pc.printf(" faseStep %f", TBfaseStep);
                        pc.printf(" fase %d",fase);
                        pc.printf(" forzaFase %d",forzaFase);
                        pc.printf(" trigRepos %d", trigRepos);
                        pc.printf(" ActualSD: %d",SDactualPosition);
                        pc.printf(" SpeedWheel: %f",speedOfSeedWheel);
                        pc.printf(" SPEED: %f \n",tractorSpeed_MtS_timed);
                    #endif
                #endif
                if (timeIntraPick >= (memoIntraPick*2)){
                    if ((aspettaStart==0)&&(zeroRequestBuf==0)){
                        if (firstStart==0){
                            all_pickSignal=1;
                        }
                    }
                }
                memoIntraPick = timeIntraPick;
                if (speedFromPick==1){
                    speedOfSeedWheel=((seedPerimeter / pickNumber)/timeIntraPick)*1000.0f;
                }
                if ((tractorSpeed_MtS_timed==0.0f)&&(zeroCycle==0)&&(zeroCycleEnd==1)){
                    if ((firstStart==0)&&(simOk==0)&&(enableSimula==0)&&(zeroCycleEnd==1)){
                        all_noSpeedSen=1;
                    }
                    #if defined(pcSerial)
                        #if defined(canDataReceived)
                            pc.printf("allarme no speed sensor");
                        #endif
                    #endif
                }
                pulseRised2=1;
                #if defined(speedMaster)
                    double oldLastPr = (double)oldLastPulseRead*1.2f;
                    if((double)speedTimeOut.read_us()> (oldLastPr)){
                        if ((firstStart==0)&&(simOk==0)&&(enableSimula==0)&&(zeroCycleEnd==1)){
                            all_speedError =1;
                        }
                        #if defined(pcSerial)
                            #if defined(canDataReceived)
                                pc.printf("allarme errore speed sensor");
                            #endif
                        #endif
                        
                    }
                #endif
                //*******************************************
                // segue calcolo clock per la generazione della posizione teorica
                // la realtà in base al segnale di presenza del becco
                realSpeed = speedOfSeedWheel;
                realGiroSD = seedPerimeter / realSpeed;
                tempoBecco = (realGiroSD/360.0f)*16000.0f;
                frequenzaReale = fixedStepGiroSD/realGiroSD;
                semiPeriodoReale = (1000000.0f/frequenzaReale);
                tempoTraBecchi_mS = 0.0f;
                seedWheelRPM = (speedOfSeedWheel)*K_WheelRPM ;                      // calcola i giri al minuto della ruota di semina           7.37                31,75
                rapportoRuote = pickNumber/cellsNumber;                             // 0.67 calcola il rapporto tra il numero di becchi ed il numero di celle 0.8    1,00
                TBrpm = seedWheelRPM*rapportoRuote;                                 // 5.896                                                                        31,75
                TBgiroStep = TBmotorSteps*TBreductionRatio;
                K_TBfrequency = TBgiroStep/60.0f;                                   // 1600 * 1.65625f /60 = 44                                                     44,00
                TBfrequency = (TBrpm*K_TBfrequency);                                // 130Hz a 0,29Mts                                                            1397,00 a 1,25mt/s con 15 becchi e 15 celle
                TBperiod=1000000.0f/TBfrequency;                                    //                                                                              715uS
            }
// ----------------------------------------
// check SD fase 
            if ((prePosSD >= fase)||(forzaFase==1)){//&&(prePosSD < (fase +30))){
                forzaFase=0;
                if (trigRepos==1){
                    SDactualPosition=0;
                    if ((countCicli<30)&&(trigCicli==0)){countCicli++;trigCicli=1;}
                    if(countCicli>=cicliAspettaStart){aspettaStart=0;}
                    if ((lowSpeed==0)&&(aspettaStart==0)&&(lockStart==0)){syncroCheck=1;beccoPronto=0;}
                    if (trigTB==0){inhibit=1;trigSD=1;}else{inhibit=0;trigTB=0;trigSD=0;}
                    trigRepos=0;
                }
            }else{
                trigCicli=0;
            }
            /*
            if ((SDactualPosition >= fase) && (SDactualPosition < (fase +(uint32_t)30))){
                if ((countCicli<30)&&(trigCicli==0)){countCicli++;trigCicli=1;}
                if(countCicli>=cicliAspettaStart){aspettaStart=0;}
                if ((lowSpeed==0)&&(aspettaStart==0)&&(lockStart==0)){syncroCheck=1;beccoPronto=0;}
            }else{
                trigCicli=0;
            }
            */
// ----------------------------------------  
// filtra il segnale del tamburo per lo stop in fase del tamburo stesso
            if (TBzeroPinInput==0){if (TBfilter.read_ms()>=5){oldTBzeroPinInput=0;}}
            if ((TBzeroPinInput==1)&&(oldTBzeroPinInput==0)){
                oldTBzeroPinInput=1;
                if (loadDaCanInCorso==0){
                    stopCicloTB=1;
                    startCicloTB=0;
                }
                TBfilter.reset();
                TBzeroCyclePulse=1;
                TBactualPosition=0;
                if (cellsCounterLow < 0xFF){
                    cellsCounterLow++;
                }else{
                    cellsCounterHig++;
                    cellsCounterLow=0;
                }
                if (loadDaCanInCorso==1){
                    cntCellsForLoad++;
                    if (cntCellsForLoad >= 5){
                        stopCicloTB=1;   
                        cntCellsForLoad=0;
                    }
                }else{
                    cntCellsForLoad=0;
                }
                if (trigSD==0){inhibit=1;trigTB=1;}else{inhibit=0;trigTB=0;trigSD=0;}
                // torna indietro per sbloccare il tamburo
                if ((TBmotorDirecti==1)&&(erroreTamburo==1)){
                    cntCellsForReload++;
                    if (cntCellsForReload > 3){
                        TBmotorDirecti=0;
                        erroreTamburo=0;
                    }    
                }
            }
            if ((double)TBactualPosition > ((TBgiroStep/cellsNumber)*1.2f)){
                if (firstStart==0){
                    all_cellSignal=1;
                }
                if (erroreTamburo==0){
                    erroreTamburo=1;
                    TBmotorDirecti=1;
                    cntCellsForReload=0;
                    cntTbError++;
                }
                #if defined(pcSerial)
                    #if defined(canDataReceived)
                        pc.printf("allarme error cells");
                    #endif
                #endif
            }
            if (((double)TBactualPosition > ((TBgiroStep/cellsNumber)*3.0f))||(cntTbError>2)){
                if (firstStart==0){
                    all_noStepRota=1;
                }
                cntTbError=0;
                #if defined(pcSerial)
                    #if defined(canDataReceived)
                        pc.printf("allarme no cells sensor");
                    #endif
                #endif
            }
// ----------------------------------------            
// read and manage joystick
            // WARNING - ENABLE CYCLE IS SOFTWARE ALWAYS ACTIVE
            if (enableCycle==1){
                if(runRequestBuf==1){
                    if (OldStartCycle!=runRequestBuf){
                        if((startCycle==0)&&(zeroCycleEnd==1)){
                            startCycle=1;
                            OldStartCycle = runRequestBuf;
                            oldZeroCycle=0;
                        }
                    }
                }else{
                    startCycle=0;
                    pntMedia=0;
                }
                if (azzeraDaCan==1){
                    if (tractorSpeed_MtS_timed==0.0f){
                        zeroRequestBuf=1;
                        oldZeroCycle=0;
                    }
                    azzeraDaCan=0;
                }
                if (loadDaCan==1){
                    if (tractorSpeed_MtS_timed==0.0f){
                        ciclaTB();
                    }                    
                }
                if ((zeroRequestBuf==1)){
                    if (oldZeroCycle!=zeroRequestBuf){
                        zeroCycle=1;
                        zeroCycleEnd=0;
                        SDzeroed=0;
                        TBzeroed=0;
                        zeroTrigger=0;
                        oldZeroCycle = zeroRequestBuf;
                    }
                }
            }else{
                startCycle=0;
                zeroCycle=0;
            }
            
    //***************************************************************************************************        
    // pulseRised define the event of speed wheel pulse occurs
    //        
            double maxInterval = pulseDistance/minWorkSpeed;
            double minIntervalPulse = pulseDistance/maxWorkSpeed;
            if (pulseRised==1){ 
                if (enableSpeed<10){enableSpeed++;}
                pulseRised=0;pulseRised1=1;speedMediaCalc();
                // calcola velocità trattore
                if(enableSpeed>=2){
                    if ((pulseSpeedInterval>=0.0f)){ //minIntervalPulse)&&(pulseSpeedInterval<maxInterval)){
                        tractorSpeed_MtS_timed = ((pulseDistance  / pulseSpeedInterval));  // tractor speed (unit= Mt/S) from pulse time interval
                        if (checkSDrotation==0){
                            checkSDrotation=1;
                            SDwheelTimer.start();
                        }
                    }
                }
                if ((pulseSpeedInterval>=minIntervalPulse)&&(pulseSpeedInterval<maxInterval)){
                    enableSDcontrol =1;
                }
                speedTimeOut.reset();
            }else{
                double oldLastPr = (double)oldLastPulseRead*1.7f;
                if((double)speedTimeOut.read_us()> (oldLastPr)){
                    tractorSpeed_MtS_timed = 0.0f;
                    enableSDcontrol =0;
                    pntMedia=0;
                    speedTimeOut.reset();
                    enableSpeed=0;
                    quincCnt=0;
                }
            }
            // esegue il controllo di velocità minima
            if ((double)speedTimer.read_ms()>=maxInterval){tractorSpeed_MtS_timed = 0.0f;
                enableSDcontrol =0;
                enableSpeed=0;
            }
    //***************************************************************************************************************
    // cycle logic control section
    //***************************************************************************************************************
                if (enableSimula==1){if(simOk==0){tractorSpeed_MtS_timed=0.0f;}}
                if ((tractorSpeed_MtS_timed>0.01f)){
                    cycleStopRequest=1;
                    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) 
                    if (speedFromPick==1) {
                        tempoTraBecchi_mS = (tempoGiroSD / pickNumber)*1000.0f;      // tempo tra due impulsi dai becchi in millisecondi ( circa 157mS a 4,5Km/h)  
                    }else{
                        tempoTraBecchi_mS = (tempoGiroSD / 25.0f)*1000.0f;      // tempo tra due impulsi dai becchi in millisecondi ( circa 157mS a 4,5Km/h)  
                    }
                    seedWheelPeriodTeory = semiPeriodoReale;
                    seedWheelPeriod=seedWheelPeriodTeory;
                    if (seedWheelPeriod < 180.0f){seedWheelPeriod = 180.0f;}
                    if ((oldSeedWheelPeriod!=seedWheelPeriod)&&(seedWheelPeriod >=180.0f )){
                        SDticker.attach_us(&step_SDPulseOut,seedWheelPeriod);  // clock time are milliseconds and attach seed motor stepper controls
                        oldSeedWheelPeriod=seedWheelPeriod;
                    }
                    //*******************************************
                    // segue calcolo duty cycle comando motore DC per allinearsi con la velocità del trattore
                    double dutyTeorico = 0.00;
                    if ((tractorSpeed_MtS_timed>0.0)&&(tractorSpeed_MtS_timed<tabSpeed[0])){dutyTeorico = tabComan[0];}
                    for (int ii = 0;ii<16;ii++){
                        if ((tractorSpeed_MtS_timed>=tabSpeed[ii])&&(tractorSpeed_MtS_timed<tabSpeed[ii+1])){
                            dutyTeorico = tabComan[ii+1];
                        }
                    }
                    if (tractorSpeed_MtS_timed > tabSpeed[16]){dutyTeorico=tractorSpeed_MtS_timed/1.25f;}
                    if ((pulseRised1==1)&&(enableSpeed>3)&&(pulseRised2==1)){
                        double erroreTempo = 0.0f;
                        if(speedFromPick==1){
                            erroreTempo = (double)timeIntraPick - tempoTraBecchi_mS;             // errore tra il tempo previsto ed il tempo reale ( >0 se sto andando più piano del previsto)
                        }else{
                            erroreTempo = (double)memoTimeHole - tempoTraBecchi_mS;             // errore tra il tempo previsto ed il tempo reale ( >0 se sto andando più piano del previsto)
                        }
                        double errorePercentuale = erroreTempo / tempoTraBecchi_mS;
                        double k1=errorePercentuale*0.065f;double k3=0.0f;double k4=0.0f;double k5=0.0f;
                        if (tractorSpeed_MtS_timed <= minSeedSpeed){k3=0.105;k4=0.207;k5=0.310;
                        }else{k3=0.1075f;k4=0.211f;k5=0.315f;}
                        if (errorePercentuale > 25.0f){k1=errorePercentuale*k5;}
                        if ((errorePercentuale >= 10.0f)&&(errorePercentuale<=25.0f)){k1=errorePercentuale*k4;}
                        if (errorePercentuale < 10.0f){k1=errorePercentuale*k3;}
                        double memoCorrezione = (dutyTeorico *k1);
                        correzione = correzione + memoCorrezione;
                        pulseRised1=0;
                        pulseRised2=0;
                    }
                    if (correzione > (dutyTeorico*0.20)){correzione = dutyTeorico*0.20;}
                    if((enableSDcontrol==1)){
                        if (correzioneAttiva==1){
                            dcActualDuty = dutyTeorico + correzione;
                        }
                    }else{
                        dcActualDuty = dutyTeorico;
                    }
                    // prova a mantenere il quinconce quando attivo
                    #if defined(speedMaster)
                        dcActualDuty=dcActualDuty;
                        DC_brake=0;
                        DC_forward=1;
                        DC_prepare();
                    #else
                        #if defined(mezzo)
                            if (quincEnable==1){
                                if (quinconceActive==1){
                                    quinCalc(2.0f);
                                }else{
                                    quinCalc(1.0f);
                                }
                            }else{
                                DC_brake=0;
                                DC_forward=1;
                                DC_prepare();
                            }
                        #else
                            if (quincEnable==1){
                                quinCalc(1.0f);
                            }else{
                                DC_brake=0;
                                DC_forward=1;
                                DC_prepare();
                            }
                        #endif
                    #endif
                    if (dcActualDuty <0.0f){dcActualDuty=0.0f;}
                    if (dcActualDuty > 1.0f){dcActualDuty = dcMaxSpeed;}
                    SDmotorPWM.write(dcActualDuty);
                    // allarme
                    if (SDwheelTimer.read_ms()>4000){
                        if (firstStart==0){
                            all_noDcRotati=1;
                        }
                        #if defined(pcSerial)
                            #if defined(canDataReceived)
                                pc.printf("allarme no DC rotation");
                            #endif
                        #endif
                        
                    }

    //***************************************************************************************************************
    // CONTROLLA TAMBURO
    //***************************************************************************************************************
                    if(lowSpeed==0){
                        if (syncroCheck==1){
                            syncroCheck=0;
                            lockStart=1;
                            periodo = TBperiod;
                            if (aspettaStart==0){cambiaTB(periodo);}
                        }
                        // controllo di stop
                        double memoIntraP = (double)memoIntraPick*1.8f;
                        if ((double)rotationTimeOut.read_ms()> (memoIntraP)){
                            syncroCheck=0;
                            aspettaStart=1;
                            countCicli=0;
                            if (TBzeroCyclePulse==1){TBticker.detach();}
                        }
                    }else{  // fine ciclo fuori da low speed
                        syncroCheck=0;
                        lockStart=0;
                        if (beccoPronto==1){
                            if (tamburoStandard==1){
                                double ritardoMassimo = 0.0f;
                                if(speedFromPick==1){
                                    ritardoMassimo = (double)timeIntraPick;
                                }else{
                                    ritardoMassimo = (double)memoTimeHole;
                                }
                                int tempoDiSincro = (int)((double)(ritardoMassimo - ((tempoBecco/2.0f)+((speedOfSeedWheel/1.25f)*ritardoMassimo))));         //
                                if (tempoDiSincro <= 1){tempoDiSincro=1;}                                
                                if ((sincroTimer.read_ms()>= tempoDiSincro)){
                                    if (tractorSpeed_MtS_timed >= minWorkSpeed){startCicloTB=1;}
                                    beccoPronto=0;
                                }
                            }else{
                                // tamburo per zucca
                                if (speedOfSeedWheel >= minWorkSpeed){startCicloTB=1;}
                                beccoPronto=0;
                            }
                        }
                        ciclaTB();
                    }
                    //*************************************************************
                }else{  // fine ciclo con velocita maggiore di 0
                    SDwheelTimer.stop();
                    SDwheelTimer.reset();
                    checkSDrotation=0;
                    oldFaseLavoro=0;
                    aspettaStart=1;
                    countCicli=0;
                    startCycle=0;
                    oldSeedWheelPeriod=0.0f;
                    oldPeriodoTB=0.0f;
                    correzione=0.0f;
                    OLDpulseSpeedInterval=1000.01f;
                    cicloTbinCorso=0;
                    cntTbError=0;
                    if (cycleStopRequest==1){
                        zeroDelay.reset();
                        zeroDelay.start();
                        runRequestBuf=0;
                        zeroRequestBuf=1;
                        cycleStopRequest=0;
                        SDzeroCyclePulse=0;
                        TBzeroCyclePulse=0;
                        zeroCycleEnd=0;
                        zeroCycle=1;
                        zeroTrigger=0;
                    }
                }
    //*************************************************************************************************        
    // ciclo di azzeramento motori
            if ((zeroCycleEnd==0)&&(zeroCycle==1)){//&&(zeroDelay.read_ms()>10000)){
                if (zeroTrigger==0){
                    TBticker.attach_us(&step_TBPulseOut,1000.0f);  // clock time are seconds and attach seed motor stepper controls
                    DC_forward=1;
                    DC_brake=1;
                    DC_prepare();
                    frenata=0;
                    zeroTrigger=1;
                    ritardoStop.reset();
                    ritardoComandoStop.reset();
                    ritardoComandoStop.start();
                    timeOutZeroSD.start();
                    quincTimeSet.reset();
                }
                int tempoFrenata=300;
                if (((ritardoStop.read_ms()>tempoFrenata)&&(SDzeroDebounced==0))||(accensione==1)||(ritardoComandoStop.read_ms()>400)){
                    accensione=0;
                    avvicinamento=1;
                    avvicinamentoOn.reset();
                    avvicinamentoOn.start();
                    SDmotorPWM.write(0.32f);      // duty cycle = 60% of power
                    DC_forward=1;
                    DC_brake=0;
                    DC_prepare();
                    ritardoComandoStop.reset();
                    ritardoComandoStop.stop();
                }
                if (avvicinamento==1){
                    if(avvicinamentoOn.read_ms()>300){
                        SDmotorPWM.write(0.7f);
                        avvicinamentoOn.reset();
                        avvicinamentoOff.reset();
                        avvicinamentoOff.start();
                    }
                    if(avvicinamentoOff.read_ms()>100){
                        SDmotorPWM.write(0.32f);
                        avvicinamentoOff.reset();
                        avvicinamentoOff.stop();
                        avvicinamentoOn.start();
                    }
                }else{
                    avvicinamentoOn.stop();
                    avvicinamentoOff.stop();
                    avvicinamentoOn.reset();
                    avvicinamentoOff.reset();
                }
                if (frenata==0){
                    if (SDzeroCyclePulse==1){
                        SDticker.detach();
                        frenata=1;
                        quincTimeSet.reset();
                        quincTimeSet.start();
                        ritardoStop.start();
                        //ritardoComandoStop.reset();
                        //ritardoComandoStop.stop();
                    }
                }else{
                    if (quinconceActive==0){
                        if (SDzeroCyclePulse==1){
                            avvicinamento=0;
                            DC_brake=1;
                            DC_prepare();
                            SDzeroed=1;
                            ritardoStop.reset();
                            ritardoStop.stop();
                        }
                    }else{
                        if (quincTimeSet.read_ms()>500){
                            avvicinamento=0;
                            DC_brake=1;
                            DC_prepare();
                            SDzeroed=1;
                            ritardoStop.reset();
                            ritardoStop.stop();
                            quincTimeSet.stop();
                        }
                    }
                }
                // azzera tutto in time out
                if (timeOutZeroSD.read_ms()>=10000){
                    #if defined(pcSerial)
                        #if defined(canDataReceived)
                            pc.printf("allarme no zero");
                        #endif
                    #endif
                    if (firstStart==0){
                        all_no_Zeroing=1;
                    }
                    avvicinamento=0;
                    DC_brake=1;
                    DC_prepare();
                    SDzeroed=1;
                    ritardoStop.reset();
                    ritardoStop.stop();
                    avvicinamentoOn.stop();
                    avvicinamentoOff.stop();
                    avvicinamentoOn.reset();
                    avvicinamentoOff.reset();
                    ritardoComandoStop.reset();
                    ritardoComandoStop.stop();
                    timeOutZeroSD.stop();
                    timeOutZeroSD.reset();
                }
                if (TBzeroCyclePulse==1){
                    TBticker.detach();
                    TBzeroed=1;
                }
                if ((SDzeroed==1)&&(TBzeroed==1)){
                    avvicinamentoOn.stop();
                    avvicinamentoOff.stop();
                    ritardoComandoStop.stop();
                    ritardoStop.stop();
                    zeroCycleEnd=1;
                    zeroCycle=0;
                    zeroTrigger=0;
                    runRequestBuf=1;
                    zeroRequestBuf=0;
                    cycleStopRequest=0;
                    SDzeroed=0;
                    TBzeroed=0;
                    timeOutZeroSD.stop();
                    timeOutZeroSD.reset();
                }
            }
    
//*************************************************************************************************        
            if (enableCycle==0){
                zeroTrigger=0;
                SDmotorPWM=0;
                SDmotorInA=0;
                SDmotorInB=0;
            }
            SDzeroCyclePulse=0;
            TBzeroCyclePulse=0;
//*************************************************************************************************        
        }else{//end ciclo normale
//*************************************************************************************************        
//      task di prova della scheda
//*************************************************************************************************        
          #if defined(provaScheda)
            clocca++;
            //led = !led;
            //txMsg.clear();
            //txMsg << clocca;
            //test.printf("aogs \n");
            //if(can1.write(txMsg)){
                #if defined(pcSerial)
                    pc.printf("Can write OK \n");
                #endif
            //}
            switch (clocca){
                case 1:
                    TBmotorStepOut=1;       // define step command for up down motor driver
                    break;
                case 2:
                    SDmotorPWM=1;       // define step command for seeding whell motor driver
                    break;
                case 3:
                    speedClock=1;              // define input of 
                    break;
                case 4:
                    break;
                case 5:
                    SDmotorInA=1;
                    SDmotorInB=0;
                    break;
                case 6:
                    break;
                case 7:
                    break;
                case 8:
                    break;
                case 9:
                    break;
                case 10:
                    break;
                case 11:
                    break;
                case 12:
                    break;
                case 13:
                    break;
                case 14:
                    SDmotorPWM=1;            // power mosfet 2 command out
                    break;
                case 15:
                    break;
                case 16:
                case 17:
                    break;
                case 18:
                    TBmotorStepOut=0;       // define step command for up down motor driver
                    SDmotorPWM=0;           // define step command for seeding whell motor driver
                    speedClock=0;              // define input of 
                    SDmotorInA=0;
                    SDmotorInB=0;
                    SDmotorPWM=0;            // power mosfet 2 command out
                    break;                    
                default:
                    clocca=0;
                    break;
            }
            wait_ms(100);
          #endif // end prova scheda
            
          #if defined(provaDC)
            int rampa=1000;
            int pausa=3000;
            switch (clocca){
                case 0:
                    DC_brake=0;
                    DC_forward=1;
                    duty_DC+=0.01f;
                    if (duty_DC>=0.2f){
                        duty_DC=0.2f;
                        clocca=1;
                    }
                    wait_ms(rampa);
                    break;
                case 1:
                    wait_ms(pausa*4);
                    clocca=2;
                    break;
                case 2:
                    DC_brake=0;
                    DC_forward=1;
                    duty_DC-=0.01f;
                    if (duty_DC<=0.0f){
                        duty_DC=0.0f;
                        clocca=3;
                    }
                    wait_ms(rampa);
                    break;
                case 3:
                    wait_ms(pausa);
                    clocca=4;
                    break;
                case 4:
                    DC_brake=0;
                    DC_forward=1;
                    duty_DC+=0.01f;
                    if (duty_DC>=1.0f){
                        duty_DC=1.0f;
                        clocca=5;
                    }
                    wait_ms(rampa);
                    break;
                case 5:
                    wait_ms(pausa);
                    clocca=6;
                    break;
                case 6:
                    DC_brake=0;
                    DC_forward=1;
                    duty_DC-=0.01f;
                    if (duty_DC<=0.0f){
                        duty_DC=0.0f;
                        clocca=7;
                    }
                    wait_ms(rampa);
                    break;
                case 7:
                    wait_ms(pausa);
                    clocca=0;
                    break;
                default:
                    break;
            }
            if (oldDuty_DC != duty_DC){            
                SDmotorPWM.write(duty_DC);      // duty cycle = stop
                oldDuty_DC=duty_DC;
                DC_prepare();
            }
          #endif
        }//end  in prova
    } // end while
}// end main