Francesco Pistone
new
Diff: main.cpp
- Revision:
- 0:1e09cd7d66b4
- Child:
- 1:e88bf5011af6
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed Jun 13 08:52:42 2018 +0000
@@ -0,0 +1,1841 @@
+ //********************************************************************************************************************
+//********************************************************************************************************************
+// 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: 09 JULY 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
+// 09 06 2018 - INSERITO CONTROLLO DI FASE CON ENCODER MASTER PER QUINCONCE - DATO SCAMBIATO IN CAN
+//
+/********************
+IL FIRMWARE SI COMPONE DI 7 FILES:
+ - main.cpp
+ - main.hpp
+ - iodefinition.hpp
+ - canbus.hpp
+ - parameters.hpp
+ - timeandtick.hpp
+ - variables.hpp
+ED UTILIZZA LE LIBRERIE STANDARD MBED PIU' UNA LIBRERIA MODIFICATA E DEDICATA PER IL CAN
+*********************
+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"
+//********************************************************************************************************************
+//********************************************************************************************************************
+// ------------------------------------------------------------------------------------------------------------------------------------------------------------------
+// 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); // fatto
+ alarmLowRegister=alarmLowRegister+(all_overCurrDC*0x10); // fatto
+ 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_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[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
+ }
+#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.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 > 13.0f){
+ #if defined(canbusActive)
+ all_overCurrDC=1;
+ #endif
+ reduceCurrent=true;
+ }
+ //}
+}
+//*******************************************************
+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 quincTrigga(){
+ if (quinconceIn==1){
+ quincClock=true;
+ }else{
+ quincClock=false;
+ }
+}
+//*******************************************************
+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 (quinconceActive==0){
+ if ((quincClock==true)&&(oldQuincIn==0)){
+ oldQuincIn=1;
+ if (trigSlave==false){
+ quincInibit=true;
+ trigDaMaster=true;
+ }else{
+ quincInibit=false;
+ trigDaMaster=false;
+ trigSlave=false;
+ }
+ if (quincStart==0){
+ quincTime.reset();
+ quincStart=1;
+ }
+ }
+ if( quincClock==false){
+ oldQuincIn=0;
+ }
+ }else{
+ if ((quincClock==false)&&(oldQuincIn==0)){
+ oldQuincIn=1;
+ if (trigSlave==false){
+ quincInibit=true;
+ trigDaMaster=true;
+ }else{
+ quincInibit=false;
+ trigDaMaster=false;
+ trigSlave=false;
+ }
+ if (quincStart==0){
+ quincTime.reset();
+ quincStart=1;
+ }
+ }
+ if( quincClock==true){
+ oldQuincIn=0;
+ }
+ }
+ // riceve l'impulso di passaggio del becco locale e determina il valore dell'errore
+ // controllo di linea e di quinconcio
+ if ((quinconceActive==0)||(quinconceActive==1)){
+ if ((quincStart==1)&&(SDzeroDebounced==1)){
+ if (quincInibit==false){
+ memoDuty=correzione;
+ accelera=0;
+ decelera=0;
+ quincStart=0;
+ percento=1.0f;
+ scostamento = (double)quincTime.read_ms();
+ quincError = 0.0f;
+ double halfRef=riferimento/2.0f;
+ if ((scostamento > 0.0f)&&(scostamento < halfRef)){
+ decelera=1;
+ accelera=0;
+ percento = 1.0f - (abs(scostamento)/riferimento);
+ }
+ if ((scostamento < riferimento)&&(scostamento > halfRef)){
+ decelera=0;
+ accelera=1;
+ quincError = riferimento - scostamento;
+ percento = 1.0f + (abs(quincError)/riferimento);
+ }
+ if (abs(percento) > 1.15f){percento=1.15f;}
+ if (abs(percento) < 0.5f){percento=0.5f;}
+ }
+ }
+ if (abs(percento) >0.0f){
+ correzione = memoDuty* abs(percento);
+ correggiSD=true;
+ }else{
+ correzione = memoDuty;
+ correggiSD=false;
+ }
+ }
+ /*
+ // gestione con confronto encoder
+ // serve l'encoder fisico sugli elementi slave perchè altrimenti la ruota cambia velocità ma non cambia
+ // il clock dell'encoder virtuale e quindi l'errore non può essere rilevato correttamente
+ if ((quinconceActive==0)&&(quincEnable==1)){
+ // da encoder master
+ if (quincVerify==1){
+ quincVerify=0;
+ quincStart=0;
+ memoDuty=dcActualDuty;
+ if (quincInibit==false){
+ 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)/SDsectorStep));
+ }
+ // se quinconce richiesto deve andare al valore di riferimento
+ if (abs(percento) > 1.55f){percento=1.55f;}
+ if (abs(percento) < 0.40f){percento=0.40f;}
+ if (abs(percento) >0.0f){
+ if (abs(percento)>1.0f){
+ correzione=correzione+(abs(percento)*0.01f);
+ }else{
+ correzione=correzione-(abs(percento)*0.01f);
+ }
+ //dcActualDuty = memoDuty * abs(percento);
+ }else{
+ correzione=correzione;
+ //dcActualDuty = memoDuty;
+ }
+ }
+ #if defined(pcSerial)
+ #if defined(Qnc)
+ pc.printf(" trigMaster: %d", trigDaMaster);
+ pc.printf(" trigSlave: %d", trigSlave);
+ pc.printf(" quincinibit: %d", quincInibit);
+ pc.printf(" masterSinc: %d", masterSinc);
+ pc.printf(" prePosSD: %d",prePosSD);
+ pc.printf(" percento: %f",percento);
+ pc.printf(" correzione: %f\n",correzione);
+ #endif
+ #endif
+ }
+ }
+ */
+}
+// ------------------------------------------------------------------------------------------------------------------------------------------------------------------
+// MAIN SECTION
+// ------------------------------------------------------------------------------------------------------------------------------------------------------------------
+
+int main()
+{
+ wd.Configure(2); //watchdog set at xx seconds
+
+ 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);
+ #if !defined(speedMaster)
+ quinconceIn.rise(&quincTrigga);
+ quinconceIn.fall(&quincTrigga);
+ #endif
+ 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;
+ }
+ if ((double)SDactualPosition >= (SDsectorStep/2.0f)){
+ 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){
+ //trigSlave=false;
+ 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;
+ SDzeroDebounced=1;
+ if (trigDaMaster==false){
+ quincInibit=true;
+ trigSlave=true;
+ }else{
+ quincInibit=false;
+ trigSlave=false;
+ trigDaMaster=false;
+ }
+ 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
+ //*******************************************
+ // esegue 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;
+ }
+// ----------------------------------------
+// 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;
+ // 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 (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)
+ }
+ //*******************************************
+ // 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;
+ }
+ // introduce il controllo di corrente
+ 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;
+ #if defined(pcSerial)
+ #if defined(Qnca)
+ pc.printf(" DUTY PRE FINALE: %f",dcActualDuty);
+ #endif
+ #endif
+ // 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
+ quinCalc(1.0f);
+ DC_brake=0;
+ DC_forward=1;
+ DC_prepare();
+ #endif
+ #endif
+
+ // il semiperiodoreale è calcolato sulla lettura del passaggio becchi reale
+ if (correggiSD == true){
+ seedWheelPeriod = semiPeriodoReale / abs(percento);
+ }else{
+ seedWheelPeriod = semiPeriodoReale;
+ }
+ 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;
+ }
+
+ //if (correzione > (dutyTeorico*0.20)){correzione = dutyTeorico*0.20;}
+ if (correzione > 0.30){correzione = 0.30;}
+ if (correzione < -0.30){correzione = -0.30;}
+ if((enableSDcontrol==1)){
+ if (correzioneAttiva==1){
+ dcActualDuty = dutyTeorico + correzione;
+ if (dcActualDuty<dcStarting){dcActualDuty=dcStarting;}
+ }
+ }else{
+ dcActualDuty = dutyTeorico;
+ }
+ if (dcActualDuty <0.0f){dcActualDuty=0.0f;}
+ if (dcActualDuty > 1.0f){dcActualDuty = dcMaxSpeed;}
+ #if defined(pcSerial)
+ #if defined(Qnca)
+ pc.printf(" DUTY FINALE: %f \n",dcActualDuty);
+ #endif
+ #endif
+ SDmotorPWM.write(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;
+ 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
+ wd.Service(); // kick the dog before the timeout
+ } // end while
+}// end main
+