Messa in campo 4 file - 26/06/2020 Francia
Dependencies: mbed X_NUCLEO_IHM03A1_for
Fork of FORIGO_Modula_V7_3_VdcStep_maggio2020 by
Diff: main.cpp
- Revision:
- 3:a469bbd294b5
- Parent:
- 2:35f13b7f3659
- Child:
- 4:de1b296e9757
--- a/main.cpp Thu Nov 23 13:09:25 2017 +0000 +++ b/main.cpp Wed Feb 13 07:18:01 2019 +0000 @@ -1,31 +1,2294 @@ -#include "mbed.h" + +//******************************************************************************************************************** +//******************************************************************************************************************** +// FIRMWARE SEMINATRICE MODULA +// VERSIONE PER SCHEDA DI CONTROLLO CON DRIVER INTEGRATI +// V7 - ATTENZIONE - LA VERSIONE V7 HA IL DRIVER STEPPER POWERSTEP01 DA 10A +// IL PROCESSORE UTILIZZATO E' IL STM32L476RG A 80MHz +// 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: 20 DECEMBER 2018 +// +// APPLICATION: MODULA CON DISTRIBUTORE RISO ED ENCODER MOTORE +// +// 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 +// 09 06 2018 - INSERITO CONTROLLO DI FASE CON ENCODER MASTER PER QUINCONCE - DATO SCAMBIATO IN CAN +// 03 01 2019 - INSERITA GESTIONE IN RTOS PER IL DRIVER POWERSTEP01 +/******************** +IL FIRMWARE SI COMPONE DI 10 FILES: + - main.cpp + - main.hpp + - iodefinition.hpp + - canbus.hpp + - parameters.hpp + - timeandtick.hpp + - variables.hpp + - powerstep.hpp + - watchdog.cpp + - watchdog.h +ED UTILIZZA LE LIBRERIE STANDARD MBED PIU' +UNA LIBRERIA MODIFICATA E DEDICATA PER IL CAN +UNA LIBRERIA DEDICATA PER IL DRIVER STEPPER +********************* +LA MACCHINA UTILIZZA SEMPRE 2 SOLI SENSORI; UNO PER SENTIRE LE CELLE DI CARICO SEME ED UNO PER SENTIRE I BECCHI DI SEMINA. +GLI AZIONAMENTI SONO COMPOSTI DA DUE MOTORI; UN DC PER IL CONTROLLO DELLA RUOTA DI SEMINA ED UNO STEPPER PER IL CONTROLLO DEL TAMBURO +UN SENSORE AGGIUNTIVO SULL'ELEMENTO MASTER RILEVA LA VELOCITA' DI AVANZAMENTO +UN SENSORE AGGIUNTIVO SULLA RUOTA DI SEMINA RILEVA LA ROTAZIONE DELLA RUOTA STESSA ATTRAVERSO FORI PRESENTI SUL DISCO DI SEMINA +********************* +LA LOGICA GENERALE PREVEDE CHE IL DC DELLA RUOTA DI SEMINA VENGA COMANDATO IN FUNZIONE DELLA VELOCITA' LETTA DAL SENSORE DI AVANZAMAENTO DEL MASTER +IL PROBLEMA PRINCIPALE E' CHE QUANDO I BECCHI SONO INSERITI NEL TERRENO NON VI E' RETROAZIONE REALE SULLA VELOCITA' DI ROTAZIONE DELLA RUOTA STESSA +PROPRIO PERCHE' L'AVANZAMANETO NEL TERRENO IMPRIME UNA VELOCITA' PROPRIA AL BECCO E QUINDI ANCHE ALLA RUOTA. +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 +IL VALORE DI RIFERIMENTO DELL'ANALOGICA DI INGRESSO VIENE AGGIORNATO OGNI VOLTA CHE LA RUOTA DI SEMINA E' FERMA +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 +LA FASE VIENE DETERMINATA DAL PASSAGGIO DEI BECCHI SUL SENSORE RELATIVO. +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 +SOPRATUTTO QUANDO I BECCHI SONO MOLTO DISTANZIATI. +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 +TARATO SULLA REALE VELOCITA' DI ROTAZIONE DELLA RUOTA DI SEMINA. +IL PRIMO ENCODER VIRTUALE SI OCCUPA DI DETERMINARE LA POSIZIONE FISICA DELLA RUOTA DI SEMINA E SI AZZERA AL PASSAGGIO DI OGNI BECCO. +IL SECONDO VIENE AZZERATO DALL'IMPULSO DI FASE DEL PRIMO ENCODER DETERMINATO DAI VALORI IMPOSTI SUL TERMINALE TRITECNICA +IL SECONDO ENCODER VIENE CONFRONTATO CON LA POSIZIONE ASSOLUTA DEL TAMBURO (DETERMINATA DAL NUMERO DI STEP EMESSI DAL CONTROLLO), RAPPORTATA TRA CELLE E BECCHI. +IL CONFRONTO DETERMINA LA POSIZIONE RELATIVA DELLA SINGOLA CELLA RISPETTO AL SINGOLO BECCO. IL MANTENIMENTO DELLA SINCRONIZZAZIONE DI FASE, DETERMINA IL SINCRO CELLA/BECCO. +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 +DI CAMBIARE L'ANGOLO DI ANTICIPO DI RILASCIO DEL SEME IN FUNZIONE DELLA VELOCITA' E RECUPERARE COSI' IL TEMPO DI VOLO DEL SEME. +IL TAMBURO HA DUE TIPI DI FUNZIONAMENTO: CONTINUO E AD IMPULSI. E' SELEZIONABILE IN FUNZIONE DELLA VELOCITA' E DEL TIPO DI DISTRIBUTORE MONTATO. +********************** +TUTTI I VALORI, CELLE, BECCHI, IMPULSI VELOCITA', ANCGOLO DI AVVIO, FASE DI SEMINA, ECC.. SONO IMPOSTABILI DA PANNELLO OPERATORE +I DATI SONO SCAMBIATI CON IL PANNELLO OPERATORE E CON GLI ALTRI MODULI ATTRAVERSO RETE CAN CON PROTOCOLLO FREESTYLE ATTRAVERSO INDIRIZZAMENTI DEDICATI +AL MOMENTO NON E' POSSIBILE ATTRIBUIRE L'INIDIRIZZO BASE DELL'ELEMENTO DA TERMINALE OPERATORE MA SOLO IN FASE DI COMPILAZIONE DEL FIRMWARE. +********************** +ALTRE SEZIONI RIGUARDANO LA GENERAZIONE DEGLI ALLARMI, LA COMUNICAZIONE CAN, LA SIMULAZIONE DI LAVORO, LA GESTIONE DELLA DIAGNOSI ECC.. +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. +LO STEPPER SEGUE IL DC. +********************** +IN FASE DI ACCENSIONE ED OGNI QUALVOLTA SI ARRIVA A VELOCITA' ZERO, LA MACCHINA ESEGUE UN CICLO DI AZZERAMENTO +NON ESISTE PULSANTE DI MARCIA/STOP; E' SEMPRE ATTIVA. +********************** +NEL PROGRAMMA E' PRESENTE UNA SEZIONE DI TEST FISICO DELLA SCHEDA ATTIVABILE SOLO IN FASE DI COMPILAZIONE +********************** +ALTRE FUNZIONI: PRECARICAMENTO DEL TAMBURO + AZZERAMENTO MANUALE + STATISTICA DI SEMINA (CONTA LE CELLE) +*/ +//******************************************************************************************************************** +//******************************************************************************************************************** +#include "main.hpp" +#include "timeandtick.hpp" +#include "canbus.hpp" +#include "watchdog.h" +#include "iodefinition.hpp" +#include "parameters.hpp" +#include "variables.hpp" +//******************************************************************************************************************** +//******************************************************************************************************************** + +//Thread thread; + +/* Variables -----------------------------------------------------------------*/ + +/* Functions -----------------------------------------------------------------*/ + +/** + * @brief This is an example of user handler for the flag interrupt. + * @param None + * @retval None + * @note If needed, implement it, and then attach and enable it: + * + motor->attach_flag_irq(&my_flag_irq_handler); + * + motor->enable_flag_irq(); + * To disable it: + * + motor->DisbleFlagIRQ(); + */ +void my_flag_irq_handler(void) +{ + /* Set ISR flag. */ + motor->isrFlag = TRUE; + /* Get the value of the status register. */ + unsigned int statusRegister = motor->get_status(); + #if defined(pcSerial) + printf(" WARNING: \"FLAG\" interrupt triggered.\r\n"); + #endif + /* Check SW_F flag: if not set, the SW input is opened */ + if ((statusRegister & POWERSTEP01_STATUS_SW_F ) != 0) { + #if defined(pcSerial) + printf(" SW closed (connected to ground).\r\n"); + #endif + } + /* Check SW_EN bit */ + if ((statusRegister & POWERSTEP01_STATUS_SW_EVN) == POWERSTEP01_STATUS_SW_EVN) { + #if defined(pcSerial) + printf(" SW turn_on event.\r\n"); + #endif + } + /* Check Command Error flag: if set, the command received by SPI can't be */ + /* performed. This occurs for instance when a move command is sent to the */ + /* Powerstep01 while it is already running */ + if ((statusRegister & POWERSTEP01_STATUS_CMD_ERROR) == POWERSTEP01_STATUS_CMD_ERROR) { + #if defined(pcSerial) + printf(" Non-performable command detected.\r\n"); + #endif + } + /* Check UVLO flag: if not set, there is an undervoltage lock-out */ + if ((statusRegister & POWERSTEP01_STATUS_UVLO)==0) { + #if defined(pcSerial) + printf(" undervoltage lock-out.\r\n"); + #endif + } + /* Check thermal STATUS flags: if set, the thermal status is not normal */ + if ((statusRegister & POWERSTEP01_STATUS_TH_STATUS)!=0) { + //thermal status: 1: Warning, 2: Bridge shutdown, 3: Device shutdown + #if defined(pcSerial) + printf(" Thermal status: %d.\r\n", (statusRegister & POWERSTEP01_STATUS_TH_STATUS)>>11); + #endif + } + /* Check OCD flag: if not set, there is an overcurrent detection */ + if ((statusRegister & POWERSTEP01_STATUS_OCD)==0) { + #if defined(pcSerial) + printf(" Overcurrent detection.\r\n"); + #endif + } + /* Reset ISR flag. */ + motor->isrFlag = FALSE; +} + +/** + * @brief This is an example of error handler. + * @param[in] error Number of the error + * @retval None + * @note If needed, implement it, and then attach it: + * + motor->attach_error_handler(&my_error_handler); + */ +void my_error_handler(uint16_t error) +{ + /* Printing to the console. */ + #if defined(pcSerial) + printf("Error %d detected\r\n\n", error); + #endif + + /* Infinite loop */ + //while (true) { + //} +} + +//******************************************************************************* +// FREE RUNNING RTOS THREAD FOR DRUM STEPPER POSITION READING +//******************************************************************************* +void step_Reading(){ + while(true){ + /* Get current position of device and print to the console */ + #if defined(pcSerial) + #if defined(rtosData) + printf(" Position: %d.\r\n", motor->get_position()); + #endif + #endif + TBpassPosition= (uint32_t) motor->get_position(); + if (TBpassPosition > TBoldPosition){ + //TBactualPosition= ((TBpassPosition-TBoldPosition)*TBreductionRatio);//*10; + }else{ + //TBactualPosition=((((2097152-TBoldPosition)+TBpassPosition))*TBreductionRatio);//*10; + } + //wait_us(50); // 50 mS di intervallo lettura + } +} +//******************************************************************************* +//******************************************************************************************************************** +// ------------------------------------------------------------------------------------------------------------------------------------------------------------------ +// 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 + } +} +// rise of seed speed motor encoder +void encoRise(){ + timeHole=metalTimer.read_us(); + int memo_TimeHole= (memoTimeHole + timeHole)/ (int)2; + memoTimeHole = timeHole; + metalTimer.reset(); + if (encoder==true){ + speedOfSeedWheel=((seedPerimeter/((SDreductionRatio*25.5f)))/(double)memo_TimeHole)*1000000.0f; //mtS + pulseRised2=1; + //SDactualPosition+=SDstepEnco; + } +} +// rise of seed presence sensor +void seedSensorTask(){ + seedSee=1; +} +//************************************************** +// generate speed clock when speed is simulated from Tritecnica display +void speedSimulationClock(){ + lastPulseRead=speedTimer.read_us(); + oldLastPulseRead=lastPulseRead; + speedTimer.reset(); + pulseRised=1; + speedFilter.reset(); +} +//******************************************************* +// interrupt task for tractor speed reading +//******************************************************* +void tractorReadSpeed(){ + if ((oldTractorSpeedRead==0)){ + lastPulseRead=speedTimer.read_us(); + oldLastPulseRead=lastPulseRead; + speedTimer.reset(); + pulseRised=1; + oldTractorSpeedRead=1; + //spazioCoperto+= pulseDistance; + } + 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; +} -void print_char(char c = '*') -{ - printf("%c", c); - fflush(stdout); +//******************************************************* +// clocked task for manage virtual encoder of seed wheel i/o +//******************************************************* +//******************************************************* +void step_SDPulseOut(){ + SDactualPosition++; + prePosSD++; + #if defined(speedMaster) + posForQuinc++; + #endif +} +//******************************************************* +void step_TBPulseOut(){ + TBmotorStepOut=!TBmotorStepOut; + if (TBmotorStepOut==0){ + if (TBmotorDirecti==TBforward){ + TBactualPosition++; + //}else{ + // TBactualPosition--; + } + } +} +//******************************************************* +void inverti(){ + if (TBmotorDirecti==TBreverse){ + TBmotorDirecti=TBforward; + motor->step_clock_mode_enable(StepperMotor::FWD); + }else{ + TBmotorDirecti=TBreverse; + motor->step_clock_mode_enable(StepperMotor::BWD); + } + #if defined(pcSerial) + #if defined(inversione) + pc.printf("cambio M %d\n",cambiaStep); + pc.printf("posizione %d \n",TBactualPosition); + #endif + #endif +} +//******************************************************* +// 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_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); + TBgiroStep = TBmotorSteps*TBreductionRatio; + K_TBfrequency = TBgiroStep/60.0f; // 1600 * 1.65625f /60 = 44 44,00 + if (speedFromPick==1) { + intraPickDistance = seedPerimeter/pickNumber; + }else{ + intraPickDistance = seedPerimeter/25.0f; // 25 è il numero di fori presenti nel disco di semina + } + SDstepEnco = 9000/(25.5*SDreductionRatio); +} +//******************************************************* +void cambiaTB(double perio){ + // update TB frequency + double limite=500.0f; + double TBper=0.0f; + if (aspettaStart==0){ + if (perio<limite){perio=limite;} + double scala =0.0f; + if (lowSpeed==1){ + scala =2.0f; + }else{ + scala =2.0f; + } + TBper=perio/scala; + if (oldPeriodoTB!=TBper){ + if (TBper >= (limite/2.0f)){ + TBticker.attach_us(&step_TBPulseOut,TBper); // clock time are milliseconds and attach seed motor stepper controls + }else{ + TBticker.detach(); + motor->soft_hiz(); + } + 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); + */ +#if defined(Zucca) + if ((tractorSpeed_MtS_timed>0.01f)){ + if (inhibit==0){ + double posError =0.0f; + double posSD=((double)SDactualPosition)/KcorT; + //double posSD=((double)SDactualPosition); + 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))); + if (ePpos>0.0f){ + cambiaTB(ePpos); + }else{ + cambiaTB(periodo);///2.0f); + } + #if defined(pcSerial) + #if defined(TBperS) + pc.printf("TBpos: %f SDpos: %f Err: %f Correggi: %f\n",(double)TBactualPosition,posSD,posError,ePpos); + #endif + #endif + } + } + } + } +#else + 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)){ + double lowLim=-500.0f; + double higLim= 500.0f; + if (posError>higLim){posError=higLim;} + if (posError<lowLim){posError=lowLim;} + if ((posError >=1.0f)||(posError<=-1.0f)){ + ePpos = periodo /(1.0f+ ((posError/25.0f))); + cambiaTB(ePpos); + } + #if defined(pcSerial) + #if defined(TBperS) + pc.printf("TBpos: %f SDpos: %f SDact: %f Err: %f Correggi: %f\n",(double)TBactualPosition,posSD,(double)SDactualPosition,posError,ePpos); + #endif + #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; + #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 ciclaTB(){ + if ((startCicloTB==1)&&(cicloTbinCorso==0)){ + motor->step_clock_mode_enable(StepperMotor::FWD); + 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(); + motor->soft_hiz(); + cicloTbinCorso = 0; + stopCicloTB=0; + loadDaCanInCorso=0; + loadDaCan=0; + } +} +// ------------------------------------------------------------------------------------------------------------------------------------------------------------------ +void stepSetting(){ + // Stepper driver init and set + TBmotorRst=0; // reset stepper driver + TBmotorDirecti=TBforward; // 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=1; + TBmotor_M2=1; + TBmotor_M3=1; + }else if (TBmotorSteps==1600){ + TBmotor_M1=0; + TBmotor_M2=1; + TBmotor_M3=1; + }else if (TBmotorSteps==3200){ + TBmotor_M1=1; + TBmotor_M2=0; + TBmotor_M3=1; + }else if (TBmotorSteps==6400){ + TBmotor_M1=0; + TBmotor_M2=0; + TBmotor_M3=1; + }else if (TBmotorSteps==12800){ + TBmotor_M1=1; + TBmotor_M2=1; + TBmotor_M3=0; + }else if (TBmotorSteps==25600){ + TBmotor_M1=0; + TBmotor_M2=1; + TBmotor_M3=0; + }else if (TBmotorSteps==2000){ + TBmotor_M1=1; + TBmotor_M2=0; + TBmotor_M3=0; + }else if (TBmotorSteps==4000){ + TBmotor_M1=0; + TBmotor_M2=0; + TBmotor_M3=0; + }*/ + TBmotorRst=1; +} +//**************************************** +void dcSetting(){ + if ((speedFromPick==0)&&(encoder==false)){ + DcEncoder.rise(&sd25Fall); + } + if (encoder==true){ + DcEncoder.rise(&encoRise); + //ElementPosition.fall(&encoRise); + } +} +//******************************************************* +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); // fatto + alarmLowRegister=alarmLowRegister+(all_overCurrDC*0x10); // fatto + alarmLowRegister=alarmLowRegister+(all_stopSistem*0x20); // verificarne la necessità + //alarmLowRegister=alarmLowRegister+(all_upElements*0x40); // manca il sensore + if (seedSensorEnable==true){ + 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_lowBattery==1){pc.printf("AllarmeBassaCorrente\n");} + if (all_overCurrDC==1){pc.printf("AllarmeAltaCorrente\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; +} +//******************************************************* +#if defined(speedMaster) + void upDateSincro(){ + char val1[8]={0,0,0,0,0,0,0,0}; + val1[3]=(posForQuinc /0x00FF0000)&0x000000FF; + val1[2]=(posForQuinc /0x0000FF00)&0x000000FF; + val1[1]=(posForQuinc /0x000000FF)&0x000000FF; + val1[0]=posForQuinc & 0x000000FF; + //double pass = tractorSpeed_MtS_timed*100.0f; + double pass = speedOfSeedWheel*100.0f; + val1[4]=(uint8_t)(pass)&0x000000FF; + val1[5]=(prePosSD /0x0000FF00)&0x000000FF; + val1[6]=(prePosSD /0x000000FF)&0x000000FF; + val1[7]=prePosSD & 0x000000FF; + #if defined(canbusActive) + #if defined(speedMaster) + if(can1.write(CANMessage(TX_SI, *&val1,8))){ + checkState=0; + } + #endif + #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]+(TBzeroPinInput*0x02); + val1[3]=val1[3]+(DcEncoder*0x04); + val1[3]=val1[3]+(seedWheelZeroPinInput*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.009f); + canUpdate.attach(&upDateSpeed,0.21f); + #else + canUpdate.attach(&upDateSpeed,0.407f); + #endif + firstStart=0; + } + + if (rxMsg.id==RX_ID){ + #if defined(pcSerial) + #if defined(canDataReceiveda) + pc.printf("Messaggio ricevuto\n"); + #endif + #endif + } + if (rxMsg.id==RX_Broadcast){ + #if defined(pcSerial) + #if defined(canDataReceivedb) + 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("Speed simula %d \n",speedSimula); + #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(canDataReceivedR) + 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 (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]; + quincPIDminus = (uint8_t) rxMsg.data[1]; + quincPIDplus = (uint8_t) rxMsg.data[2]; + quincLIMminus = (uint8_t) rxMsg.data[3]; + quincLIMplus = (uint8_t) rxMsg.data[4]; + quincSector = (uint8_t) rxMsg.data[5]; + aggiornaParametri(); + } + } + if (rxMsg.id==RX_QuincSinc){ + masterSinc = (uint32_t) rxMsg.data[3] * 0x00FF0000; + masterSinc = masterSinc + ((uint32_t) rxMsg.data[2] * 0x0000FF00); + masterSinc = masterSinc + ((uint32_t) rxMsg.data[1] * 0x000000FF); + masterSinc = masterSinc + ((uint32_t) rxMsg.data[0]); + speedFromMaster = (double)rxMsg.data[4]/100.0f; + mast2_Sinc = ((uint32_t) rxMsg.data[5] * 0x0000FF00); + mast2_Sinc = mast2_Sinc + ((uint32_t) rxMsg.data[6] * 0x000000FF); + mast2_Sinc = mast2_Sinc + ((uint32_t) rxMsg.data[7]); + canDataCheck=1; + } + if (rxMsg.id==RX_Configure){ + uint8_t flags = rxMsg.data[0]; + uint16_t steps = (uint32_t) rxMsg.data[2]*0x00000100; + steps = steps + ((uint32_t)rxMsg.data[1]); + TBmotorSteps =steps; + //stepSetting(); + cellsCountSet = rxMsg.data[3]; + if ((flags&0x01)==0x01){ + if (encoder==false){ + encoder=true; + DcEncoder.rise(NULL); + dcSetting(); + } + }else{ + if (encoder==true){ + encoder=false; + DcEncoder.rise(NULL); + dcSetting(); + } + } + if ((flags&0x02)==0x02){ + tankLevelEnable=true; + }else{ + tankLevelEnable=false; + } + if ((flags&0x04)==0x04){ + seedSensorEnable=true; + }else{ + seedSensorEnable=false; + } + if ((flags&0x08)==0x08){ + stendiNylonEnable=true; + }else{ + stendiNylonEnable=false; + } + if ((flags&0x10)==0x10){ + canDataCheckEnable=true; + }else{ + canDataCheckEnable=false; + } + if ((flags&0x20)==0x20){ + tamburoStandard=1; + }else{ + tamburoStandard=0; + } + if ((flags&0x40)==0x40){ + currentCheckEnable=true; + }else{ + currentCheckEnable=false; + } + } + } + #endif + #if defined(overWriteCanSimulation) + enableSimula=1; + speedSimula=25; + avviaSimula=1; + simOk=1; + #endif + } -Thread thread; - -DigitalOut led1(LED1); +//******************************************************* +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.00 e 1.00 + 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("CorrenteStart: %f ",SD_CurrentStart); + pc.printf(" CorrenteScaled: %f ",SD_CurrentScaled); + pc.printf(" CorrenteMedia: %f \n",SD_CurrentAnalogica); + #endif + #endif + reduceCurrent=false; + incrementCurrent=false; + /* + if (SD_CurrentScaled < 3.0f){ + #if defined(canbusActive) + all_lowBattery=1; + #endif + incrementCurrent=true; + } + */ + if (SD_CurrentScaled > 10.0f){ + timeCurr.start(); + if (timeCurr.read() > 5.0f){ + #if defined(canbusActive) + all_overCurrDC=1; + #endif + reduceCurrent=true; + timeCurr.reset(); + } + }else{ + timeCurr.stop(); + } + //} +} +//******************************************************* +void DC_prepare(){ + // direction or brake preparation + if (DC_brake==1){ + SDmotorInA=1; + SDmotorInB=1; + }else{ + if (DC_forward==0){ + 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 quincTrigon(){ + quincClock=true; +} +void quincTrigof(){ + quincClock=false; +} +//******************************************************* +void quinCalc(){ + // riceve l'impulso di sincro dal master e fa partire il timer di verifica dell'errore + #if !defined(mezzo) + if ((quincClock==true)&&(oldQuincIn==0)){ + oldQuincIn=1; + if (quincStart==0){ + oldQuincTimeSD = (double) quincTimeSD.read_ms(); + quincTime.reset(); + quincTimeSD.reset(); + quincStart=1; + } + } + if(quincClock==false){ + oldQuincIn=0; + } + #else + if ((((quinconceActive==0)&&(quincClock==true))||((quinconceActive==1)&&(quincClock==false)))&&(oldQuincIn==0)){ + oldQuincIn=1; + if (quincStart==0){ + oldQuincTimeSD = (double) quincTimeSD.read_ms(); + quincTime.reset(); + quincStart=1; + } + } + if (quinconceActive==0){ + if (quincClock==false){ + oldQuincIn=0; + } + }else{ + if (quincClock==true){ + oldQuincIn=0; + } + } + #endif + //**************************************************************************************** + if (quincCnt>=4){ + if (countPicks==0){ + if ((sincroQui==1)&&(quincStart==0)){ + // decelera + countPicks=1; + } + if ((sincroQui==0)&&(quincStart==1)){ + // accelera + countPicks=2; + } + } + if ((sincroQui==1)&&(quincStart==1)){ + if (countPicks==1){ //decelera + scostamento = oldQuincTimeSD; + if (scostamento < (tempoBecchiPerQuinc*0.75f)){ + double scostPerc = (scostamento/tempoBecchiPerQuinc); + percento -= ((double)quincPIDminus/100.0f)*(scostPerc); + #if defined(pcSerial) + #if defined(laq) + pc.printf("RALL scos2: %f tbpq: %f percento: %f \n",scostamento,tempoBecchiPerQuinc,percento); + #endif + #endif + } + //if (scostamento <15.0f){percento=0.0f;} + } + if (countPicks==2){ //accelera + scostamento = (double)quincTime.read_ms(); + if (scostamento < (tempoBecchiPerQuinc*0.75f)){ + double scostPerc = (scostamento/tempoBecchiPerQuinc); + percento += ((double)quincPIDplus/100.0f)*(scostPerc); + #if defined(pcSerial) + #if defined(laq) + pc.printf( + "ACCE scos1: %f tbpq: %f percento: %f \n",scostamento,tempoBecchiPerQuinc,percento); + #endif + #endif + } + //if (scostamento <15.0f){percento=0.0f;} + } + sincroQui=0; + quincStart=0; + countPicks=0; + #if !defined(speedMaster) + if (quincCnt>=3){ + if (speedFromMaster>0.0f){ + if (enableSimula==0){ + tractorSpeed_MtS_timed = speedFromMaster + percento; + } + } + } + #endif + } + + //******************************************************************* + if (canDataCheckEnable==true){ + if (canDataCheck==1){ // sincro da comunicazione can del valore di posizione del tamburo master + canDataCheck=0; + double parametro = SDsectorStep/3.0f; + double differenza=0.0f; + #if defined(mezzo) + if (quinconceActive==1){ + differenza = (double)masterSinc - (double)prePosSD; + }else{ + differenza = (double)mast2_Sinc - (double)prePosSD; + } + #else + differenza = (double)mast2_Sinc - (double)prePosSD; + #endif + if ((differenza > 0.0f)&&(differenza < parametro)){ + double diffPerc = differenza / parametro; + percento += ((double)quincPIDplus/100.0f)*abs(diffPerc); + #if defined(pcSerial) + #if defined(quinca) + pc.printf("m1 %d m2 %d prePo %d diffe %f perce %f parm %f %\n",masterSinc, mast2_Sinc,prePosSD,differenza,percento, parametro); + #endif + #endif + } + if ((differenza < 0.0f)&&(abs(differenza) < parametro)){ + double diffPerc = (double)differenza / parametro; + percento -= ((double)quincPIDminus/100.0f)*abs(diffPerc); + #if defined(pcSerial) + #if defined(quinca) + pc.printf("m1 %d m2 %d prePo %d diffe %f perce %f parm %f %\n",masterSinc, mast2_Sinc,prePosSD,differenza,percento, parametro); + #endif + #endif + } -void print_thread() -{ - while (true) { - wait(1); - print_char(); + #if !defined(speedMaster) + if (quincCnt>=3){ + if (speedFromMaster>0.0f){ + //Questo + if (enableSimula==0){ + tractorSpeed_MtS_timed = speedFromMaster + percento; + } + } + } + #endif + } + } + + } + if ((percento) > ((double) quincLIMplus/100.0f)){ + percento= (double)quincLIMplus/100.0f; + } + if ((percento) < (((double)quincLIMminus*-1.0f)/100.0f)){ + percento=((double)quincLIMminus*-1.0f)/100.0f; } } +// ---------------------------------------- +#if defined(seedSensor) + void resetDelay(){ + delaySeedCheck.reset(); + delaySeedCheck.stop(); + } +#endif +// ------------------------------------------------------------------------------------------------------------------------------------------------------------------ +// MAIN SECTION +// --------------------------------------------------------------------------------------------------------------------------------------------------------------- -int main() -{ - printf("\n\n*** RTOS basic example ***\n"); +//******************************************************************************* +int main(){ + //wait(1.0f); + wd.Configure(2); //watchdog set at xx seconds + + stepSetting(); + + for (int a=0; a<5;a++){ + mediaSpeed[a]=0; + } + + + // 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(); + quincTimeSD.start(); + //intraEncoTimer.start(); + + //******************************************************* + // controls for check DC motor current + pwmCheck.rise(&startDelay); + wait_ms(500); + + TBmotor_SW=1; +//----- Initialization + /* Initializing SPI bus. */ + // dev_spi(mosi,miso,sclk) + // D11= PA7; D12= PA6; D13= PA5 + DevSPI dev_spi(D11, D12, D13); + + /* Initializing Motor Control Component. */ + // powerstep01( flag, busy,stby, stck, cs, dev_spi) + // motor = new PowerStep01(D2, D4, D8, D9, D10, dev_spi); // linea standard per IHM03A1 + motor = new PowerStep01(PA_8, PC_7, PC_4, PB_3, PB_6, dev_spi); // linea per scheda seminatrice V7 + if (motor->init(&init) != COMPONENT_OK) { + exit(EXIT_FAILURE); + } + + /* Attaching and enabling an interrupt handler. */ + motor->attach_flag_irq(&my_flag_irq_handler); + motor->enable_flag_irq(); + //motor->disable_flag_irq(); + + /* Attaching an error handler */ + motor->attach_error_handler(&my_error_handler); + + //thread.start(step_Reading); + + if (inProva==0){ + tractorSpeedRead.rise(&tractorReadSpeed); + #if !defined(speedMaster) + quinconceIn.rise(&quincTrigon); + quinconceIn.fall(&quincTrigof); + #endif + #if defined(speedMaster) + tftUpdate.attach(&videoUpdate,0.50f); + #endif + seedCorrection.attach(&seedCorrect,0.010f); // con 16 becchi a 4,5Kmh ci sono 37mS tra un becco e l'altro, quindi 8 correzioni di tb + dcSetting(); + #if defined(seedSensor) + seedCheck.fall(&seedSensorTask); + #endif + }else{ + tftUpdate.attach(&videoUpdate,0.125f); + } + + aggiornaParametri(); + + SDmotorPWM.period_us(periodoSD); // frequency 1KHz pilotaggio motore DC + SDmotorPWM.write(1.0f); // duty cycle = stop + TBmotorDirecti=TBforward; // tb motor direction set + + #if defined(provaStepper) + TBmotorRst=1; + TBmotorDirecti=TBforward; + // definire il pin di clock che è PB_3 + motor->step_clock_mode_enable(StepperMotor::FWD); + // attiva l'out per il controllo dello stepper in stepClockMode + DigitalOut TBmotorStepOut(PB_3); // PowerStep01 Step Input + TBticker.attach_us(&step_TBPulseOut,500.0f); // clock time are seconds and attach seed motor stepper controls + TATicker.attach(&inverti,2.0f); + #else + // definire il pin di clock che è PB_3 + motor->step_clock_mode_enable(StepperMotor::FWD); + // attiva l'out per il controllo dello stepper in stepClockMode + DigitalOut TBmotorStepOut(PB_3); // PowerStep01 Step Input + #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 + #if !defined(simulaBanco) + seedWheelZeroPinInput = !seedWheelZeroPinInputRev; + #else + if ((prePosSD-500) >= SDsectorStep){ + seedWheelZeroPinInput=1; + } + if ((prePosSD > 500)&&(prePosSD<580)){ + seedWheelZeroPinInput=0; + } + #endif + TBzeroPinInput = !TBzeroPinInputRev; + + // se quinconce attivo ed unita' master invia segnale di sincro + #if defined(speedMaster) + if (seedWheelZeroPinInput==1){ + quinconceOut=0; + } + if (((double)(prePosSD-500) >= (SDsectorStep/((double)quincSector)))&&(quinconceOut=1)){ + quinconceOut=1; + } + if (quinconceActive==1){ + if ((quinconceOut==1)&&(oldQuinconceOut==1)){ + posForQuinc=500; + oldQuinconceOut=0; + } + if (((double)posForQuinc-500.0f)> (SDsectorStep-200)){ + oldQuinconceOut=1; + } + } + #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){ + #if defined(pcSerial) + #if defined(canDataReceived) + pc.printf("Pulseinterval %f \n",pulseSpeedInterval); + #endif + #endif + 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; + } + - thread.start(print_thread); +//************************************************************************************************************** +//************************************************************************************************************** +// 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)&&(oldSeedWheelZeroPinInput==1)){ + if(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; + quincTime.reset(); + quincTimeSD.reset(); + SDzeroDebounced=1; + sincroQui=1; + SDwheelTimer.reset(); + #if defined(speedMaster) + if (quinconceActive==0){ + posForQuinc=500; + } + #endif + if (quincCnt<10){ + quincCnt++; + } + if ((aspettaStart==0)&&(lowSpeed==1)){ + beccoPronto=1; + } + SDzeroCyclePulse=1; + lockStart=0; + double fase1=0.0f; + forzaFase=0; + double limite=fixedStepGiroSD/pickNumber; + if (tamburoStandard==0){ + fase1=TBdeltaStep; + }else{ + if(speedForCorrection >= speedOfSeedWheel){ + fase1=TBdeltaStep; + }else{ + //fase1=(TBdeltaStep)-(((speedOfSeedWheel-speedForCorrection)/maxWorkSpeed)*(TBfaseStep)); + fase1=(TBdeltaStep)-(((speedOfSeedWheel)/maxWorkSpeed)*(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)&&(encoder==false)){ + speedOfSeedWheel=((seedPerimeter / pickNumber)/timeIntraPick)*1000.0f; + #if defined(pcSerial) + #if defined(Qnca) + pc.printf("perim: %f pickN: %f sped: %f\n", seedPerimeter, pickNumber,speedOfSeedWheel); + #endif + #endif + } + if (encoder==false){ + pulseRised2=1; + } + #if defined(speedMaster) + if ((tractorSpeed_MtS_timed==0.0f)&&(zeroCycle==0)&&(zeroCycleEnd==1)){ + if ((firstStart==0)&&(simOk==0)&&(enableSimula==0)&&(zeroCycleEnd==1)){ + all_noSpeedSen=1; + } + } + double oldLastPr = (double)oldLastPulseRead*1.5f; + if((double)speedTimeOut.read_us()> (oldLastPr)){ + if ((firstStart==0)&&(simOk==0)&&(enableSimula==0)&&(zeroCycleEnd==1)){ + all_speedError =1; + } + } + #endif + //******************************************* + // esegue calcolo clock per la generazione della posizione teorica + // la realtà in base al segnale di presenza del becco + realGiroSD = seedPerimeter / speedOfSeedWheel; + 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 + TBrpm = seedWheelRPM*rapportoRuote; // 5.896 31,75 + 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 + //TBperiod=5.681818f*TBrpm; //prova dopo test con contagiri + //TBperiod=2.0f*TBrpm; //prova dopo test con contagiri + } +// ---------------------------------------- +// 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; + } +// ---------------------------------------- +// 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 (cntTbError>0){ + cntCellsCorrect++; + } + if (cntCellsCorrect>3){ + cntTbError=0; + cntCellsCorrect=0; + } + // conteggio celle erogate + if (cellsCounterLow < 0xFF){ + cellsCounterLow++; + }else{ + cellsCounterHig++; + cellsCounterLow=0; + } + // ciclo conteggio celle per carico manuale + if (loadDaCanInCorso==1){ + cntCellsForLoad++; + if (cntCellsForLoad >= 5){ + stopCicloTB=1; + cntCellsForLoad=0; + } + }else{ + cntCellsForLoad=0; + } + // inibizione controllo di sincro per fuori fase + if (trigSD==0){ + inhibit=1; + trigTB=1; + }else{ + inhibit=0; + trigTB=0; + trigSD=0; + } + // conta le celle indietro per sbloccare il tamburo + if ((TBmotorDirecti==TBreverse)&&(erroreTamburo==1)){ + cntCellsForReload++; + if (cntCellsForReload >= cellsCountSet){ + TBmotorDirecti=TBforward; // rotazione normale + motor->step_clock_mode_enable(StepperMotor::FWD); + erroreTamburo=0; + cntCellsCorrect=0; + } + } + #if defined(seedSensor) + resetDelay(); + delaySeedCheck.start(); + #endif + } + if ((double)TBactualPosition > ((TBgiroStep/cellsNumber)*1.2f)&&(erroreTamburo==0)){ + if (firstStart==0){ + if (cntTbError>2){ + all_cellSignal=1; + #if defined(seedSensor) + resetDelay(); + #endif + } + } + if (erroreTamburo==0){ + erroreTamburo=1; + TBmotorDirecti=TBreverse; // rotazione inversa + motor->step_clock_mode_enable(StepperMotor::BWD); + cntCellsForReload=0; + cntTbError++; + #if defined(seedSensor) + resetDelay(); + #endif + } + } + if (((double)TBactualPosition > ((TBgiroStep/cellsNumber)*3.0f))||(cntTbError>4)){ + if (firstStart==0){ + all_noStepRota=1; + #if defined(seedSensor) + resetDelay(); + #endif + } + cntTbError=0; + } +// ---------------------------------------- +// 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)){ + if((quincCnt<3)||(speedFromMaster==0.0f)||(enableSimula==1)){ + tractorSpeed_MtS_timed = ((pulseDistance / pulseSpeedInterval)); // tractor speed (unit= Mt/S) from pulse time interval + } + /*#if !defined(speedMaster) + if (quincCnt>=5){ + if (speedFromMaster>0.0f){ + tractorSpeed_MtS_timed = speedFromMaster + percento; + } + } + #endif*/ + if (checkSDrotation==0){ + checkSDrotation=1; + SDwheelTimer.start(); + } + } + } + speedTimeOut.reset(); + }else{ + double oldLastPr = (double)oldLastPulseRead*1.7f; + if((double)speedTimeOut.read_us()> (oldLastPr)){ + tractorSpeed_MtS_timed = 0.0f; + #if defined(seedSensor) + resetDelay(); + #endif + pntMedia=0; + speedTimeOut.reset(); + enableSpeed=0; + quincCnt=0; + } + } + + #if defined(seedSensor) + if (seedSensorEnable==true){ + if (delaySeedCheck.read_ms()>100){ + if (seedSee==0){ + all_noSeedOnCe=1; + } + resetDelay(); + } + } + #endif + // esegue il controllo di velocità minima + /*if ((double)speedTimer.read_ms()>=maxInterval){ + tractorSpeed_MtS_timed = 0.0f; + enableSpeed=0; + }*/ + // esegue il controllo di velocità massima + /*if ((double)speedTimer.read_ms()<=minIntervalPulse){ + tractorSpeed_MtS_timed = 4.5f; + }*/ + //*************************************************************************************************************** + // cycle logic control section + //*************************************************************************************************************** + if (enableSimula==1){if(simOk==0){tractorSpeed_MtS_timed=0.0f;}} + if ((tractorSpeed_MtS_timed>0.01f)){ + cycleStopRequest=1; + // calcola il tempo teorico di passaggio becchi sulla base della velocità del trattore + 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 (encoder==false){ + 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) + } + }else{ + tempoTraBecchi_mS = (tempoGiroSD / (SDreductionRatio*25.5f))*1000.0f; // tempo tra due impulsi dai becchi in millisecondi ( circa 157mS a 4,5Km/h) + double pippo=0.0f; + #if !defined(speedMaster) + pippo = seedPerimeter / speedFromMaster; + #endif + tempoBecchiPerQuinc = (pippo / pickNumber)*1000.0f; + } + //******************************************* + // 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/maxWorkSpeed;} + #if !defined(speedMaster) + quinCalc(); + #endif + if ((enableSpeed>3)&&(pulseRised2==1)&&(quincCnt>=2)){ + double erroreTempo = 0.0f; + if(encoder==false){ + if(speedFromPick==1){ + erroreTempo = (double)timeIntraPick - tempoTraBecchi_mS; + }else{ + erroreTempo = (double)memoTimeHole - tempoTraBecchi_mS; // errore tra il tempo previsto ed il tempo reale ( >0 se sto andando più piano del previsto) + } + }else{ + erroreTempo = ((double)memoTimeHole/1000.0f) - 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 k3=0.0f; + double k4=0.0f; + double k5=0.0f; + double k6=0.0f; + /*if (tractorSpeed_MtS_timed <= minSeedSpeed){ + k3=1.030f; + k4=5.103f; + k5=10.00f; + k6=20.50f; + }else{*/ + #if defined(speedMaster) + k3=0.010f; + #else + k3=0.050f; + #endif + k4=1.103f; + k5=10.00f; + k6=20.50f; + //} + double L1 = 0.045f; + double L_1=-0.045f; + double L2 = 0.150f; + double L_2=-0.150f; + double L3 = 0.301f; + double L_3=-0.301f; + double k1=0.0f; + if ((errorePercentuale > L3)||(errorePercentuale < L_3)){ + k1=errorePercentuale*k6; + } + if (((errorePercentuale >= L2)&&(errorePercentuale<=L3))||((errorePercentuale <= L_2)&&(errorePercentuale>=L_3))){ + k1=errorePercentuale*k5; + } + if (((errorePercentuale < L2)&&(errorePercentuale>L1))||((errorePercentuale > L_2)&&(errorePercentuale<L_1))){ + k1=errorePercentuale*k4; + } + if ((errorePercentuale < L1)||(errorePercentuale > L_1)){ + k1=errorePercentuale*k3; + } + double memoCorrezione = k1; + if (quincCnt >= 2){ + correzione = correzione + memoCorrezione; + if (correzione > (1.0f - dutyTeorico)){correzione = (1.0f - dutyTeorico);} + if ((correzione < 0.0f)&&(dutyTeorico+correzione<0.0f)){correzione = -1.0f*dutyTeorico;} + } + pulseRised1=0; + pulseRised2=0; + #if defined(pcSerial) + #if defined(Qnca) + pc.printf("ErTem: %f K1: %f Corr: %f MemoCorr:%f DutyTeo: %f \n",erroreTempo, k1,correzione, memoCorrezione, dutyTeorico); + pc.printf("TsP: %f SpW: %f InPic: %f TBec: %f EPerc: %f Duty:%f \n",tractorSpeed_MtS_timed,speedOfSeedWheel,timeIntraPick, tempoTraBecchi_mS,errorePercentuale, dcActualDuty); + #endif + #endif + #if defined(pcSerial) + #if defined(Qncb) + pc.printf("TsP: %f SpW: %f InPic: %f EPerc: %f Duty:%f \n",tractorSpeed_MtS_timed,speedOfSeedWheel,timeIntraPick, errorePercentuale, dcActualDuty); + #endif + #endif + } + // introduce il controllo di corrente + if (currentCheckEnable==true){ + if (incrementCurrent){ + boostDcOut +=0.005f; + } + if (reduceCurrent){ + boostDcOut -=0.005f; + } + if (boostDcOut >= 0.2f){ + boostDcOut=0.2f; + all_genericals=1; + } + if (boostDcOut <=-0.2f){ + boostDcOut=-0.2f; + all_genericals=1; + } + correzione += boostDcOut; + } + DC_brake=0; + DC_forward=1; + DC_prepare(); - while (true) { - led1 = !led1; - wait(0.5); - } -} + // il semiperiodoreale è calcolato sulla lettura del passaggio becchi reale + seedWheelPeriod = semiPeriodoReale; + if (seedWheelPeriod < 180.0f){seedWheelPeriod = 180.0f;} + if ((oldSeedWheelPeriod!=seedWheelPeriod)&&(seedWheelPeriod >=180.0f )){ + SDticker.attach_us(&step_SDPulseOut,seedWheelPeriod); // clock time are microseconds and attach seed motor stepper controls + oldSeedWheelPeriod=seedWheelPeriod; + } + + if((quincCnt>=3)){ + if (correzioneAttiva==1){ + dcActualDuty = dutyTeorico + correzione; + }else{ + dcActualDuty = dutyTeorico; + } + }else{ + dcActualDuty = dutyTeorico; + } + if (dcActualDuty <=0.0f){dcActualDuty=0.05f;} + if (dcActualDuty > 0.95f){dcActualDuty = 0.95f;}//dcMaxSpeed;} + if (olddcActualDuty!=dcActualDuty){ + SDmotorPWM.write(1.0f-dcActualDuty); + olddcActualDuty=dcActualDuty; + } + // allarme + if (SDwheelTimer.read_ms()>4000){ + if (firstStart==0){ + all_noDcRotati=1; + } + #if defined(pcSerial) + #if defined(VediAllarmi) + pc.printf("allarme no DC rotation"); + #endif + #endif + + } + + //*************************************************************************************************************** + // CONTROLLA TAMBURO + //*************************************************************************************************************** + if(lowSpeed==0){ + if (syncroCheck==1){ + syncroCheck=0; + lockStart=1; + periodo = TBperiod; + motor->step_clock_mode_enable(StepperMotor::FWD); + 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(); + motor->soft_hiz(); + } + } + }else{ // fine ciclo fuori da low speed + syncroCheck=0; + lockStart=0; + if (beccoPronto==1){ + if (tamburoStandard==1){ + double ritardoMassimo = 0.0f; + if (encoder==false){ + if(speedFromPick==1){ + ritardoMassimo = (double)timeIntraPick; + }else{ + ritardoMassimo = (double)memoTimeHole; + } + }else{ + ritardoMassimo = (double)timeIntraPick; + } + int tempoDiSincro = (int)((double)(ritardoMassimo - ((tempoBecco/2.0f)+((speedOfSeedWheel/maxWorkSpeed)*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(); + #if defined(seedSensor) + resetDelay(); + #endif + 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; + noSDzeroRequest=1; + } + } + //************************************************************************************************* + // ciclo di azzeramento motori + if ((zeroCycleEnd==0)&&(zeroCycle==1)){//&&(zeroDelay.read_ms()>10000)){ + if (zeroTrigger==0){ + motor->step_clock_mode_enable(StepperMotor::FWD); + 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(1.0f-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(1.0f-0.7f); + avvicinamentoOn.reset(); + avvicinamentoOff.reset(); + avvicinamentoOff.start(); + } + if(avvicinamentoOff.read_ms()>100){ + SDmotorPWM.write(1.0f-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 defined(mezzo) + if (quinconceActive==0){ + if (SDzeroCyclePulse==1){ + avvicinamento=0; + DC_brake=1; + DC_prepare(); + SDzeroed=1; + ritardoStop.reset(); + ritardoStop.stop(); + } + }else{ + if (quincTimeSet.read_ms()>700){ + avvicinamento=0; + DC_brake=1; + DC_prepare(); + SDzeroed=1; + ritardoStop.reset(); + ritardoStop.stop(); + quincTimeSet.stop(); + } + } + #else + if (SDzeroCyclePulse==1){ + avvicinamento=0; + DC_brake=1; + DC_prepare(); + SDzeroed=1; + ritardoStop.reset(); + ritardoStop.stop(); + } + #endif + } + // azzera tutto in time out + if ((timeOutZeroSD.read_ms()>=10000)||(noSDzeroRequest==1)){ + if ((firstStart==0)&&(noSDzeroRequest==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(); + noSDzeroRequest=0; + } + if (TBzeroCyclePulse==1){ + TBticker.detach(); + motor->soft_hiz(); + 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(); + DC_brake=1; + DC_prepare(); + SDzeroed=1; + } + } + +//************************************************************************************************* + 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>=1.0f){ + duty_DC=1.0f; + 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(1.0f-duty_DC); // duty cycle = stop + oldDuty_DC=duty_DC; + DC_prepare(); + } + #endif + }//end in prova + wd.Service(); // kick the dog before the timeout + } // end while +} // end main