Francesco Pistone / Mbed 2 deprecated FORIGO_Modula_V6_R2C_DE_HwV5_OTTOBRE2018

tr

Dependencies:   mbed CANMsg

main.cpp@8:0e643ea7834f, 2018-07-24 (annotated)

Committer:
nerit
Date:
Tue Jul 24 08:22:50 2018 +0000
Revision:
8:0e643ea7834f
Parent:
7:c9fd242538d9
Child:
9:503e2aba047c
Quinconce con Box versione 5, correzione bugs legati alla simulazione. Versione utilizzata per le prove con Agronomo e relazione per EIMA 2018

Who changed what in which revision?

UserRevisionLine numberNew contents of line
nerit 1:e88bf5011af6 1
nerit 1:e88bf5011af6 2 //********************************************************************************************************************
nerit 0:1e09cd7d66b4 3 //********************************************************************************************************************
nerit 0:1e09cd7d66b4 4 // FIRMWARE SEMINATRICE MODULA
nerit 0:1e09cd7d66b4 5 // VERSIONE PER SCHEDA DI CONTROLLO CON DRIVER INTEGRATI
nerit 0:1e09cd7d66b4 6 // V4 - ATTENZIONE - LA VERSIONE V4 HA IL DRIVER STEPPER LV8727
nerit 0:1e09cd7d66b4 7 // IL MOTORE DC E' GESTITO CON IL DRIVER VNH3SP30-E E CON LA LETTURA
nerit 0:1e09cd7d66b4 8 // DELLA CORRENTE ASSORBITA TRAMITE IL CONVERTITORE MLX91210-CAS102 CON 50A FONDOSCALA
nerit 0:1e09cd7d66b4 9 // CHE FORNISCE UNA TENSIONE DI USCITA PARI A 40mV/A
nerit 0:1e09cd7d66b4 10 // FIRST RELEASE OF BOARD DEC 2017
nerit 0:1e09cd7d66b4 11 // FIRST RELEASE OF FIRMWARE JAN 2018
nerit 0:1e09cd7d66b4 12 //
nerit 0:1e09cd7d66b4 13 // THIS RELEASE: 09 JULY 2018
nerit 0:1e09cd7d66b4 14 //
nerit 0:1e09cd7d66b4 15 // APPLICATION: MODULA CON DISTRIBUTORE ZUCCA OPPURE RISO E PUO' FUNZIONARE ANCHE CON SENSORE A 25 FORI SUL DISCO O
nerit 0:1e09cd7d66b4 16 // ENCODER MOTORE SETTANDO GLI APPOSITI FLAGS
nerit 0:1e09cd7d66b4 17 //
nerit 0:1e09cd7d66b4 18 // 29 05 2018 - INSERITO SECONDO ENCODER VIRTUALE PER LA GESTIONE DEL SINCRONISMO TRA TAMBURO E RUOTA DI SEMINA
nerit 0:1e09cd7d66b4 19 // IN PRATICA IL PRIMO ENCODER è SINCRONO CON IL SEGNALE DEI BECCHI E VIENE AZZERATO DA QUESTI, MENTRE
nerit 0:1e09cd7d66b4 20 // IL SECONDO E' INCREMENTATO IN SINCRONO CON IL PRIMO MA AZZERATO DALLA FASE. IL SUO VALORE E' POI DIVISO
nerit 0:1e09cd7d66b4 21 // PER IL RAPPORTO RUOTE E LA CORREZIONE AGISCE SULLA VELOCITA' DEL TAMBURO PER MANTENERE LA FASE DEL SECONDO
nerit 0:1e09cd7d66b4 22 // ENCODER
nerit 0:1e09cd7d66b4 23 // 05 06 2018 - INSERITO IL CONTROLLO DI GESTIONE DEL QUINCONCE SENZA ENCODER
nerit 0:1e09cd7d66b4 24 // 09 06 2018 - INSERITO CONTROLLO DI FASE CON ENCODER MASTER PER QUINCONCE - DATO SCAMBIATO IN CAN
nerit 0:1e09cd7d66b4 25 //
nerit 0:1e09cd7d66b4 26 /********************
nerit 0:1e09cd7d66b4 27 IL FIRMWARE SI COMPONE DI 7 FILES:
nerit 0:1e09cd7d66b4 28 - main.cpp
nerit 0:1e09cd7d66b4 29 - main.hpp
nerit 0:1e09cd7d66b4 30 - iodefinition.hpp
nerit 0:1e09cd7d66b4 31 - canbus.hpp
nerit 0:1e09cd7d66b4 32 - parameters.hpp
nerit 0:1e09cd7d66b4 33 - timeandtick.hpp
nerit 0:1e09cd7d66b4 34 - variables.hpp
nerit 0:1e09cd7d66b4 35 ED UTILIZZA LE LIBRERIE STANDARD MBED PIU' UNA LIBRERIA MODIFICATA E DEDICATA PER IL CAN
nerit 0:1e09cd7d66b4 36 *********************
nerit 0:1e09cd7d66b4 37 LA MACCHINA UTILIZZA SEMPRE 2 SOLI SENSORI; UNO PER SENTIRE LE CELLE DI CARICO SEME ED UNO PER SENTIRE I BECCHI DI SEMINA.
nerit 0:1e09cd7d66b4 38 GLI AZIONAMENTI SONO COMPOSTI DA DUE MOTORI; UN DC PER IL CONTROLLO DELLA RUOTA DI SEMINA ED UNO STEPPER PER IL CONTROLLO DEL TAMBURO
nerit 0:1e09cd7d66b4 39 UN SENSORE AGGIUNTIVO SULL'ELEMENTO MASTER RILEVA LA VELOCITA' DI AVANZAMENTO
nerit 0:1e09cd7d66b4 40 UN SENSORE AGGIUNTIVO SULLA RUOTA DI SEMINA RILEVA LA ROTAZIONE DELLA RUOTA STESSA ATTRAVERSO FORI PRESENTI SUL DISCO DI SEMINA
nerit 0:1e09cd7d66b4 41 *********************
nerit 0:1e09cd7d66b4 42 LA LOGICA GENERALE PREVEDE CHE IL DC DELLA RUOTA DI SEMINA VENGA COMANDATO IN FUNZIONE DELLA VELOCITA' LETTA DAL SENSORE DI AVANZAMAENTO DEL MASTER
nerit 0:1e09cd7d66b4 43 IL PROBLEMA PRINCIPALE E' CHE QUANDO I BECCHI SONO INSERITI NEL TERRENO NON VI E' RETROAZIONE REALE SULLA VELOCITA' DI ROTAZIONE DELLA RUOTA STESSA
nerit 0:1e09cd7d66b4 44 PROPRIO PERCHE' L'AVANZAMANETO NEL TERRENO IMPRIME UNA VELOCITA' PROPRIA AL BECCO E QUINDI ANCHE ALLA RUOTA.
nerit 0:1e09cd7d66b4 45 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
nerit 0:1e09cd7d66b4 46 IL VALORE DI RIFERIMENTO DELL'ANALOGICA DI INGRESSO VIENE AGGIORNATO OGNI VOLTA CHE LA RUOTA DI SEMINA E' FERMA
nerit 0:1e09cd7d66b4 47 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
nerit 0:1e09cd7d66b4 48 LA FASE VIENE DETERMINATA DAL PASSAGGIO DEI BECCHI SUL SENSORE RELATIVO.
nerit 0:1e09cd7d66b4 49 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
nerit 0:1e09cd7d66b4 50 SOPRATUTTO QUANDO I BECCHI SONO MOLTO DISTANZIATI.
nerit 0:1e09cd7d66b4 51 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
nerit 0:1e09cd7d66b4 52 TARATO SULLA REALE VELOCITA' DI ROTAZIONE DELLA RUOTA DI SEMINA.
nerit 0:1e09cd7d66b4 53 IL PRIMO ENCODER VIRTUALE SI OCCUPA DI DETERMINARE LA POSIZIONE FISICA DELLA RUOTA DI SEMINA E SI AZZERA AL PASSAGGIO DI OGNI BECCO.
nerit 0:1e09cd7d66b4 54 IL SECONDO VIENE AZZERATO DALL'IMPULSO DI FASE DEL PRIMO ENCODER DETERMINATO DAI VALORI IMPOSTI SUL TERMINALE TRITECNICA
nerit 0:1e09cd7d66b4 55 IL SECONDO ENCODER VIENE CONFRONTATO CON LA POSIZIONE ASSOLUTA DEL TAMBURO (DETERMINATA DAL NUMERO DI STEP EMESSI DAL CONTROLLO), RAPPORTATA TRA CELLE E BECCHI.
nerit 0:1e09cd7d66b4 56 IL CONFRONTO DETERMINA LA POSIZIONE RELATIVA DELLA SINGOLA CELLA RISPETTO AL SINGOLO BECCO. IL MANTENIMENTO DELLA SINCRONIZZAZIONE DI FASE, DETERMINA IL SINCRO CELLA/BECCO.
nerit 0:1e09cd7d66b4 57 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
nerit 0:1e09cd7d66b4 58 DI CAMBIARE L'ANGOLO DI ANTICIPO DI RILASCIO DEL SEME IN FUNZIONE DELLA VELOCITA' E RECUPERARE COSI' IL TEMPO DI VOLO DEL SEME.
nerit 0:1e09cd7d66b4 59 IL TAMBURO HA DUE TIPI DI FUNZIONAMENTO: CONTINUO E AD IMPULSI. E' SELEZIONABILE IN FUNZIONE DELLA VELOCITA' E DEL TIPO DI DISTRIBUTORE MONTATO.
nerit 0:1e09cd7d66b4 60 **********************
nerit 0:1e09cd7d66b4 61 TUTTI I VALORI, CELLE, BECCHI, IMPULSI VELOCITA', ANCGOLO DI AVVIO, FASE DI SEMINA, ECC.. SONO IMPOSTABILI DA PANNELLO OPERATORE
nerit 0:1e09cd7d66b4 62 I DATI SONO SCAMBIATI CON IL PANNELLO OPERATORE E CON GLI ALTRI MODULI ATTRAVERSO RETE CAN CON PROTOCOLLO FREESTYLE ATTRAVERSO INDIRIZZAMENTI DEDICATI
nerit 0:1e09cd7d66b4 63 AL MOMENTO NON E' POSSIBILE ATTRIBUIRE L'INIDIRIZZO BASE DELL'ELEMENTO DA TERMINALE OPERATORE MA SOLO IN FASE DI COMPILAZIONE DEL FIRMWARE.
nerit 0:1e09cd7d66b4 64 **********************
nerit 0:1e09cd7d66b4 65 ALTRE SEZIONI RIGUARDANO LA GENERAZIONE DEGLI ALLARMI, LA COMUNICAZIONE CAN, LA SIMULAZIONE DI LAVORO, LA GESTIONE DELLA DIAGNOSI ECC..
nerit 0:1e09cd7d66b4 66 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.
nerit 0:1e09cd7d66b4 67 LO STEPPER SEGUE IL DC.
nerit 0:1e09cd7d66b4 68 **********************
nerit 0:1e09cd7d66b4 69 IN FASE DI ACCENSIONE ED OGNI QUALVOLTA SI ARRIVA A VELOCITA' ZERO, LA MACCHINA ESEGUE UN CICLO DI AZZERAMENTO
nerit 0:1e09cd7d66b4 70 NON ESISTE PULSANTE DI MARCIA/STOP; E' SEMPRE ATTIVA.
nerit 0:1e09cd7d66b4 71 **********************
nerit 0:1e09cd7d66b4 72 NEL PROGRAMMA E' PRESENTE UNA SEZIONE DI TEST FISICO DELLA SCHEDA ATTIVABILE SOLO IN FASE DI COMPILAZIONE
nerit 0:1e09cd7d66b4 73 **********************
nerit 0:1e09cd7d66b4 74 ALTRE FUNZIONI: PRECARICAMENTO DEL TAMBURO
nerit 0:1e09cd7d66b4 75 AZZERAMENTO MANUALE
nerit 0:1e09cd7d66b4 76 STATISTICA DI SEMINA (CONTA LE CELLE)
nerit 0:1e09cd7d66b4 77 */
nerit 0:1e09cd7d66b4 78 //********************************************************************************************************************
nerit 0:1e09cd7d66b4 79 //********************************************************************************************************************
nerit 0:1e09cd7d66b4 80
nerit 0:1e09cd7d66b4 81 #include "main.hpp"
nerit 0:1e09cd7d66b4 82 #include "timeandtick.hpp"
nerit 0:1e09cd7d66b4 83 #include "canbus.hpp"
nerit 0:1e09cd7d66b4 84 #include "watchdog.h"
nerit 0:1e09cd7d66b4 85 #include "iodefinition.hpp"
nerit 0:1e09cd7d66b4 86 #include "parameters.hpp"
nerit 0:1e09cd7d66b4 87 #include "variables.hpp"
nerit 0:1e09cd7d66b4 88 //********************************************************************************************************************
nerit 0:1e09cd7d66b4 89 //********************************************************************************************************************
nerit 0:1e09cd7d66b4 90 // ------------------------------------------------------------------------------------------------------------------------------------------------------------------
nerit 0:1e09cd7d66b4 91 // TASK SECTION
nerit 0:1e09cd7d66b4 92 // ------------------------------------------------------------------------------------------------------------------------------------------------------------------
nerit 0:1e09cd7d66b4 93 //************************************************************************
nerit 3:c0f11ca4df02 94 //************************************************************************
nerit 0:1e09cd7d66b4 95 // rise of seed speed 25 pulse sensor
nerit 0:1e09cd7d66b4 96 void sd25Fall(){
nerit 0:1e09cd7d66b4 97 timeHole=metalTimer.read_ms();
nerit 0:1e09cd7d66b4 98 int memo_TimeHole= (memoTimeHole + timeHole)/ (int)2;
nerit 0:1e09cd7d66b4 99 memoTimeHole = timeHole;
nerit 0:1e09cd7d66b4 100 metalTimer.reset();
nerit 0:1e09cd7d66b4 101 if (speedFromPick==0){
nerit 0:1e09cd7d66b4 102 speedOfSeedWheel=((seedPerimeter/25.0f)/(double)memo_TimeHole)*1000.0f; //mtS
nerit 0:1e09cd7d66b4 103 }
nerit 0:1e09cd7d66b4 104 }
nerit 5:3b95bbfe2dc9 105 // rise of seed speed motor encoder
nerit 5:3b95bbfe2dc9 106 void encoRise(){
nerit 5:3b95bbfe2dc9 107 timeHole=metalTimer.read_us();
nerit 5:3b95bbfe2dc9 108 int memo_TimeHole= (memoTimeHole + timeHole)/ (int)2;
nerit 5:3b95bbfe2dc9 109 memoTimeHole = timeHole;
nerit 5:3b95bbfe2dc9 110 metalTimer.reset();
nerit 5:3b95bbfe2dc9 111 if (encoder==true){
nerit 5:3b95bbfe2dc9 112 speedOfSeedWheel=((seedPerimeter/((SDreductionRatio*25.5f)))/(double)memo_TimeHole)*1000000.0f; //mtS
nerit 7:c9fd242538d9 113 pulseRised2=1;
nerit 5:3b95bbfe2dc9 114 }
nerit 5:3b95bbfe2dc9 115 }
nerit 7:c9fd242538d9 116 // rise of seed presence sensor
nerit 7:c9fd242538d9 117 void seedSensorTask(){
nerit 7:c9fd242538d9 118 seedSee=1;
nerit 7:c9fd242538d9 119 }
nerit 0:1e09cd7d66b4 120 //**************************************************
nerit 0:1e09cd7d66b4 121 // generate speed clock when speed is simulated from Tritecnica display
nerit 0:1e09cd7d66b4 122 void speedSimulationClock(){
nerit 0:1e09cd7d66b4 123 lastPulseRead=speedTimer.read_us();
nerit 0:1e09cd7d66b4 124 oldLastPulseRead=lastPulseRead;
nerit 0:1e09cd7d66b4 125 speedTimer.reset();
nerit 0:1e09cd7d66b4 126 pulseRised=1;
nerit 7:c9fd242538d9 127 speedFilter.reset();
nerit 0:1e09cd7d66b4 128 }
nerit 0:1e09cd7d66b4 129 //*******************************************************
nerit 0:1e09cd7d66b4 130 // interrupt task for tractor speed reading
nerit 0:1e09cd7d66b4 131 //*******************************************************
nerit 0:1e09cd7d66b4 132 void tractorReadSpeed(){
nerit 0:1e09cd7d66b4 133 if ((oldTractorSpeedRead==0)){
nerit 0:1e09cd7d66b4 134 lastPulseRead=speedTimer.read_us();
nerit 0:1e09cd7d66b4 135 oldLastPulseRead=lastPulseRead;
nerit 0:1e09cd7d66b4 136 speedTimer.reset();
nerit 0:1e09cd7d66b4 137 pulseRised=1;
nerit 0:1e09cd7d66b4 138 oldTractorSpeedRead=1;
nerit 6:3fca0ca1949e 139 spazioCoperto+= pulseDistance;
nerit 0:1e09cd7d66b4 140 }
nerit 0:1e09cd7d66b4 141 speedFilter.reset();
nerit 0:1e09cd7d66b4 142 speedClock=1;
nerit 0:1e09cd7d66b4 143 }
nerit 0:1e09cd7d66b4 144 //*******************************************************
nerit 0:1e09cd7d66b4 145 void speedMediaCalc(){
nerit 0:1e09cd7d66b4 146 double lastPd=(double) lastPulseRead/1000.0f;
nerit 0:1e09cd7d66b4 147 pulseSpeedInterval = (mediaSpeed[0]+lastPd)/2.0f;
nerit 0:1e09cd7d66b4 148 if (enableSimula==1){
nerit 0:1e09cd7d66b4 149 double TMT = (double)(speedSimula) * 100.0f /3600.0f;
nerit 0:1e09cd7d66b4 150 pulseSpeedInterval = pulseDistance / TMT;
nerit 0:1e09cd7d66b4 151 }
nerit 0:1e09cd7d66b4 152 mediaSpeed[0]=lastPd;
nerit 0:1e09cd7d66b4 153 OLDpulseSpeedInterval=pulseSpeedInterval;
nerit 0:1e09cd7d66b4 154 }
nerit 0:1e09cd7d66b4 155
nerit 0:1e09cd7d66b4 156 //*******************************************************
nerit 0:1e09cd7d66b4 157 // clocked task for manage virtual encoder of seed wheel i/o
nerit 0:1e09cd7d66b4 158 //*******************************************************
nerit 0:1e09cd7d66b4 159 //*******************************************************
nerit 0:1e09cd7d66b4 160 void step_SDPulseOut(){
nerit 0:1e09cd7d66b4 161 SDactualPosition++;
nerit 0:1e09cd7d66b4 162 prePosSD++;
nerit 5:3b95bbfe2dc9 163 #if defined(speedMaster)
nerit 5:3b95bbfe2dc9 164 posForQuinc++;
nerit 5:3b95bbfe2dc9 165 #endif
nerit 0:1e09cd7d66b4 166 }
nerit 0:1e09cd7d66b4 167 //*******************************************************
nerit 0:1e09cd7d66b4 168 void step_TBPulseOut(){
nerit 0:1e09cd7d66b4 169 TBmotorStepOut=!TBmotorStepOut;
nerit 0:1e09cd7d66b4 170 if (TBmotorStepOut==0){
nerit 0:1e09cd7d66b4 171 TBactualPosition++;
nerit 0:1e09cd7d66b4 172 }
nerit 0:1e09cd7d66b4 173 }
nerit 0:1e09cd7d66b4 174 //*******************************************************
nerit 0:1e09cd7d66b4 175 // aggiornamento parametri di lavoro fissi e da Tritecnica
nerit 0:1e09cd7d66b4 176 void aggiornaParametri(){
nerit 0:1e09cd7d66b4 177 speedPerimeter = Pi * speedWheelDiameter ; // perimeter of speed wheel
nerit 0:1e09cd7d66b4 178 pulseDistance = (speedPerimeter / speedWheelPulse)*1000.0f; // linear space between speed wheel pulse
nerit 0:1e09cd7d66b4 179 seedPerimeter = Pi * (seedWheelDiameter-(deepOfSeed*2.0f)); // perimeter of seed wheel
nerit 0:1e09cd7d66b4 180 intraPickDistance = seedPerimeter/pickNumber;
nerit 0:1e09cd7d66b4 181 K_WheelRPM = 60.0f/seedPerimeter; // calcola il K per i giri al minuto della ruota di semina
nerit 0:1e09cd7d66b4 182 K_WhellFrequency = (seedWheelMotorSteps*SDreductionRatio)/60.0f; // calcola il K per la frequenza di comando del motore di semina
nerit 0:1e09cd7d66b4 183 rapportoRuote = pickNumber/cellsNumber; // calcola il rapporto tra il numero di becchi ed il numero di celle
nerit 0:1e09cd7d66b4 184 K_percentuale = TBmotorSteps*TBreductionRatio;
nerit 0:1e09cd7d66b4 185 SDsectorStep = (double)fixedStepGiroSD / (double)pickNumber;
nerit 0:1e09cd7d66b4 186 TBsectorStep = (TBmotorSteps*TBreductionRatio)/cellsNumber;
nerit 0:1e09cd7d66b4 187 KcorT = (SDsectorStep/TBsectorStep);///2.0f;
nerit 0:1e09cd7d66b4 188 angoloFase=angoloPh;
nerit 0:1e09cd7d66b4 189 avvioGradi=angoloAv;
nerit 0:1e09cd7d66b4 190 stepGrado=fixedStepGiroSD/360.0f;
nerit 0:1e09cd7d66b4 191 TBdeltaStep=(fixedStepGiroSD/pickNumber)+(stepGrado*avvioGradi);
nerit 0:1e09cd7d66b4 192 TBfaseStep = (stepGrado*angoloFase);
nerit 0:1e09cd7d66b4 193 K_WheelRPM = 60.0f/seedPerimeter; // calcola il K per i giri al minuto della ruota di semina 25.4 25,40
nerit 7:c9fd242538d9 194 TBgiroStep = TBmotorSteps*TBreductionRatio;
nerit 7:c9fd242538d9 195 K_TBfrequency = TBgiroStep/60.0f; // 1600 * 1.65625f /60 = 44 44,00
nerit 0:1e09cd7d66b4 196 if (speedFromPick==1) {
nerit 0:1e09cd7d66b4 197 intraPickDistance = seedPerimeter/pickNumber;
nerit 0:1e09cd7d66b4 198 }else{
nerit 0:1e09cd7d66b4 199 intraPickDistance = seedPerimeter/25.0f; // 25 è il numero di fori presenti nel disco di semina
nerit 0:1e09cd7d66b4 200 }
nerit 0:1e09cd7d66b4 201 }
nerit 0:1e09cd7d66b4 202 //*******************************************************
nerit 0:1e09cd7d66b4 203 void cambiaTB(double perio){
nerit 0:1e09cd7d66b4 204 // update TB frequency
nerit 0:1e09cd7d66b4 205 double TBper=0.0f;
nerit 0:1e09cd7d66b4 206 if (aspettaStart==0){
nerit 0:1e09cd7d66b4 207 if (perio<250.0f){perio=500.0f;}
nerit 0:1e09cd7d66b4 208 double scala =0.0f;
nerit 0:1e09cd7d66b4 209 if (lowSpeed==1){
nerit 0:1e09cd7d66b4 210 scala =2.0f;
nerit 0:1e09cd7d66b4 211 }else{
nerit 0:1e09cd7d66b4 212 scala =1.8f;
nerit 0:1e09cd7d66b4 213 }
nerit 0:1e09cd7d66b4 214 TBper=perio/scala;
nerit 0:1e09cd7d66b4 215 if (oldPeriodoTB!=TBper){
nerit 0:1e09cd7d66b4 216 if (TBper >= 250.0f){
nerit 0:1e09cd7d66b4 217 TBticker.attach_us(&step_TBPulseOut,TBper); // clock time are milliseconds and attach seed motor stepper controls
nerit 0:1e09cd7d66b4 218 }else{
nerit 0:1e09cd7d66b4 219 TBticker.detach();
nerit 0:1e09cd7d66b4 220 }
nerit 0:1e09cd7d66b4 221 oldPeriodoTB=TBper;
nerit 0:1e09cd7d66b4 222 }
nerit 0:1e09cd7d66b4 223 }
nerit 0:1e09cd7d66b4 224 }
nerit 0:1e09cd7d66b4 225 //*******************************************************
nerit 0:1e09cd7d66b4 226 void seedCorrect(){
nerit 0:1e09cd7d66b4 227 /*
nerit 0:1e09cd7d66b4 228 posError determina la posizione relativa di TB rispetto ad SD
nerit 0:1e09cd7d66b4 229 la reale posizione di SD viene modificata in funzione della velocità per
nerit 0:1e09cd7d66b4 230 traslare la posizione relativa di TB. All'aumentare della velocità la posizione
nerit 0:1e09cd7d66b4 231 di SD viene incrementata così che TB acceleri per raggiungerla in modo da rilasciare il seme prima
nerit 0:1e09cd7d66b4 232 La taratura del sistema avviene determinando prima il valore di angoloFase alla minima velocità,
nerit 0:1e09cd7d66b4 233 poi, alla massima velocità, dovrebbe spostarsi la posizione relativa con una variabile proporzionale alla velocità, ma c'è un però.
nerit 0:1e09cd7d66b4 234 Il problema è che il momento di avvio determina una correzione dell'angolo di partenza del tamburo
nerit 0:1e09cd7d66b4 235 angolo che viene rideterminato ogni volta che il sensore becchi legge un transito.
nerit 0:1e09cd7d66b4 236 Di fatto c'è una concorrenza tra l'angolo di avvio determinato e la correzione di posizione relativa
nerit 0:1e09cd7d66b4 237 del tamburo. E' molto probabile che convenga modificare solo la posizione relativa e non anche l'angolo di avvio
nerit 0:1e09cd7d66b4 238 Ancora di più se viene eliminata la parte gestita da ciclata.
nerit 0:1e09cd7d66b4 239 In questo modo dovrebbe esserci solo un andamento in accelerazione di TB che viene poi eventualmente decelerato
nerit 0:1e09cd7d66b4 240 dal passaggio sul sensore di TB. Funzione corretta perchè il sincronismo tra i sensori genera l'inibizione della correzione
nerit 0:1e09cd7d66b4 241 di fase di TB. In pratica il ciclo viene resettato al passaggio sul sensore di SD che riporta a 0 la posizione di SD.
nerit 0:1e09cd7d66b4 242 Appena il sensore di TB viene impegnato allora viene abilitato il controllo di fase del tamburo.
nerit 0:1e09cd7d66b4 243 Questo si traduce nel fatto che il controllo di posizione viene gestito solo all'interno di uno slot di semina in modo che
nerit 0:1e09cd7d66b4 244 il tamburo non risenta della condizione di reset della posizione di SD mentre lui è ancora nella fase precedente. Si fermerebbe.
nerit 0:1e09cd7d66b4 245
nerit 0:1e09cd7d66b4 246 // La considerazione finale è che mantenendo l'angolo di avvio fisso e regolato sulla bassa velocità, intervenendo solo sulla correzione
nerit 0:1e09cd7d66b4 247 // di posizione in questa routine, dovrebbe essere possibile seminare correttamente a tutte le velocità regolando solo 2 parametri.
nerit 0:1e09cd7d66b4 248 */
nerit 0:1e09cd7d66b4 249 /*
nerit 0:1e09cd7d66b4 250 SDsectorStep = (double)fixedStepGiroSD / (double)pickNumber;
nerit 0:1e09cd7d66b4 251 TBsectorStep = (TBmotorSteps*TBreductionRatio)/cellsNumber;
nerit 0:1e09cd7d66b4 252 KcorT = (SDsectorStep/TBsectorStep);
nerit 0:1e09cd7d66b4 253 angoloFase=angoloPh;
nerit 0:1e09cd7d66b4 254 stepGrado=fixedStepGiroSD/360.0f;
nerit 0:1e09cd7d66b4 255 avvioGradi = costante da terminale tritecnica
nerit 0:1e09cd7d66b4 256 TBdeltaStep=(fixedStepGiroSD/pickNumber)-(stepGrado*avvioGradi);
nerit 0:1e09cd7d66b4 257 TBfaseStep = (stepGrado*angoloFase);
nerit 0:1e09cd7d66b4 258 */
nerit 0:1e09cd7d66b4 259
nerit 0:1e09cd7d66b4 260 if ((tractorSpeed_MtS_timed>0.01f)){
nerit 0:1e09cd7d66b4 261 if (inhibit==0){
nerit 0:1e09cd7d66b4 262 double posError =0.0f;
nerit 0:1e09cd7d66b4 263 double posSD=((double)SDactualPosition)/KcorT;
nerit 0:1e09cd7d66b4 264 posError = posSD - (double)TBactualPosition;
nerit 0:1e09cd7d66b4 265 // interviene sulla velocità di TB per raggiungere la corretta posizione relativa
nerit 0:1e09cd7d66b4 266 if((lowSpeed==0)&&(aspettaStart==0)){
nerit 0:1e09cd7d66b4 267 if (posError>50.0f){posError=50.0f;}
nerit 0:1e09cd7d66b4 268 if (posError<-50.0f){posError=-50.0f;}
nerit 0:1e09cd7d66b4 269 if ((posError >=1.0f)||(posError<=-1.0f)){
nerit 0:1e09cd7d66b4 270 ePpos = periodo /(1.0f+ ((posError/100.0f)));
nerit 0:1e09cd7d66b4 271 cambiaTB(ePpos);
nerit 0:1e09cd7d66b4 272 }
nerit 0:1e09cd7d66b4 273 }
nerit 0:1e09cd7d66b4 274 }
nerit 4:d32258ec411f 275 }
nerit 4:d32258ec411f 276 }
nerit 4:d32258ec411f 277 //*******************************************************
nerit 4:d32258ec411f 278 void videoUpdate(){
nerit 4:d32258ec411f 279 for(int aa=0;aa<4;aa++){speedForDisplay[aa]=speedForDisplay[aa+1];}
nerit 4:d32258ec411f 280 speedForDisplay[4]=tractorSpeed_MtS_timed;
nerit 4:d32258ec411f 281 totalSpeed=0.0f;
nerit 4:d32258ec411f 282 for (int aa=0; aa<5; aa++){totalSpeed += speedForDisplay[aa];}
nerit 4:d32258ec411f 283 totalSpeed = totalSpeed / 5.0f;
nerit 0:1e09cd7d66b4 284 #if defined(pcSerial)
nerit 0:1e09cd7d66b4 285 #if defined(SDreset)
nerit 0:1e09cd7d66b4 286 pc.printf("Fase: %d",fase);
nerit 0:1e09cd7d66b4 287 pc.printf(" PrePosSD: %d",prePosSD);
nerit 0:1e09cd7d66b4 288 pc.printf(" PosSD: %d",SDactualPosition);
nerit 0:1e09cd7d66b4 289 pc.printf(" speed: %f",tractorSpeed_MtS_timed);
nerit 0:1e09cd7d66b4 290 pc.printf(" Trigger: %d \n", trigRepos);
nerit 0:1e09cd7d66b4 291 #endif
nerit 0:1e09cd7d66b4 292 #endif
nerit 0:1e09cd7d66b4 293 }
nerit 0:1e09cd7d66b4 294 //*******************************************************
nerit 0:1e09cd7d66b4 295 void ciclaTB(){
nerit 0:1e09cd7d66b4 296 if ((startCicloTB==1)&&(cicloTbinCorso==0)){
nerit 0:1e09cd7d66b4 297 TBticker.attach_us(&step_TBPulseOut,TBperiod/2.5f); // clock time are milliseconds and attach seed motor stepper controls
nerit 0:1e09cd7d66b4 298 cicloTbinCorso = 1;
nerit 0:1e09cd7d66b4 299 startCicloTB=0;
nerit 0:1e09cd7d66b4 300 }
nerit 0:1e09cd7d66b4 301 if ((loadDaCan==1)&&(loadDaCanInCorso==0)){
nerit 0:1e09cd7d66b4 302 TBticker.attach_us(&step_TBPulseOut,1000.0f); // clock time are milliseconds and attach seed motor stepper controls
nerit 0:1e09cd7d66b4 303 loadDaCanInCorso=1;
nerit 0:1e09cd7d66b4 304 stopCicloTB=0;
nerit 0:1e09cd7d66b4 305 }
nerit 0:1e09cd7d66b4 306 if ((stopCicloTB==1)&&(TBactualPosition>5)){
nerit 0:1e09cd7d66b4 307 TBticker.detach();
nerit 0:1e09cd7d66b4 308 cicloTbinCorso = 0;
nerit 0:1e09cd7d66b4 309 stopCicloTB=0;
nerit 0:1e09cd7d66b4 310 loadDaCanInCorso=0;
nerit 0:1e09cd7d66b4 311 loadDaCan=0;
nerit 0:1e09cd7d66b4 312 }
nerit 0:1e09cd7d66b4 313 }
nerit 7:c9fd242538d9 314 // ------------------------------------------------------------------------------------------------------------------------------------------------------------------
nerit 7:c9fd242538d9 315 void stepSetting(){
nerit 7:c9fd242538d9 316 // Stepper driver init and set
nerit 7:c9fd242538d9 317 TBmotorRst=1; // reset stepper driver
nerit 7:c9fd242538d9 318 TBmotorDirecti=1; // reset stepper direction
nerit 7:c9fd242538d9 319 // M1 M2 M3 RESOLUTION
nerit 7:c9fd242538d9 320 // 0 0 0 1/2
nerit 7:c9fd242538d9 321 // 1 0 0 1/8
nerit 7:c9fd242538d9 322 // 0 1 0 1/16
nerit 7:c9fd242538d9 323 // 1 1 0 1/32
nerit 7:c9fd242538d9 324 // 0 0 1 1/64
nerit 7:c9fd242538d9 325 // 1 0 1 1/128
nerit 7:c9fd242538d9 326 // 0 1 1 1/10
nerit 7:c9fd242538d9 327 // 1 1 1 1/20
nerit 7:c9fd242538d9 328 if (TBmotorSteps==400){
nerit 7:c9fd242538d9 329 TBmotor_M1=1;
nerit 7:c9fd242538d9 330 TBmotor_M2=1;
nerit 7:c9fd242538d9 331 TBmotor_M3=1;
nerit 7:c9fd242538d9 332 }else if (TBmotorSteps==1600){
nerit 7:c9fd242538d9 333 TBmotor_M1=0;
nerit 7:c9fd242538d9 334 TBmotor_M2=1;
nerit 7:c9fd242538d9 335 TBmotor_M3=1;
nerit 7:c9fd242538d9 336 }else if (TBmotorSteps==3200){
nerit 7:c9fd242538d9 337 TBmotor_M1=1;
nerit 7:c9fd242538d9 338 TBmotor_M2=0;
nerit 7:c9fd242538d9 339 TBmotor_M3=1;
nerit 7:c9fd242538d9 340 }else if (TBmotorSteps==6400){
nerit 7:c9fd242538d9 341 TBmotor_M1=0;
nerit 7:c9fd242538d9 342 TBmotor_M2=0;
nerit 7:c9fd242538d9 343 TBmotor_M3=1;
nerit 7:c9fd242538d9 344 }else if (TBmotorSteps==12800){
nerit 7:c9fd242538d9 345 TBmotor_M1=1;
nerit 7:c9fd242538d9 346 TBmotor_M2=1;
nerit 7:c9fd242538d9 347 TBmotor_M3=0;
nerit 7:c9fd242538d9 348 }else if (TBmotorSteps==25600){
nerit 7:c9fd242538d9 349 TBmotor_M1=0;
nerit 7:c9fd242538d9 350 TBmotor_M2=1;
nerit 7:c9fd242538d9 351 TBmotor_M3=0;
nerit 7:c9fd242538d9 352 }else if (TBmotorSteps==2000){
nerit 7:c9fd242538d9 353 TBmotor_M1=1;
nerit 7:c9fd242538d9 354 TBmotor_M2=0;
nerit 7:c9fd242538d9 355 TBmotor_M3=0;
nerit 7:c9fd242538d9 356 }else if (TBmotorSteps==4000){
nerit 7:c9fd242538d9 357 TBmotor_M1=0;
nerit 7:c9fd242538d9 358 TBmotor_M2=0;
nerit 7:c9fd242538d9 359 TBmotor_M3=0;
nerit 7:c9fd242538d9 360 }
nerit 7:c9fd242538d9 361 TBmotorRst=0;
nerit 7:c9fd242538d9 362 }
nerit 7:c9fd242538d9 363 //****************************************
nerit 7:c9fd242538d9 364 void dcSetting(){
nerit 7:c9fd242538d9 365 if ((speedFromPick==0)&&(encoder==false)){
nerit 7:c9fd242538d9 366 DcEncoder.rise(&sd25Fall);
nerit 7:c9fd242538d9 367 }
nerit 7:c9fd242538d9 368 if (encoder==true){
nerit 7:c9fd242538d9 369 DcEncoder.rise(&encoRise);
nerit 7:c9fd242538d9 370 //ElementPosition.fall(&encoRise);
nerit 7:c9fd242538d9 371 }
nerit 7:c9fd242538d9 372 }
nerit 0:1e09cd7d66b4 373 //*******************************************************
nerit 0:1e09cd7d66b4 374 void allarmi(){
nerit 0:1e09cd7d66b4 375 uint8_t alarmLowRegister1=0x00;
nerit 0:1e09cd7d66b4 376 alarmLowRegister=0x00;
nerit 0:1e09cd7d66b4 377 alarmHighRegister=0x80;
nerit 0:1e09cd7d66b4 378
nerit 0:1e09cd7d66b4 379 //alarmLowRegister=alarmLowRegister+(all_semiFiniti*0x01); // manca il sensore
nerit 0:1e09cd7d66b4 380 alarmLowRegister=alarmLowRegister+(all_pickSignal*0x02); // fatto
nerit 0:1e09cd7d66b4 381 alarmLowRegister=alarmLowRegister+(all_cellSignal*0x04); // fatto
nerit 0:1e09cd7d66b4 382 alarmLowRegister=alarmLowRegister+(all_lowBattery*0x08); // fatto
nerit 0:1e09cd7d66b4 383 alarmLowRegister=alarmLowRegister+(all_overCurrDC*0x10); // fatto
nerit 0:1e09cd7d66b4 384 alarmLowRegister=alarmLowRegister+(all_stopSistem*0x20); // verificarne la necessità
nerit 0:1e09cd7d66b4 385 //alarmLowRegister=alarmLowRegister+(all_upElements*0x40); // manca il sensore
nerit 7:c9fd242538d9 386 if (seedSensorEnable==true){
nerit 7:c9fd242538d9 387 alarmLowRegister=alarmLowRegister+(all_noSeedOnCe*0x80); // manca il sensore
nerit 7:c9fd242538d9 388 }
nerit 0:1e09cd7d66b4 389
nerit 0:1e09cd7d66b4 390 //alarmLowRegister1=alarmLowRegister1+(all_cfgnErrors*0x01); // da scrivere
nerit 0:1e09cd7d66b4 391 alarmLowRegister1=alarmLowRegister1+(all_noDcRotati*0x02); // fatto
nerit 0:1e09cd7d66b4 392 alarmLowRegister1=alarmLowRegister1+(all_noStepRota*0x04); // fatto
nerit 0:1e09cd7d66b4 393 alarmLowRegister1=alarmLowRegister1+(all_speedError*0x08); // fatto
nerit 0:1e09cd7d66b4 394 alarmLowRegister1=alarmLowRegister1+(all_noSpeedSen*0x10); // fatto
nerit 0:1e09cd7d66b4 395 alarmLowRegister1=alarmLowRegister1+(all_no_Zeroing*0x20); // fatto
nerit 0:1e09cd7d66b4 396 alarmLowRegister1=alarmLowRegister1+(all_genericals*0x40);
nerit 0:1e09cd7d66b4 397 if (alarmLowRegister1 > 0){
nerit 0:1e09cd7d66b4 398 alarmHighRegister = 0x81;
nerit 0:1e09cd7d66b4 399 alarmLowRegister = alarmLowRegister1;
nerit 0:1e09cd7d66b4 400 }
nerit 0:1e09cd7d66b4 401
nerit 0:1e09cd7d66b4 402 #if defined(pcSerial)
nerit 0:1e09cd7d66b4 403 #if defined(VediAllarmi)
nerit 0:1e09cd7d66b4 404 if (all_pickSignal==1){pc.printf("AllarmeBecchi\n");}
nerit 0:1e09cd7d66b4 405 if (all_cellSignal==1){pc.printf("AllarmeCelle\n");}
nerit 0:1e09cd7d66b4 406 if (all_lowBattery==1){pc.printf("AllarmeBassaCorrente\n");}
nerit 0:1e09cd7d66b4 407 if (all_overCurrDC==1){pc.printf("AllarmeAltaCorrente\n");}
nerit 0:1e09cd7d66b4 408 if (all_stopSistem==1){pc.printf("AllarmeStop\n");}
nerit 0:1e09cd7d66b4 409 if (all_noDcRotati==1){pc.printf("AllarmeDCnoRotation\n");}
nerit 0:1e09cd7d66b4 410 if (all_noStepRota==1){pc.printf("AllarmeNoStepRotation\n");}
nerit 0:1e09cd7d66b4 411 if (all_speedError==1){pc.printf("AllarmeSpeedError\n");}
nerit 0:1e09cd7d66b4 412 if (all_noSpeedSen==1){pc.printf("AllarmeNoSpeedSensor\n");}
nerit 0:1e09cd7d66b4 413 if (all_no_Zeroing==1){pc.printf("AllarmeNoZero\n");}
nerit 0:1e09cd7d66b4 414 if (all_genericals==1){pc.printf("AllarmeGenerico\n");}
nerit 0:1e09cd7d66b4 415 pc.printf("Code: 0x%x%x\n",alarmHighRegister,alarmLowRegister);
nerit 0:1e09cd7d66b4 416 #endif
nerit 0:1e09cd7d66b4 417 #endif
nerit 0:1e09cd7d66b4 418 all_semiFiniti=0;
nerit 0:1e09cd7d66b4 419 all_pickSignal=0;
nerit 0:1e09cd7d66b4 420 all_cellSignal=0;
nerit 0:1e09cd7d66b4 421 all_lowBattery=0;
nerit 0:1e09cd7d66b4 422 all_overCurrDC=0;
nerit 0:1e09cd7d66b4 423 all_stopSistem=0;
nerit 0:1e09cd7d66b4 424 all_upElements=0;
nerit 0:1e09cd7d66b4 425 all_noSeedOnCe=0;
nerit 0:1e09cd7d66b4 426 all_cfgnErrors=0;
nerit 0:1e09cd7d66b4 427 all_noDcRotati=0;
nerit 0:1e09cd7d66b4 428 all_noStepRota=0;
nerit 0:1e09cd7d66b4 429 all_speedError=0;
nerit 0:1e09cd7d66b4 430 all_noSpeedSen=0;
nerit 0:1e09cd7d66b4 431 all_no_Zeroing=0;
nerit 0:1e09cd7d66b4 432 all_genericals=0;
nerit 0:1e09cd7d66b4 433 }
nerit 0:1e09cd7d66b4 434 //*******************************************************
nerit 0:1e09cd7d66b4 435 #if defined(speedMaster)
nerit 0:1e09cd7d66b4 436 void upDateSincro(){
nerit 0:1e09cd7d66b4 437 char val1[8]={0,0,0,0,0,0,0,0};
nerit 7:c9fd242538d9 438 val1[3]=(posForQuinc /0x00FF0000)&0x000000FF;
nerit 7:c9fd242538d9 439 val1[2]=(posForQuinc /0x0000FF00)&0x000000FF;
nerit 7:c9fd242538d9 440 val1[1]=(posForQuinc /0x000000FF)&0x000000FF;
nerit 7:c9fd242538d9 441 val1[0]=posForQuinc & 0x000000FF;
nerit 7:c9fd242538d9 442 //double pass = tractorSpeed_MtS_timed*100.0f;
nerit 7:c9fd242538d9 443 double pass = speedOfSeedWheel*100.0f;
nerit 5:3b95bbfe2dc9 444 val1[4]=(uint8_t)(pass)&0x000000FF;
nerit 5:3b95bbfe2dc9 445 val1[5]=(prePosSD /0x0000FF00)&0x000000FF;
nerit 5:3b95bbfe2dc9 446 val1[6]=(prePosSD /0x000000FF)&0x000000FF;
nerit 5:3b95bbfe2dc9 447 val1[7]=prePosSD & 0x000000FF;
nerit 0:1e09cd7d66b4 448 #if defined(canbusActive)
nerit 0:1e09cd7d66b4 449 #if defined(speedMaster)
nerit 0:1e09cd7d66b4 450 if(can1.write(CANMessage(TX_SI, *&val1,8))){
nerit 0:1e09cd7d66b4 451 checkState=0;
nerit 0:1e09cd7d66b4 452 }
nerit 0:1e09cd7d66b4 453 #endif
nerit 0:1e09cd7d66b4 454 #endif
nerit 0:1e09cd7d66b4 455 }
nerit 0:1e09cd7d66b4 456 #endif
nerit 0:1e09cd7d66b4 457 //*******************************************************
nerit 0:1e09cd7d66b4 458 void upDateSpeed(){
nerit 0:1e09cd7d66b4 459 /*
nerit 0:1e09cd7d66b4 460 aggiorna dati OPUSA3
nerit 0:1e09cd7d66b4 461 val1[0] contiene il dato di velocità
nerit 0:1e09cd7d66b4 462 val1[1] contiene il byte basso della tabella allarmi
nerit 0:1e09cd7d66b4 463 uint8_t all_semiFiniti = 0; // semi finiti (generato dal relativo sensore)
nerit 0:1e09cd7d66b4 464 uint8_t all_pickSignal = 0; // errore segnale becchi (generato dal tempo tra un becco ed il successivo)
nerit 0:1e09cd7d66b4 465 uint8_t all_cellSignal = 0; // errore segnale celle (generato dal sensore tamburo )
nerit 0:1e09cd7d66b4 466 uint8_t all_lowBattery = 0; // allarme batteria (valore interno di tritecnica)
nerit 0:1e09cd7d66b4 467 uint8_t all_overCurrDC = 0; // sovracorrente motore DC (generato dal sensore di corrente)
nerit 0:1e09cd7d66b4 468 uint8_t all_stopSistem = 0; // sistema in stop (a tempo con ruota ferma)
nerit 0:1e09cd7d66b4 469 uint8_t all_upElements = 0; // elementi sollevati (generato dal relativo sensore)
nerit 0:1e09cd7d66b4 470 uint8_t all_noSeedOnCe = 0; // fallanza di semina (manca il seme nella cella)
nerit 0:1e09cd7d66b4 471 uint8_t all_cfgnErrors = 0; // errore di configurazione (incongruenza dei parametri impostati)
nerit 0:1e09cd7d66b4 472 uint8_t all_noDcRotati = 0; // ruota di semina bloccata (arriva la velocità ma non i becchi)
nerit 0:1e09cd7d66b4 473 uint8_t all_noStepRota = 0; // tamburo di semina bloccato (comando il tamburo ma non ricevo il sensore)
nerit 0:1e09cd7d66b4 474 uint8_t all_speedError = 0; // errore sensore velocità (tempo impulsi non costante)
nerit 0:1e09cd7d66b4 475 uint8_t all_noSpeedSen = 0; // mancanza segnale di velocità (sento i becchi ma non la velocita)
nerit 0:1e09cd7d66b4 476 uint8_t all_no_Zeroing = 0; // mancato azzeramento sistema (generato dal timeout del comando motore DC)
nerit 0:1e09cd7d66b4 477 uint8_t all_genericals = 0; // allarme generico
nerit 0:1e09cd7d66b4 478 val1[2] contiene il byte alto della tabella di allarmi
nerit 0:1e09cd7d66b4 479 val1[3] contiene i segnali per la diagnostica
nerit 0:1e09cd7d66b4 480 bit 0= sensore ruota fonica
nerit 0:1e09cd7d66b4 481 bit 1= sensore celle
nerit 0:1e09cd7d66b4 482 bit 2= sensore posizione
nerit 0:1e09cd7d66b4 483 bit 3= sensore becchi
nerit 0:1e09cd7d66b4 484 bit 4= motore DC
nerit 0:1e09cd7d66b4 485 bit 5= controller
nerit 0:1e09cd7d66b4 486 bit 6= motore stepper
nerit 0:1e09cd7d66b4 487 */
nerit 0:1e09cd7d66b4 488 char val1[8]={0,0,0,0,0,0,0,0};
nerit 0:1e09cd7d66b4 489
nerit 0:1e09cd7d66b4 490 val1[3]=0;
nerit 0:1e09cd7d66b4 491 val1[3]=val1[3]+(tractorSpeedRead*0x01);
nerit 3:c0f11ca4df02 492 val1[3]=val1[3]+(TBzeroPinInput*0x02);
nerit 7:c9fd242538d9 493 val1[3]=val1[3]+(DcEncoder*0x04);
nerit 3:c0f11ca4df02 494 val1[3]=val1[3]+(seedWheelZeroPinInput*0x08);
nerit 0:1e09cd7d66b4 495 #if defined(simulaPerCan)
nerit 0:1e09cd7d66b4 496 if (buttonUser==0){
nerit 0:1e09cd7d66b4 497 val1[1]=0x02;
nerit 0:1e09cd7d66b4 498 }else{
nerit 0:1e09cd7d66b4 499 val1[1]=0x00;
nerit 0:1e09cd7d66b4 500 }
nerit 0:1e09cd7d66b4 501 val1[3]=valore;
nerit 0:1e09cd7d66b4 502 valore+=1;
nerit 0:1e09cd7d66b4 503 if(valore>50){
nerit 0:1e09cd7d66b4 504 valore=0;
nerit 0:1e09cd7d66b4 505 }
nerit 0:1e09cd7d66b4 506 tractorSpeed_MtS_timed=valore;
nerit 0:1e09cd7d66b4 507 #endif
nerit 0:1e09cd7d66b4 508 allarmi();
nerit 0:1e09cd7d66b4 509 valore = totalSpeed*36.0f; // tractorSpeed_MtS_timed*36.0f;
nerit 0:1e09cd7d66b4 510 val1[0]=valore;
nerit 0:1e09cd7d66b4 511 val1[1]=alarmHighRegister; // registro alto allarmi. Parte sempre da 0x80
nerit 0:1e09cd7d66b4 512 val1[2]=alarmLowRegister; // registro basso allarmi
nerit 0:1e09cd7d66b4 513 //val1[3]=val1[3];// registro di stato degli ingressi
nerit 0:1e09cd7d66b4 514 val1[4]=resetComandi;
nerit 0:1e09cd7d66b4 515 val1[5]=cellsCounterLow;
nerit 0:1e09cd7d66b4 516 val1[6]=cellsCounterHig;
nerit 0:1e09cd7d66b4 517 #if defined(canbusActive)
nerit 0:1e09cd7d66b4 518 if(can1.write(CANMessage(TX_ID, *&val1,8))){
nerit 0:1e09cd7d66b4 519 checkState=0;
nerit 0:1e09cd7d66b4 520 }
nerit 0:1e09cd7d66b4 521 #endif
nerit 0:1e09cd7d66b4 522 }
nerit 0:1e09cd7d66b4 523 //*******************************************************
nerit 0:1e09cd7d66b4 524 void leggiCAN(){
nerit 0:1e09cd7d66b4 525 #if defined(canbusActive)
nerit 0:1e09cd7d66b4 526 if(can1.read(rxMsg)) {
nerit 0:1e09cd7d66b4 527 if (firstStart==1){
nerit 0:1e09cd7d66b4 528 #if defined(speedMaster)
nerit 5:3b95bbfe2dc9 529 sincUpdate.attach(&upDateSincro,0.009f);
nerit 7:c9fd242538d9 530 canUpdate.attach(&upDateSpeed,0.21f);
nerit 7:c9fd242538d9 531 #else
nerit 7:c9fd242538d9 532 canUpdate.attach(&upDateSpeed,0.407f);
nerit 0:1e09cd7d66b4 533 #endif
nerit 0:1e09cd7d66b4 534 firstStart=0;
nerit 0:1e09cd7d66b4 535 }
nerit 0:1e09cd7d66b4 536
nerit 0:1e09cd7d66b4 537 if (rxMsg.id==RX_ID){
nerit 0:1e09cd7d66b4 538 #if defined(pcSerial)
nerit 7:c9fd242538d9 539 #if defined(canDataReceiveda)
nerit 0:1e09cd7d66b4 540 pc.printf("Messaggio ricevuto\n");
nerit 0:1e09cd7d66b4 541 #endif
nerit 0:1e09cd7d66b4 542 #endif
nerit 0:1e09cd7d66b4 543 }
nerit 0:1e09cd7d66b4 544 if (rxMsg.id==RX_Broadcast){
nerit 0:1e09cd7d66b4 545 #if defined(pcSerial)
nerit 7:c9fd242538d9 546 #if defined(canDataReceivedb)
nerit 0:1e09cd7d66b4 547 pc.printf("BroadCast ricevuto\n");
nerit 0:1e09cd7d66b4 548 #endif
nerit 0:1e09cd7d66b4 549 #endif
nerit 0:1e09cd7d66b4 550 enableSimula= rxMsg.data[0];
nerit 0:1e09cd7d66b4 551 speedSimula = rxMsg.data[1];
nerit 0:1e09cd7d66b4 552 avviaSimula = rxMsg.data[2];
nerit 0:1e09cd7d66b4 553 selezionato = rxMsg.data[3];
nerit 0:1e09cd7d66b4 554 comandiDaCan = rxMsg.data[4];
nerit 0:1e09cd7d66b4 555 #if defined(pcSerial)
nerit 0:1e09cd7d66b4 556 #if defined(canDataReceived)
nerit 7:c9fd242538d9 557 pc.printf("Speed simula %d \n",speedSimula);
nerit 0:1e09cd7d66b4 558 #endif
nerit 0:1e09cd7d66b4 559 #endif
nerit 0:1e09cd7d66b4 560 switch (comandiDaCan){
nerit 0:1e09cd7d66b4 561 case 1:
nerit 0:1e09cd7d66b4 562 case 3:
nerit 0:1e09cd7d66b4 563 azzeraDaCan=1;
nerit 0:1e09cd7d66b4 564 resetComandi=0x01;
nerit 0:1e09cd7d66b4 565 comandiDaCan=0;
nerit 0:1e09cd7d66b4 566 break;
nerit 0:1e09cd7d66b4 567 case 2:
nerit 0:1e09cd7d66b4 568 loadDaCan=1;
nerit 0:1e09cd7d66b4 569 resetComandi=0x02;
nerit 0:1e09cd7d66b4 570 comandiDaCan=0;
nerit 0:1e09cd7d66b4 571 break;
nerit 0:1e09cd7d66b4 572 default:
nerit 0:1e09cd7d66b4 573 comandiDaCan=0;
nerit 0:1e09cd7d66b4 574 resetComandi=0xFF;
nerit 0:1e09cd7d66b4 575 break;
nerit 0:1e09cd7d66b4 576 }
nerit 0:1e09cd7d66b4 577 #if defined(pcSerial)
nerit 7:c9fd242538d9 578 #if defined(canDataReceivedR)
nerit 0:1e09cd7d66b4 579 pc.printf("Comandi: %x\n",resetComandi);
nerit 0:1e09cd7d66b4 580 #endif
nerit 0:1e09cd7d66b4 581 #endif
nerit 0:1e09cd7d66b4 582 if (speedSimula>45){
nerit 0:1e09cd7d66b4 583 speedSimula=45;
nerit 0:1e09cd7d66b4 584 }
nerit 0:1e09cd7d66b4 585 // modulo 1
nerit 0:1e09cd7d66b4 586 if (RX_ID==0x100){
nerit 0:1e09cd7d66b4 587 if ((selezionato&0x01)==0x01){
nerit 0:1e09cd7d66b4 588 simOk=1;
nerit 0:1e09cd7d66b4 589 }else{
nerit 0:1e09cd7d66b4 590 simOk=0;
nerit 0:1e09cd7d66b4 591 }
nerit 0:1e09cd7d66b4 592 }
nerit 0:1e09cd7d66b4 593 // modulo 2
nerit 0:1e09cd7d66b4 594 if (RX_ID==0x102){
nerit 0:1e09cd7d66b4 595 if ((selezionato&0x02)==0x02){
nerit 0:1e09cd7d66b4 596 simOk=1;
nerit 0:1e09cd7d66b4 597 }else{
nerit 0:1e09cd7d66b4 598 simOk=0;
nerit 0:1e09cd7d66b4 599 }
nerit 0:1e09cd7d66b4 600 }
nerit 0:1e09cd7d66b4 601 // modulo 3
nerit 0:1e09cd7d66b4 602 if (RX_ID==0x104){
nerit 0:1e09cd7d66b4 603 if ((selezionato&0x04)==0x04){
nerit 0:1e09cd7d66b4 604 simOk=1;
nerit 0:1e09cd7d66b4 605 }else{
nerit 0:1e09cd7d66b4 606 simOk=0;
nerit 0:1e09cd7d66b4 607 }
nerit 0:1e09cd7d66b4 608 }
nerit 0:1e09cd7d66b4 609 // modulo 4
nerit 0:1e09cd7d66b4 610 if (RX_ID==0x106){
nerit 0:1e09cd7d66b4 611 if ((selezionato&0x08)==0x08){
nerit 0:1e09cd7d66b4 612 simOk=1;
nerit 0:1e09cd7d66b4 613 }else{
nerit 0:1e09cd7d66b4 614 simOk=0;
nerit 0:1e09cd7d66b4 615 }
nerit 0:1e09cd7d66b4 616 }
nerit 0:1e09cd7d66b4 617 // modulo 5
nerit 0:1e09cd7d66b4 618 if (RX_ID==0x108){
nerit 0:1e09cd7d66b4 619 if ((selezionato&0x10)==0x10){
nerit 0:1e09cd7d66b4 620 simOk=1;
nerit 0:1e09cd7d66b4 621 }else{
nerit 0:1e09cd7d66b4 622 simOk=0;
nerit 0:1e09cd7d66b4 623 }
nerit 0:1e09cd7d66b4 624 }
nerit 0:1e09cd7d66b4 625
nerit 0:1e09cd7d66b4 626 }
nerit 0:1e09cd7d66b4 627 if (rxMsg.id==RX_Settings){
nerit 0:1e09cd7d66b4 628 if (tractorSpeed_MtS_timed==0.0f){
nerit 0:1e09cd7d66b4 629 pickNumber = rxMsg.data[0]; // numero di becchi installati sulla ruota di semina
nerit 0:1e09cd7d66b4 630 cellsNumber = rxMsg.data[1]; // numero di celle del tamburo
nerit 0:1e09cd7d66b4 631 deepOfSeed=(rxMsg.data[2]/10000.0f); // deep of seeding
nerit 0:1e09cd7d66b4 632 seedWheelDiameter = ((rxMsg.data[4]*0xFF)+rxMsg.data[3])/10000.0f; // seed wheel diameter setting
nerit 0:1e09cd7d66b4 633 speedWheelDiameter = ((rxMsg.data[6]*0xFF)+rxMsg.data[5])/10000.0f; // variable for tractor speed calculation (need to be set from UI) ( Unit= meters )
nerit 0:1e09cd7d66b4 634 speedWheelPulse = rxMsg.data[7]; // variable which define the number of pulse each turn of tractor speed wheel (need to be set from UI)
nerit 0:1e09cd7d66b4 635 aggiornaParametri();
nerit 0:1e09cd7d66b4 636 }
nerit 0:1e09cd7d66b4 637 }
nerit 0:1e09cd7d66b4 638 if (rxMsg.id==RX_AngoloPh){
nerit 0:1e09cd7d66b4 639 if (tractorSpeed_MtS_timed==0.0f){
nerit 0:1e09cd7d66b4 640 #if defined(M1)
nerit 0:1e09cd7d66b4 641 angoloPh = (double) rxMsg.data[0] ;
nerit 0:1e09cd7d66b4 642 aggiornaParametri();
nerit 0:1e09cd7d66b4 643 #endif
nerit 0:1e09cd7d66b4 644 #if defined(M2)
nerit 0:1e09cd7d66b4 645 angoloPh = (double) rxMsg.data[1] ;
nerit 0:1e09cd7d66b4 646 aggiornaParametri();
nerit 0:1e09cd7d66b4 647 #endif
nerit 0:1e09cd7d66b4 648 #if defined(M3)
nerit 0:1e09cd7d66b4 649 angoloPh = (double) rxMsg.data[2] ;
nerit 0:1e09cd7d66b4 650 aggiornaParametri();
nerit 0:1e09cd7d66b4 651 #endif
nerit 0:1e09cd7d66b4 652 #if defined(M4)
nerit 0:1e09cd7d66b4 653 angoloPh = (double) rxMsg.data[3] ;
nerit 0:1e09cd7d66b4 654 aggiornaParametri();
nerit 0:1e09cd7d66b4 655 #endif
nerit 0:1e09cd7d66b4 656 #if defined(M5)
nerit 0:1e09cd7d66b4 657 angoloPh = (double) rxMsg.data[4] ;
nerit 0:1e09cd7d66b4 658 aggiornaParametri();
nerit 0:1e09cd7d66b4 659 #endif
nerit 0:1e09cd7d66b4 660 #if defined(M6)
nerit 0:1e09cd7d66b4 661 angoloPh = (double) rxMsg.data[5] ;
nerit 0:1e09cd7d66b4 662 aggiornaParametri();
nerit 0:1e09cd7d66b4 663 #endif
nerit 0:1e09cd7d66b4 664 }
nerit 0:1e09cd7d66b4 665 }
nerit 0:1e09cd7d66b4 666 if (rxMsg.id==RX_AngoloAv){
nerit 0:1e09cd7d66b4 667 if (tractorSpeed_MtS_timed==0.0f){
nerit 0:1e09cd7d66b4 668 #if defined(M1)
nerit 0:1e09cd7d66b4 669 angoloAv = (double) rxMsg.data[0] ;
nerit 0:1e09cd7d66b4 670 aggiornaParametri();
nerit 0:1e09cd7d66b4 671 #endif
nerit 0:1e09cd7d66b4 672 #if defined(M2)
nerit 0:1e09cd7d66b4 673 angoloAv = (double) rxMsg.data[1] ;
nerit 0:1e09cd7d66b4 674 aggiornaParametri();
nerit 0:1e09cd7d66b4 675 #endif
nerit 0:1e09cd7d66b4 676 #if defined(M3)
nerit 0:1e09cd7d66b4 677 angoloAv = (double) rxMsg.data[2] ;
nerit 0:1e09cd7d66b4 678 aggiornaParametri();
nerit 0:1e09cd7d66b4 679 #endif
nerit 0:1e09cd7d66b4 680 #if defined(M4)
nerit 0:1e09cd7d66b4 681 angoloAv = (double) rxMsg.data[3] ;
nerit 0:1e09cd7d66b4 682 aggiornaParametri();
nerit 0:1e09cd7d66b4 683 #endif
nerit 0:1e09cd7d66b4 684 #if defined(M5)
nerit 0:1e09cd7d66b4 685 angoloAv = (double) rxMsg.data[4] ;
nerit 0:1e09cd7d66b4 686 aggiornaParametri();
nerit 0:1e09cd7d66b4 687 #endif
nerit 0:1e09cd7d66b4 688 #if defined(M6)
nerit 0:1e09cd7d66b4 689 angoloAv = (double) rxMsg.data[5] ;
nerit 0:1e09cd7d66b4 690 aggiornaParametri();
nerit 0:1e09cd7d66b4 691 #endif
nerit 0:1e09cd7d66b4 692 }
nerit 0:1e09cd7d66b4 693 }
nerit 0:1e09cd7d66b4 694 if (rxMsg.id==RX_Quinconce){
nerit 0:1e09cd7d66b4 695 if (tractorSpeed_MtS_timed==0.0f){
nerit 4:d32258ec411f 696 quinconceActive = (uint8_t) rxMsg.data[0];
nerit 5:3b95bbfe2dc9 697 quincPIDminus = (uint8_t) rxMsg.data[1];
nerit 5:3b95bbfe2dc9 698 quincPIDplus = (uint8_t) rxMsg.data[2];
nerit 5:3b95bbfe2dc9 699 quincLIMminus = (uint8_t) rxMsg.data[3];
nerit 5:3b95bbfe2dc9 700 quincLIMplus = (uint8_t) rxMsg.data[4];
nerit 5:3b95bbfe2dc9 701 quincSector = (uint8_t) rxMsg.data[5];
nerit 0:1e09cd7d66b4 702 aggiornaParametri();
nerit 0:1e09cd7d66b4 703 }
nerit 0:1e09cd7d66b4 704 }
nerit 0:1e09cd7d66b4 705 if (rxMsg.id==RX_QuincSinc){
nerit 7:c9fd242538d9 706 masterSinc = (uint32_t) rxMsg.data[3] * 0x00FF0000;
nerit 7:c9fd242538d9 707 masterSinc = masterSinc + ((uint32_t) rxMsg.data[2] * 0x0000FF00);
nerit 7:c9fd242538d9 708 masterSinc = masterSinc + ((uint32_t) rxMsg.data[1] * 0x000000FF);
nerit 7:c9fd242538d9 709 masterSinc = masterSinc + ((uint32_t) rxMsg.data[0]);
nerit 5:3b95bbfe2dc9 710 speedFromMaster = (double)rxMsg.data[4]/100.0f;
nerit 5:3b95bbfe2dc9 711 mast2_Sinc = ((uint32_t) rxMsg.data[5] * 0x0000FF00);
nerit 5:3b95bbfe2dc9 712 mast2_Sinc = mast2_Sinc + ((uint32_t) rxMsg.data[6] * 0x000000FF);
nerit 5:3b95bbfe2dc9 713 mast2_Sinc = mast2_Sinc + ((uint32_t) rxMsg.data[7]);
nerit 5:3b95bbfe2dc9 714 canDataCheck=1;
nerit 0:1e09cd7d66b4 715 }
nerit 6:3fca0ca1949e 716 if (rxMsg.id==RX_Configure){
nerit 6:3fca0ca1949e 717 uint8_t flags = rxMsg.data[0];
nerit 6:3fca0ca1949e 718 uint16_t steps = (uint32_t) rxMsg.data[1]*0xFF00;
nerit 6:3fca0ca1949e 719 steps = steps + ((uint32_t)rxMsg.data[2]);
nerit 7:c9fd242538d9 720 stepSetting();
nerit 6:3fca0ca1949e 721 cellsCountSet = rxMsg.data[3];
nerit 6:3fca0ca1949e 722 if ((flags&0x01)==0x01){
nerit 7:c9fd242538d9 723 if (encoder==false){
nerit 7:c9fd242538d9 724 encoder=true;
nerit 7:c9fd242538d9 725 DcEncoder.rise(NULL);
nerit 7:c9fd242538d9 726 dcSetting();
nerit 7:c9fd242538d9 727 }
nerit 6:3fca0ca1949e 728 }else{
nerit 7:c9fd242538d9 729 if (encoder==true){
nerit 7:c9fd242538d9 730 encoder=false;
nerit 7:c9fd242538d9 731 DcEncoder.rise(NULL);
nerit 7:c9fd242538d9 732 dcSetting();
nerit 7:c9fd242538d9 733 }
nerit 6:3fca0ca1949e 734 }
nerit 6:3fca0ca1949e 735 if ((flags&0x02)==0x02){
nerit 6:3fca0ca1949e 736 tankLevelEnable=true;
nerit 6:3fca0ca1949e 737 }else{
nerit 6:3fca0ca1949e 738 tankLevelEnable=false;
nerit 6:3fca0ca1949e 739 }
nerit 6:3fca0ca1949e 740 if ((flags&0x04)==0x04){
nerit 6:3fca0ca1949e 741 seedSensorEnable=true;
nerit 6:3fca0ca1949e 742 }else{
nerit 6:3fca0ca1949e 743 seedSensorEnable=false;
nerit 6:3fca0ca1949e 744 }
nerit 6:3fca0ca1949e 745 if ((flags&0x08)==0x08){
nerit 6:3fca0ca1949e 746 stendiNylonEnable=true;
nerit 6:3fca0ca1949e 747 }else{
nerit 6:3fca0ca1949e 748 stendiNylonEnable=false;
nerit 6:3fca0ca1949e 749 }
nerit 6:3fca0ca1949e 750 if ((flags&0x10)==0x10){
nerit 6:3fca0ca1949e 751 canDataCheckEnable=true;
nerit 6:3fca0ca1949e 752 }else{
nerit 6:3fca0ca1949e 753 canDataCheckEnable=false;
nerit 6:3fca0ca1949e 754 }
nerit 6:3fca0ca1949e 755 if ((flags&0x20)==0x20){
nerit 6:3fca0ca1949e 756 tamburoStandard=1;
nerit 6:3fca0ca1949e 757 }else{
nerit 6:3fca0ca1949e 758 tamburoStandard=0;
nerit 6:3fca0ca1949e 759 }
nerit 6:3fca0ca1949e 760 if ((flags&0x40)==0x40){
nerit 6:3fca0ca1949e 761 currentCheckEnable=true;
nerit 6:3fca0ca1949e 762 }else{
nerit 6:3fca0ca1949e 763 currentCheckEnable=false;
nerit 6:3fca0ca1949e 764 }
nerit 6:3fca0ca1949e 765 }
nerit 0:1e09cd7d66b4 766 }
nerit 0:1e09cd7d66b4 767 #endif
nerit 0:1e09cd7d66b4 768 #if defined(overWriteCanSimulation)
nerit 0:1e09cd7d66b4 769 enableSimula=1;
nerit 0:1e09cd7d66b4 770 speedSimula=25;
nerit 0:1e09cd7d66b4 771 avviaSimula=1;
nerit 0:1e09cd7d66b4 772 simOk=1;
nerit 0:1e09cd7d66b4 773 #endif
nerit 0:1e09cd7d66b4 774
nerit 0:1e09cd7d66b4 775 }
nerit 0:1e09cd7d66b4 776
nerit 0:1e09cd7d66b4 777 //*******************************************************
nerit 0:1e09cd7d66b4 778 void DC_CheckCurrent(){
nerit 0:1e09cd7d66b4 779
nerit 0:1e09cd7d66b4 780 // TODO: tabella di riferimento assorbimenti alle varie velocità al fine di gestire
nerit 0:1e09cd7d66b4 781 // gli allarmi e le correzioni di velocità
nerit 0:1e09cd7d66b4 782
nerit 0:1e09cd7d66b4 783 //float SD_analogMatrix[10]={0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f};
nerit 0:1e09cd7d66b4 784 //int SD_analogIndex[10]={0,0,0,0,0,0,0,0,0,0};
nerit 0:1e09cd7d66b4 785 // Analog reading
nerit 0:1e09cd7d66b4 786 //number = floor(number * 100) / 100;
nerit 0:1e09cd7d66b4 787 //if (pwmCheck==1){
nerit 0:1e09cd7d66b4 788 timeout.detach();
nerit 0:1e09cd7d66b4 789 for (int ii=1;ii<20;ii++){
nerit 0:1e09cd7d66b4 790 SD_analogMatrix[ii]=SD_analogMatrix[ii+1];
nerit 0:1e09cd7d66b4 791 }
nerit 0:1e09cd7d66b4 792 SD_CurrentAnalog = floor(SDcurrent.read()*100)/100; // valore in ingresso compreso tra 0.00 e 1.00
nerit 0:1e09cd7d66b4 793 SD_analogMatrix[20]=SD_CurrentAnalog;
nerit 0:1e09cd7d66b4 794 if (SDmotorPWM==0.0f){
nerit 0:1e09cd7d66b4 795 SD_CurrentStart=SD_CurrentAnalog;
nerit 0:1e09cd7d66b4 796 }
nerit 0:1e09cd7d66b4 797 float sommaTutto=0.0f;
nerit 0:1e09cd7d66b4 798 for (int ii=1;ii<21;ii++){
nerit 0:1e09cd7d66b4 799 sommaTutto=sommaTutto+SD_analogMatrix[ii];
nerit 0:1e09cd7d66b4 800 }
nerit 0:1e09cd7d66b4 801 float SD_CurrentAnalogica=sommaTutto/20.0f;
nerit 0:1e09cd7d66b4 802 SD_CurrentScaled = floor(( (SD_CurrentAnalogica - SD_CurrentStart)*3.3f) / (SD_CurrentFactor/1000.0f)*10)/10;
nerit 0:1e09cd7d66b4 803 #if defined(pcSerial)
nerit 0:1e09cd7d66b4 804 #if defined(correnteAssorbita)
nerit 0:1e09cd7d66b4 805 pc.printf("CorrenteStart: %f ",SD_CurrentStart);
nerit 0:1e09cd7d66b4 806 pc.printf(" CorrenteScaled: %f ",SD_CurrentScaled);
nerit 0:1e09cd7d66b4 807 pc.printf(" CorrenteMedia: %f \n",SD_CurrentAnalogica);
nerit 0:1e09cd7d66b4 808 #endif
nerit 0:1e09cd7d66b4 809 #endif
nerit 0:1e09cd7d66b4 810 reduceCurrent=false;
nerit 0:1e09cd7d66b4 811 incrementCurrent=false;
nerit 0:1e09cd7d66b4 812 /*
nerit 0:1e09cd7d66b4 813 if (SD_CurrentScaled < 3.0f){
nerit 0:1e09cd7d66b4 814 #if defined(canbusActive)
nerit 0:1e09cd7d66b4 815 all_lowBattery=1;
nerit 0:1e09cd7d66b4 816 #endif
nerit 0:1e09cd7d66b4 817 incrementCurrent=true;
nerit 0:1e09cd7d66b4 818 }
nerit 0:1e09cd7d66b4 819 */
nerit 4:d32258ec411f 820 if (SD_CurrentScaled > 10.0f){
nerit 4:d32258ec411f 821 timeCurr.start();
nerit 4:d32258ec411f 822 if (timeCurr.read() > 5.0f){
nerit 4:d32258ec411f 823 #if defined(canbusActive)
nerit 4:d32258ec411f 824 all_overCurrDC=1;
nerit 4:d32258ec411f 825 #endif
nerit 4:d32258ec411f 826 reduceCurrent=true;
nerit 4:d32258ec411f 827 timeCurr.reset();
nerit 4:d32258ec411f 828 }
nerit 4:d32258ec411f 829 }else{
nerit 4:d32258ec411f 830 timeCurr.stop();
nerit 0:1e09cd7d66b4 831 }
nerit 0:1e09cd7d66b4 832 //}
nerit 0:1e09cd7d66b4 833 }
nerit 0:1e09cd7d66b4 834 //*******************************************************
nerit 0:1e09cd7d66b4 835 void DC_prepare(){
nerit 0:1e09cd7d66b4 836 // direction or brake preparation
nerit 7:c9fd242538d9 837 if (DC_brake==1){
nerit 7:c9fd242538d9 838 SDmotorInA=1;
nerit 7:c9fd242538d9 839 SDmotorInB=1;
nerit 7:c9fd242538d9 840 }else{
nerit 7:c9fd242538d9 841 if (DC_forward==0){
nerit 7:c9fd242538d9 842 SDmotorInA=1;
nerit 7:c9fd242538d9 843 SDmotorInB=0;
nerit 7:c9fd242538d9 844 }else{
nerit 7:c9fd242538d9 845 SDmotorInA=0;
nerit 7:c9fd242538d9 846 SDmotorInB=1;
nerit 7:c9fd242538d9 847 }
nerit 7:c9fd242538d9 848 }
nerit 0:1e09cd7d66b4 849 // fault reading
nerit 0:1e09cd7d66b4 850 if (SDmotorInA==1){SD_faultA=1;}else{SD_faultA=0;}
nerit 0:1e09cd7d66b4 851 if (SDmotorInB==1){SD_faultB=1;}else{SD_faultB=0;}
nerit 0:1e09cd7d66b4 852 }
nerit 0:1e09cd7d66b4 853 //*******************************************************
nerit 0:1e09cd7d66b4 854 void startDelay(){
nerit 0:1e09cd7d66b4 855 int ritardo =0;
nerit 0:1e09cd7d66b4 856 ritardo = (int)((float)(dcActualDuty*800.0f));
nerit 0:1e09cd7d66b4 857 timeout.attach_us(&DC_CheckCurrent,ritardo);
nerit 0:1e09cd7d66b4 858 }
nerit 0:1e09cd7d66b4 859 //*******************************************************
nerit 4:d32258ec411f 860 void quincTrigon(){
nerit 4:d32258ec411f 861 quincClock=true;
nerit 4:d32258ec411f 862 }
nerit 4:d32258ec411f 863 void quincTrigof(){
nerit 4:d32258ec411f 864 quincClock=false;
nerit 0:1e09cd7d66b4 865 }
nerit 0:1e09cd7d66b4 866 //*******************************************************
nerit 2:d9c7430ae953 867 void quinCalc(){
nerit 0:1e09cd7d66b4 868 // riceve l'impulso di sincro dal master e fa partire il timer di verifica dell'errore
nerit 2:d9c7430ae953 869 #if !defined(mezzo)
nerit 0:1e09cd7d66b4 870 if ((quincClock==true)&&(oldQuincIn==0)){
nerit 0:1e09cd7d66b4 871 oldQuincIn=1;
nerit 0:1e09cd7d66b4 872 if (quincStart==0){
nerit 5:3b95bbfe2dc9 873 oldQuincTimeSD = (double) quincTimeSD.read_ms();
nerit 0:1e09cd7d66b4 874 quincTime.reset();
nerit 8:0e643ea7834f 875 quincTimeSD.reset();
nerit 0:1e09cd7d66b4 876 quincStart=1;
nerit 0:1e09cd7d66b4 877 }
nerit 0:1e09cd7d66b4 878 }
nerit 2:d9c7430ae953 879 if(quincClock==false){
nerit 0:1e09cd7d66b4 880 oldQuincIn=0;
nerit 0:1e09cd7d66b4 881 }
nerit 2:d9c7430ae953 882 #else
nerit 2:d9c7430ae953 883 if ((((quinconceActive==0)&&(quincClock==true))||((quinconceActive==1)&&(quincClock==false)))&&(oldQuincIn==0)){
nerit 0:1e09cd7d66b4 884 oldQuincIn=1;
nerit 0:1e09cd7d66b4 885 if (quincStart==0){
nerit 5:3b95bbfe2dc9 886 oldQuincTimeSD = (double) quincTimeSD.read_ms();
nerit 0:1e09cd7d66b4 887 quincTime.reset();
nerit 0:1e09cd7d66b4 888 quincStart=1;
nerit 0:1e09cd7d66b4 889 }
nerit 0:1e09cd7d66b4 890 }
nerit 4:d32258ec411f 891 if (quinconceActive==0){
nerit 4:d32258ec411f 892 if (quincClock==false){
nerit 4:d32258ec411f 893 oldQuincIn=0;
nerit 4:d32258ec411f 894 }
nerit 4:d32258ec411f 895 }else{
nerit 4:d32258ec411f 896 if (quincClock==true){
nerit 4:d32258ec411f 897 oldQuincIn=0;
nerit 4:d32258ec411f 898 }
nerit 0:1e09cd7d66b4 899 }
nerit 2:d9c7430ae953 900 #endif
nerit 5:3b95bbfe2dc9 901 //****************************************************************************************
nerit 7:c9fd242538d9 902 if (quincCnt>=4){
nerit 5:3b95bbfe2dc9 903 if (countPicks==0){
nerit 5:3b95bbfe2dc9 904 if ((sincroQui==1)&&(quincStart==0)){
nerit 5:3b95bbfe2dc9 905 // decelera
nerit 5:3b95bbfe2dc9 906 countPicks=1;
nerit 4:d32258ec411f 907 }
nerit 5:3b95bbfe2dc9 908 if ((sincroQui==0)&&(quincStart==1)){
nerit 5:3b95bbfe2dc9 909 // accelera
nerit 5:3b95bbfe2dc9 910 countPicks=2;
nerit 4:d32258ec411f 911 }
nerit 4:d32258ec411f 912 }
nerit 5:3b95bbfe2dc9 913 if ((sincroQui==1)&&(quincStart==1)){
nerit 5:3b95bbfe2dc9 914 if (countPicks==1){ //decelera
nerit 5:3b95bbfe2dc9 915 scostamento = oldQuincTimeSD;
nerit 5:3b95bbfe2dc9 916 if (scostamento < (tempoBecchiPerQuinc*0.75f)){
nerit 5:3b95bbfe2dc9 917 double scostPerc = (scostamento/tempoBecchiPerQuinc);
nerit 7:c9fd242538d9 918 percento -= ((double)quincPIDminus/100.0f)*(scostPerc);
nerit 5:3b95bbfe2dc9 919 #if defined(pcSerial)
nerit 5:3b95bbfe2dc9 920 #if defined(laq)
nerit 5:3b95bbfe2dc9 921 pc.printf("RALL scos2: %f tbpq: %f percento: %f \n",scostamento,tempoBecchiPerQuinc,percento);
nerit 5:3b95bbfe2dc9 922 #endif
nerit 5:3b95bbfe2dc9 923 #endif
nerit 5:3b95bbfe2dc9 924 }
nerit 7:c9fd242538d9 925 //if (scostamento <15.0f){percento=0.0f;}
nerit 5:3b95bbfe2dc9 926 }
nerit 5:3b95bbfe2dc9 927 if (countPicks==2){ //accelera
nerit 5:3b95bbfe2dc9 928 scostamento = (double)quincTime.read_ms();
nerit 5:3b95bbfe2dc9 929 if (scostamento < (tempoBecchiPerQuinc*0.75f)){
nerit 5:3b95bbfe2dc9 930 double scostPerc = (scostamento/tempoBecchiPerQuinc);
nerit 7:c9fd242538d9 931 percento += ((double)quincPIDplus/100.0f)*(scostPerc);
nerit 5:3b95bbfe2dc9 932 #if defined(pcSerial)
nerit 5:3b95bbfe2dc9 933 #if defined(laq)
nerit 5:3b95bbfe2dc9 934 pc.printf(
nerit 5:3b95bbfe2dc9 935 "ACCE scos1: %f tbpq: %f percento: %f \n",scostamento,tempoBecchiPerQuinc,percento);
nerit 5:3b95bbfe2dc9 936 #endif
nerit 5:3b95bbfe2dc9 937 #endif
nerit 5:3b95bbfe2dc9 938 }
nerit 7:c9fd242538d9 939 //if (scostamento <15.0f){percento=0.0f;}
nerit 5:3b95bbfe2dc9 940 }
nerit 5:3b95bbfe2dc9 941 sincroQui=0;
nerit 5:3b95bbfe2dc9 942 quincStart=0;
nerit 5:3b95bbfe2dc9 943 countPicks=0;
nerit 7:c9fd242538d9 944 #if !defined(speedMaster)
nerit 8:0e643ea7834f 945 if (quincCnt>=3){
nerit 7:c9fd242538d9 946 if (speedFromMaster>0.0f){
nerit 8:0e643ea7834f 947 if (enableSimula==0){
nerit 8:0e643ea7834f 948 tractorSpeed_MtS_timed = speedFromMaster + percento;
nerit 8:0e643ea7834f 949 }
nerit 7:c9fd242538d9 950 }
nerit 7:c9fd242538d9 951 }
nerit 7:c9fd242538d9 952 #endif
nerit 5:3b95bbfe2dc9 953 }
nerit 5:3b95bbfe2dc9 954
nerit 5:3b95bbfe2dc9 955 //*******************************************************************
nerit 5:3b95bbfe2dc9 956 if (canDataCheckEnable==true){
nerit 5:3b95bbfe2dc9 957 if (canDataCheck==1){ // sincro da comunicazione can del valore di posizione del tamburo master
nerit 5:3b95bbfe2dc9 958 canDataCheck=0;
nerit 7:c9fd242538d9 959 double parametro = SDsectorStep/3.0f;
nerit 5:3b95bbfe2dc9 960 double differenza=0.0f;
nerit 5:3b95bbfe2dc9 961 #if defined(mezzo)
nerit 5:3b95bbfe2dc9 962 if (quinconceActive==1){
nerit 5:3b95bbfe2dc9 963 differenza = (double)masterSinc - (double)prePosSD;
nerit 5:3b95bbfe2dc9 964 }else{
nerit 5:3b95bbfe2dc9 965 differenza = (double)mast2_Sinc - (double)prePosSD;
nerit 5:3b95bbfe2dc9 966 }
nerit 5:3b95bbfe2dc9 967 #else
nerit 5:3b95bbfe2dc9 968 differenza = (double)mast2_Sinc - (double)prePosSD;
nerit 5:3b95bbfe2dc9 969 #endif
nerit 5:3b95bbfe2dc9 970 if ((differenza > 0.0f)&&(differenza < parametro)){
nerit 5:3b95bbfe2dc9 971 double diffPerc = differenza / parametro;
nerit 7:c9fd242538d9 972 percento += ((double)quincPIDplus/100.0f)*abs(diffPerc);
nerit 5:3b95bbfe2dc9 973 #if defined(pcSerial)
nerit 5:3b95bbfe2dc9 974 #if defined(quinca)
nerit 5:3b95bbfe2dc9 975 pc.printf("m1 %d m2 %d prePo %d diffe %f perce %f parm %f %\n",masterSinc, mast2_Sinc,prePosSD,differenza,percento, parametro);
nerit 5:3b95bbfe2dc9 976 #endif
nerit 5:3b95bbfe2dc9 977 #endif
nerit 5:3b95bbfe2dc9 978 }
nerit 5:3b95bbfe2dc9 979 if ((differenza < 0.0f)&&(abs(differenza) < parametro)){
nerit 5:3b95bbfe2dc9 980 double diffPerc = (double)differenza / parametro;
nerit 7:c9fd242538d9 981 percento -= ((double)quincPIDminus/100.0f)*abs(diffPerc);
nerit 5:3b95bbfe2dc9 982 #if defined(pcSerial)
nerit 5:3b95bbfe2dc9 983 #if defined(quinca)
nerit 5:3b95bbfe2dc9 984 pc.printf("m1 %d m2 %d prePo %d diffe %f perce %f parm %f %\n",masterSinc, mast2_Sinc,prePosSD,differenza,percento, parametro);
nerit 5:3b95bbfe2dc9 985 #endif
nerit 5:3b95bbfe2dc9 986 #endif
nerit 5:3b95bbfe2dc9 987 }
nerit 7:c9fd242538d9 988
nerit 7:c9fd242538d9 989 #if !defined(speedMaster)
nerit 8:0e643ea7834f 990 if (quincCnt>=3){
nerit 7:c9fd242538d9 991 if (speedFromMaster>0.0f){
nerit 8:0e643ea7834f 992 if (enableSimula==0){
nerit 8:0e643ea7834f 993 tractorSpeed_MtS_timed = speedFromMaster + percento;
nerit 8:0e643ea7834f 994 }
nerit 7:c9fd242538d9 995 }
nerit 7:c9fd242538d9 996 }
nerit 7:c9fd242538d9 997 #endif
nerit 5:3b95bbfe2dc9 998 }
nerit 5:3b95bbfe2dc9 999 }
nerit 5:3b95bbfe2dc9 1000
nerit 5:3b95bbfe2dc9 1001 }
nerit 5:3b95bbfe2dc9 1002 if ((percento) > ((double) quincLIMplus/100.0f)){
nerit 5:3b95bbfe2dc9 1003 percento= (double)quincLIMplus/100.0f;
nerit 5:3b95bbfe2dc9 1004 }
nerit 5:3b95bbfe2dc9 1005 if ((percento) < (((double)quincLIMminus*-1.0f)/100.0f)){
nerit 5:3b95bbfe2dc9 1006 percento=((double)quincLIMminus*-1.0f)/100.0f;
nerit 0:1e09cd7d66b4 1007 }
nerit 0:1e09cd7d66b4 1008 }
nerit 7:c9fd242538d9 1009 // ----------------------------------------
nerit 7:c9fd242538d9 1010 #if defined(seedSensor)
nerit 7:c9fd242538d9 1011 void resetDelay(){
nerit 7:c9fd242538d9 1012 delaySeedCheck.reset();
nerit 7:c9fd242538d9 1013 delaySeedCheck.stop();
nerit 7:c9fd242538d9 1014 }
nerit 7:c9fd242538d9 1015 #endif
nerit 0:1e09cd7d66b4 1016 // ------------------------------------------------------------------------------------------------------------------------------------------------------------------
nerit 0:1e09cd7d66b4 1017 // MAIN SECTION
nerit 3:c0f11ca4df02 1018 // ---------------------------------------------------------------------------------------------------------------------------------------------------------------
nerit 0:1e09cd7d66b4 1019
nerit 0:1e09cd7d66b4 1020 int main()
nerit 0:1e09cd7d66b4 1021 {
nerit 5:3b95bbfe2dc9 1022 //wait(1.0f);
nerit 0:1e09cd7d66b4 1023 wd.Configure(2); //watchdog set at xx seconds
nerit 7:c9fd242538d9 1024
nerit 7:c9fd242538d9 1025 stepSetting();
nerit 7:c9fd242538d9 1026
nerit 0:1e09cd7d66b4 1027 for (int a=0; a<5;a++){
nerit 0:1e09cd7d66b4 1028 mediaSpeed[a]=0;
nerit 0:1e09cd7d66b4 1029 }
nerit 0:1e09cd7d66b4 1030
nerit 0:1e09cd7d66b4 1031
nerit 0:1e09cd7d66b4 1032 // DC reset ad set
nerit 0:1e09cd7d66b4 1033 int decima = 100;
nerit 0:1e09cd7d66b4 1034 wait_ms(200);
nerit 0:1e09cd7d66b4 1035 SD_CurrentStart=floor(SDcurrent.read()*decima)/decima;
nerit 0:1e09cd7d66b4 1036 wait_ms(2);
nerit 0:1e09cd7d66b4 1037 SD_CurrentStart+=floor(SDcurrent.read()*decima)/decima;
nerit 0:1e09cd7d66b4 1038 wait_ms(1);
nerit 0:1e09cd7d66b4 1039 SD_CurrentStart+=floor(SDcurrent.read()*decima)/decima;
nerit 0:1e09cd7d66b4 1040 wait_ms(3);
nerit 0:1e09cd7d66b4 1041 SD_CurrentStart+=floor(SDcurrent.read()*decima)/decima;
nerit 0:1e09cd7d66b4 1042 SD_CurrentStart=(floor((SD_CurrentStart/4.0f)*decima)/decima)-0.01f;
nerit 0:1e09cd7d66b4 1043 wait_ms(100);
nerit 0:1e09cd7d66b4 1044 DC_prepare();
nerit 0:1e09cd7d66b4 1045
nerit 0:1e09cd7d66b4 1046 speedTimer.start(); // speed pulse timer
nerit 0:1e09cd7d66b4 1047 intraPickTimer.start();
nerit 0:1e09cd7d66b4 1048 speedTimeOut.start();
nerit 0:1e09cd7d66b4 1049 speedFilter.start();
nerit 0:1e09cd7d66b4 1050 seedFilter.start();
nerit 0:1e09cd7d66b4 1051 TBfilter.start();
nerit 0:1e09cd7d66b4 1052 sincroTimer.start();
nerit 0:1e09cd7d66b4 1053 rotationTimeOut.start();
nerit 0:1e09cd7d66b4 1054 metalTimer.start();
nerit 0:1e09cd7d66b4 1055 quincTime.start();
nerit 1:e88bf5011af6 1056 quincTimeSD.start();
nerit 7:c9fd242538d9 1057 //intraEncoTimer.start();
nerit 0:1e09cd7d66b4 1058
nerit 0:1e09cd7d66b4 1059 //*******************************************************
nerit 0:1e09cd7d66b4 1060 // controls for check DC motor current
nerit 0:1e09cd7d66b4 1061 pwmCheck.rise(&startDelay);
nerit 0:1e09cd7d66b4 1062
nerit 0:1e09cd7d66b4 1063 if (inProva==0){
nerit 0:1e09cd7d66b4 1064 tractorSpeedRead.rise(&tractorReadSpeed);
nerit 0:1e09cd7d66b4 1065 #if !defined(speedMaster)
nerit 4:d32258ec411f 1066 quinconceIn.rise(&quincTrigon);
nerit 4:d32258ec411f 1067 quinconceIn.fall(&quincTrigof);
nerit 0:1e09cd7d66b4 1068 #endif
nerit 5:3b95bbfe2dc9 1069 #if defined(speedMaster)
nerit 5:3b95bbfe2dc9 1070 tftUpdate.attach(&videoUpdate,0.50f);
nerit 5:3b95bbfe2dc9 1071 #endif
nerit 0:1e09cd7d66b4 1072 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
nerit 7:c9fd242538d9 1073 dcSetting();
nerit 7:c9fd242538d9 1074 #if defined(seedSensor)
nerit 8:0e643ea7834f 1075 seedCheck.fall(&seedSensorTask);
nerit 7:c9fd242538d9 1076 #endif
nerit 0:1e09cd7d66b4 1077 }else{
nerit 0:1e09cd7d66b4 1078 tftUpdate.attach(&videoUpdate,0.125f);
nerit 0:1e09cd7d66b4 1079 }
nerit 0:1e09cd7d66b4 1080
nerit 0:1e09cd7d66b4 1081 aggiornaParametri();
nerit 0:1e09cd7d66b4 1082
nerit 0:1e09cd7d66b4 1083 SDmotorPWM.period_us(periodoSD); // frequency 1KHz pilotaggio motore DC
nerit 7:c9fd242538d9 1084 SDmotorPWM.write(1.0f); // duty cycle = stop
nerit 7:c9fd242538d9 1085 TBmotorDirecti=1; // tb motor direction set
nerit 0:1e09cd7d66b4 1086
nerit 0:1e09cd7d66b4 1087 #if defined(provaStepper)
nerit 7:c9fd242538d9 1088 TBmotorRst=0;
nerit 7:c9fd242538d9 1089 TBticker.attach_us(&step_TBPulseOut,1000.0f); // clock time are seconds and attach seed motor stepper controls
nerit 0:1e09cd7d66b4 1090 #endif // end prova stepper
nerit 0:1e09cd7d66b4 1091
nerit 0:1e09cd7d66b4 1092 #if defined(canbusActive)
nerit 0:1e09cd7d66b4 1093 can1.attach(&leggiCAN, CAN::RxIrq);
nerit 0:1e09cd7d66b4 1094 #endif
nerit 0:1e09cd7d66b4 1095
nerit 0:1e09cd7d66b4 1096 //**************************************************************************************************************
nerit 0:1e09cd7d66b4 1097 // MAIN LOOP
nerit 0:1e09cd7d66b4 1098 //**************************************************************************************************************
nerit 0:1e09cd7d66b4 1099 while (true){
nerit 0:1e09cd7d66b4 1100 // ripetitore segnale di velocità
nerit 0:1e09cd7d66b4 1101 if (tractorSpeedRead==0){speedClock=0;}
nerit 0:1e09cd7d66b4 1102
nerit 0:1e09cd7d66b4 1103 // inversione segnali ingressi
nerit 4:d32258ec411f 1104 #if !defined(simulaBanco)
nerit 4:d32258ec411f 1105 seedWheelZeroPinInput = !seedWheelZeroPinInputRev;
nerit 4:d32258ec411f 1106 #else
nerit 4:d32258ec411f 1107 if ((prePosSD-500) >= SDsectorStep){
nerit 4:d32258ec411f 1108 seedWheelZeroPinInput=1;
nerit 4:d32258ec411f 1109 }
nerit 4:d32258ec411f 1110 if ((prePosSD > 500)&&(prePosSD<580)){
nerit 4:d32258ec411f 1111 seedWheelZeroPinInput=0;
nerit 4:d32258ec411f 1112 }
nerit 4:d32258ec411f 1113 #endif
nerit 0:1e09cd7d66b4 1114 TBzeroPinInput = !TBzeroPinInputRev;
nerit 0:1e09cd7d66b4 1115
nerit 0:1e09cd7d66b4 1116 // se quinconce attivo ed unita' master invia segnale di sincro
nerit 0:1e09cd7d66b4 1117 #if defined(speedMaster)
nerit 0:1e09cd7d66b4 1118 if (seedWheelZeroPinInput==1){
nerit 7:c9fd242538d9 1119 quinconceOut=0;
nerit 0:1e09cd7d66b4 1120 }
nerit 5:3b95bbfe2dc9 1121 if (((double)(prePosSD-500) >= (SDsectorStep/((double)quincSector)))&&(quinconceOut=1)){
nerit 7:c9fd242538d9 1122 quinconceOut=1;
nerit 7:c9fd242538d9 1123 }
nerit 7:c9fd242538d9 1124 if (quinconceActive==1){
nerit 7:c9fd242538d9 1125 if ((quinconceOut==1)&&(oldQuinconceOut==1)){
nerit 5:3b95bbfe2dc9 1126 posForQuinc=500;
nerit 7:c9fd242538d9 1127 oldQuinconceOut=0;
nerit 7:c9fd242538d9 1128 }
nerit 7:c9fd242538d9 1129 if (((double)posForQuinc-500.0f)> (SDsectorStep-200)){
nerit 7:c9fd242538d9 1130 oldQuinconceOut=1;
nerit 7:c9fd242538d9 1131 }
nerit 7:c9fd242538d9 1132 }
nerit 0:1e09cd7d66b4 1133 #endif
nerit 0:1e09cd7d66b4 1134
nerit 0:1e09cd7d66b4 1135 #if defined(overWriteCanSimulation)
nerit 0:1e09cd7d66b4 1136 leggiCAN();
nerit 0:1e09cd7d66b4 1137 #endif
nerit 0:1e09cd7d66b4 1138
nerit 0:1e09cd7d66b4 1139 // simulazione velocita
nerit 0:1e09cd7d66b4 1140 if (enableSimula==1){
nerit 0:1e09cd7d66b4 1141 double TMT = 0.0f;
nerit 0:1e09cd7d66b4 1142 if (speedSimula > 0){
nerit 0:1e09cd7d66b4 1143 TMT = (double)(speedSimula) * 100.0f /3600.0f;
nerit 0:1e09cd7d66b4 1144 pulseSpeedInterval = pulseDistance / TMT;
nerit 0:1e09cd7d66b4 1145 }else{
nerit 0:1e09cd7d66b4 1146 pulseSpeedInterval = 10000.0f;
nerit 0:1e09cd7d66b4 1147 }
nerit 0:1e09cd7d66b4 1148 if (avviaSimula==1){
nerit 0:1e09cd7d66b4 1149 if(oldSimulaSpeed!=pulseSpeedInterval){
nerit 7:c9fd242538d9 1150 #if defined(pcSerial)
nerit 7:c9fd242538d9 1151 #if defined(canDataReceived)
nerit 7:c9fd242538d9 1152 pc.printf("Pulseinterval %f \n",pulseSpeedInterval);
nerit 7:c9fd242538d9 1153 #endif
nerit 7:c9fd242538d9 1154 #endif
nerit 0:1e09cd7d66b4 1155 spedSimclock.attach_us(&speedSimulationClock,pulseSpeedInterval);
nerit 0:1e09cd7d66b4 1156 oldSimulaSpeed=pulseSpeedInterval;
nerit 0:1e09cd7d66b4 1157 }
nerit 0:1e09cd7d66b4 1158 }else{
nerit 0:1e09cd7d66b4 1159 oldSimulaSpeed=10000.0f;
nerit 0:1e09cd7d66b4 1160 spedSimclock.detach();
nerit 0:1e09cd7d66b4 1161 }
nerit 0:1e09cd7d66b4 1162 }else{
nerit 0:1e09cd7d66b4 1163 spedSimclock.detach();
nerit 0:1e09cd7d66b4 1164 }
nerit 0:1e09cd7d66b4 1165
nerit 0:1e09cd7d66b4 1166 //*******************************************************
nerit 0:1e09cd7d66b4 1167 // determina se sono in bassa velocità per il controllo di TB
nerit 0:1e09cd7d66b4 1168 if (speedOfSeedWheel<=minSeedSpeed){
nerit 0:1e09cd7d66b4 1169 if (lowSpeedRequired==0){
nerit 0:1e09cd7d66b4 1170 ritardaLowSpeed.reset();
nerit 0:1e09cd7d66b4 1171 ritardaLowSpeed.start();
nerit 0:1e09cd7d66b4 1172 }
nerit 0:1e09cd7d66b4 1173 lowSpeedRequired=1;
nerit 0:1e09cd7d66b4 1174 }else{
nerit 0:1e09cd7d66b4 1175 if (lowSpeedRequired==1){
nerit 0:1e09cd7d66b4 1176 lowSpeedRequired=0;
nerit 0:1e09cd7d66b4 1177 ritardaLowSpeed.reset();
nerit 0:1e09cd7d66b4 1178 ritardaLowSpeed.stop();
nerit 0:1e09cd7d66b4 1179 }
nerit 0:1e09cd7d66b4 1180 }
nerit 0:1e09cd7d66b4 1181
nerit 0:1e09cd7d66b4 1182 if (ritardaLowSpeed.read_ms()> 2000){
nerit 0:1e09cd7d66b4 1183 lowSpeed=1;
nerit 0:1e09cd7d66b4 1184 }else{
nerit 0:1e09cd7d66b4 1185 lowSpeed=0;
nerit 0:1e09cd7d66b4 1186 }
nerit 0:1e09cd7d66b4 1187
nerit 0:1e09cd7d66b4 1188
nerit 0:1e09cd7d66b4 1189 //**************************************************************************************************************
nerit 0:1e09cd7d66b4 1190 //**************************************************************************************************************
nerit 0:1e09cd7d66b4 1191 // LOGICAL CONTROLS
nerit 0:1e09cd7d66b4 1192 //**************************************************************************************************************
nerit 0:1e09cd7d66b4 1193 //**************************************************************************************************************
nerit 0:1e09cd7d66b4 1194
nerit 0:1e09cd7d66b4 1195 if (inProva==0){
nerit 7:c9fd242538d9 1196 if ((startCycleSimulation==0)&&(enableSimula==0)){
nerit 7:c9fd242538d9 1197 zeroRequestBuf=1;
nerit 7:c9fd242538d9 1198 runRequestBuf=0;
nerit 7:c9fd242538d9 1199 enableCycle=1;
nerit 7:c9fd242538d9 1200 }else{
nerit 7:c9fd242538d9 1201 zeroRequestBuf=1;
nerit 7:c9fd242538d9 1202 runRequestBuf=0;
nerit 7:c9fd242538d9 1203 enableCycle=1;
nerit 7:c9fd242538d9 1204 }
nerit 7:c9fd242538d9 1205 if ((tractorSpeedRead==0)&&(speedFilter.read_ms()>3)){
nerit 7:c9fd242538d9 1206 oldTractorSpeedRead=0;
nerit 7:c9fd242538d9 1207 }
nerit 0:1e09cd7d66b4 1208 // ----------------------------------------
nerit 0:1e09cd7d66b4 1209 // filtra il segnale dei becchi per misurare il tempo di intervallo tra loro
nerit 0:1e09cd7d66b4 1210 // ----------------------------------------
nerit 4:d32258ec411f 1211 if ((seedWheelZeroPinInput==0)&&(oldSeedWheelZeroPinInput==1)){
nerit 0:1e09cd7d66b4 1212 if(seedFilter.read_ms()>=4){
nerit 0:1e09cd7d66b4 1213 oldSeedWheelZeroPinInput=0;
nerit 0:1e09cd7d66b4 1214 SDzeroDebounced=0;
nerit 0:1e09cd7d66b4 1215 }
nerit 0:1e09cd7d66b4 1216 }
nerit 0:1e09cd7d66b4 1217 if ((seedWheelZeroPinInput==1)&&(oldSeedWheelZeroPinInput==0)){
nerit 5:3b95bbfe2dc9 1218 timeIntraPick = (double)intraPickTimer.read_ms();
nerit 0:1e09cd7d66b4 1219 prePosSD=500; // preposizionamento SD
nerit 0:1e09cd7d66b4 1220 intraPickTimer.reset();
nerit 0:1e09cd7d66b4 1221 rotationTimeOut.reset();
nerit 0:1e09cd7d66b4 1222 seedFilter.reset();
nerit 0:1e09cd7d66b4 1223 sincroTimer.reset();
nerit 0:1e09cd7d66b4 1224 oldSeedWheelZeroPinInput=1;
nerit 8:0e643ea7834f 1225 quincTime.reset();
nerit 1:e88bf5011af6 1226 quincTimeSD.reset();
nerit 0:1e09cd7d66b4 1227 SDzeroDebounced=1;
nerit 4:d32258ec411f 1228 sincroQui=1;
nerit 0:1e09cd7d66b4 1229 SDwheelTimer.reset();
nerit 5:3b95bbfe2dc9 1230 #if defined(speedMaster)
nerit 5:3b95bbfe2dc9 1231 if (quinconceActive==0){
nerit 5:3b95bbfe2dc9 1232 posForQuinc=500;
nerit 5:3b95bbfe2dc9 1233 }
nerit 5:3b95bbfe2dc9 1234 #endif
nerit 0:1e09cd7d66b4 1235 if (quincCnt<10){
nerit 0:1e09cd7d66b4 1236 quincCnt++;
nerit 0:1e09cd7d66b4 1237 }
nerit 0:1e09cd7d66b4 1238 if ((aspettaStart==0)&&(lowSpeed==1)){
nerit 0:1e09cd7d66b4 1239 beccoPronto=1;
nerit 0:1e09cd7d66b4 1240 }
nerit 0:1e09cd7d66b4 1241 SDzeroCyclePulse=1;
nerit 0:1e09cd7d66b4 1242 lockStart=0;
nerit 0:1e09cd7d66b4 1243 double fase1=0.0f;
nerit 0:1e09cd7d66b4 1244 forzaFase=0;
nerit 0:1e09cd7d66b4 1245 double limite=fixedStepGiroSD/pickNumber;
nerit 0:1e09cd7d66b4 1246 if (tamburoStandard==0){
nerit 0:1e09cd7d66b4 1247 fase1=TBdeltaStep;
nerit 0:1e09cd7d66b4 1248 }else{
nerit 0:1e09cd7d66b4 1249 if(speedForCorrection >= speedOfSeedWheel){
nerit 0:1e09cd7d66b4 1250 fase1=TBdeltaStep;
nerit 0:1e09cd7d66b4 1251 }else{
nerit 6:3fca0ca1949e 1252 //fase1=(TBdeltaStep)-(((speedOfSeedWheel-speedForCorrection)/maxWorkSpeed)*(TBfaseStep));
nerit 6:3fca0ca1949e 1253 fase1=(TBdeltaStep)-(((speedOfSeedWheel)/maxWorkSpeed)*(TBfaseStep));
nerit 0:1e09cd7d66b4 1254 }
nerit 0:1e09cd7d66b4 1255 if (fase1 > limite){
nerit 0:1e09cd7d66b4 1256 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)
nerit 0:1e09cd7d66b4 1257 //forzaFase=1;
nerit 0:1e09cd7d66b4 1258 }
nerit 0:1e09cd7d66b4 1259 if ((fase1 > (limite -20.0f))&&(fase1<(limite +5.0f))){
nerit 0:1e09cd7d66b4 1260 fase1 = limite -20.0f; // se la fase è molto vicina al limite interpone uno spazio minimo di lavoro del sincronismo
nerit 0:1e09cd7d66b4 1261 forzaFase=1;
nerit 0:1e09cd7d66b4 1262 }
nerit 0:1e09cd7d66b4 1263 trigRepos=1;
nerit 0:1e09cd7d66b4 1264 }
nerit 0:1e09cd7d66b4 1265 fase = (uint32_t)fase1+500;
nerit 0:1e09cd7d66b4 1266 #if defined(pcSerial)
nerit 0:1e09cd7d66b4 1267 #if defined(inCorre)
nerit 0:1e09cd7d66b4 1268 pc.printf(" limite %f", limite);
nerit 0:1e09cd7d66b4 1269 pc.printf(" delta %f", TBdeltaStep);
nerit 0:1e09cd7d66b4 1270 pc.printf(" faseStep %f", TBfaseStep);
nerit 0:1e09cd7d66b4 1271 pc.printf(" fase %d",fase);
nerit 0:1e09cd7d66b4 1272 pc.printf(" forzaFase %d",forzaFase);
nerit 0:1e09cd7d66b4 1273 pc.printf(" trigRepos %d", trigRepos);
nerit 0:1e09cd7d66b4 1274 pc.printf(" ActualSD: %d",SDactualPosition);
nerit 0:1e09cd7d66b4 1275 pc.printf(" SpeedWheel: %f",speedOfSeedWheel);
nerit 0:1e09cd7d66b4 1276 pc.printf(" SPEED: %f \n",tractorSpeed_MtS_timed);
nerit 0:1e09cd7d66b4 1277 #endif
nerit 0:1e09cd7d66b4 1278 #endif
nerit 0:1e09cd7d66b4 1279 if (timeIntraPick >= (memoIntraPick*2)){
nerit 0:1e09cd7d66b4 1280 if ((aspettaStart==0)&&(zeroRequestBuf==0)){
nerit 0:1e09cd7d66b4 1281 if (firstStart==0){
nerit 0:1e09cd7d66b4 1282 all_pickSignal=1;
nerit 0:1e09cd7d66b4 1283 }
nerit 0:1e09cd7d66b4 1284 }
nerit 0:1e09cd7d66b4 1285 }
nerit 0:1e09cd7d66b4 1286 memoIntraPick = timeIntraPick;
nerit 5:3b95bbfe2dc9 1287 if ((speedFromPick==1)&&(encoder==false)){
nerit 0:1e09cd7d66b4 1288 speedOfSeedWheel=((seedPerimeter / pickNumber)/timeIntraPick)*1000.0f;
nerit 0:1e09cd7d66b4 1289 #if defined(pcSerial)
nerit 4:d32258ec411f 1290 #if defined(Qnca)
nerit 4:d32258ec411f 1291 pc.printf("perim: %f pickN: %f sped: %f\n", seedPerimeter, pickNumber,speedOfSeedWheel);
nerit 0:1e09cd7d66b4 1292 #endif
nerit 0:1e09cd7d66b4 1293 #endif
nerit 0:1e09cd7d66b4 1294 }
nerit 7:c9fd242538d9 1295 if (encoder==false){
nerit 7:c9fd242538d9 1296 pulseRised2=1;
nerit 7:c9fd242538d9 1297 }
nerit 4:d32258ec411f 1298 #if defined(speedMaster)
nerit 4:d32258ec411f 1299 if ((tractorSpeed_MtS_timed==0.0f)&&(zeroCycle==0)&&(zeroCycleEnd==1)){
nerit 4:d32258ec411f 1300 if ((firstStart==0)&&(simOk==0)&&(enableSimula==0)&&(zeroCycleEnd==1)){
nerit 4:d32258ec411f 1301 all_noSpeedSen=1;
nerit 4:d32258ec411f 1302 }
nerit 4:d32258ec411f 1303 }
nerit 5:3b95bbfe2dc9 1304 double oldLastPr = (double)oldLastPulseRead*1.5f;
nerit 0:1e09cd7d66b4 1305 if((double)speedTimeOut.read_us()> (oldLastPr)){
nerit 0:1e09cd7d66b4 1306 if ((firstStart==0)&&(simOk==0)&&(enableSimula==0)&&(zeroCycleEnd==1)){
nerit 0:1e09cd7d66b4 1307 all_speedError =1;
nerit 0:1e09cd7d66b4 1308 }
nerit 0:1e09cd7d66b4 1309 }
nerit 0:1e09cd7d66b4 1310 #endif
nerit 0:1e09cd7d66b4 1311 //*******************************************
nerit 0:1e09cd7d66b4 1312 // esegue calcolo clock per la generazione della posizione teorica
nerit 0:1e09cd7d66b4 1313 // la realtà in base al segnale di presenza del becco
nerit 0:1e09cd7d66b4 1314 realSpeed = speedOfSeedWheel;
nerit 0:1e09cd7d66b4 1315 realGiroSD = seedPerimeter / realSpeed;
nerit 0:1e09cd7d66b4 1316 tempoBecco = (realGiroSD/360.0f)*16000.0f;
nerit 0:1e09cd7d66b4 1317 frequenzaReale = fixedStepGiroSD/realGiroSD;
nerit 0:1e09cd7d66b4 1318 semiPeriodoReale = (1000000.0f/frequenzaReale);
nerit 0:1e09cd7d66b4 1319 tempoTraBecchi_mS = 0.0f;
nerit 0:1e09cd7d66b4 1320 seedWheelRPM = (speedOfSeedWheel)*K_WheelRPM ; // calcola i giri al minuto della ruota di semina 7.37 31,75
nerit 0:1e09cd7d66b4 1321 TBrpm = seedWheelRPM*rapportoRuote; // 5.896 31,75
nerit 0:1e09cd7d66b4 1322 TBfrequency = (TBrpm*K_TBfrequency); // 130Hz a 0,29Mts 1397,00 a 1,25mt/s con 15 becchi e 15 celle
nerit 0:1e09cd7d66b4 1323 TBperiod=1000000.0f/TBfrequency; // 715uS
nerit 0:1e09cd7d66b4 1324 }
nerit 0:1e09cd7d66b4 1325 // ----------------------------------------
nerit 0:1e09cd7d66b4 1326 // check SD fase
nerit 0:1e09cd7d66b4 1327 if ((prePosSD >= fase)||(forzaFase==1)){//&&(prePosSD < (fase +30))){
nerit 0:1e09cd7d66b4 1328 forzaFase=0;
nerit 0:1e09cd7d66b4 1329 if (trigRepos==1){
nerit 0:1e09cd7d66b4 1330 SDactualPosition=0;
nerit 0:1e09cd7d66b4 1331 if ((countCicli<30)&&(trigCicli==0)){countCicli++;trigCicli=1;}
nerit 0:1e09cd7d66b4 1332 if(countCicli>=cicliAspettaStart){aspettaStart=0;}
nerit 0:1e09cd7d66b4 1333 if ((lowSpeed==0)&&(aspettaStart==0)&&(lockStart==0)){syncroCheck=1;beccoPronto=0;}
nerit 0:1e09cd7d66b4 1334 if (trigTB==0){
nerit 0:1e09cd7d66b4 1335 inhibit=1;
nerit 0:1e09cd7d66b4 1336 trigSD=1;
nerit 0:1e09cd7d66b4 1337 }else{
nerit 0:1e09cd7d66b4 1338 inhibit=0;
nerit 0:1e09cd7d66b4 1339 trigTB=0;
nerit 0:1e09cd7d66b4 1340 trigSD=0;
nerit 0:1e09cd7d66b4 1341 }
nerit 0:1e09cd7d66b4 1342 trigRepos=0;
nerit 0:1e09cd7d66b4 1343 }
nerit 0:1e09cd7d66b4 1344 }else{
nerit 0:1e09cd7d66b4 1345 trigCicli=0;
nerit 0:1e09cd7d66b4 1346 }
nerit 0:1e09cd7d66b4 1347 // ----------------------------------------
nerit 0:1e09cd7d66b4 1348 // filtra il segnale del tamburo per lo stop in fase del tamburo stesso
nerit 0:1e09cd7d66b4 1349 if (TBzeroPinInput==0){if (TBfilter.read_ms()>=5){oldTBzeroPinInput=0;}}
nerit 0:1e09cd7d66b4 1350 if ((TBzeroPinInput==1)&&(oldTBzeroPinInput==0)){
nerit 0:1e09cd7d66b4 1351 oldTBzeroPinInput=1;
nerit 0:1e09cd7d66b4 1352 if (loadDaCanInCorso==0){
nerit 0:1e09cd7d66b4 1353 stopCicloTB=1;
nerit 0:1e09cd7d66b4 1354 startCicloTB=0;
nerit 0:1e09cd7d66b4 1355 }
nerit 0:1e09cd7d66b4 1356 TBfilter.reset();
nerit 0:1e09cd7d66b4 1357 TBzeroCyclePulse=1;
nerit 0:1e09cd7d66b4 1358 TBactualPosition=0;
nerit 0:1e09cd7d66b4 1359 if (cellsCounterLow < 0xFF){
nerit 0:1e09cd7d66b4 1360 cellsCounterLow++;
nerit 0:1e09cd7d66b4 1361 }else{
nerit 0:1e09cd7d66b4 1362 cellsCounterHig++;
nerit 0:1e09cd7d66b4 1363 cellsCounterLow=0;
nerit 0:1e09cd7d66b4 1364 }
nerit 0:1e09cd7d66b4 1365 if (loadDaCanInCorso==1){
nerit 0:1e09cd7d66b4 1366 cntCellsForLoad++;
nerit 0:1e09cd7d66b4 1367 if (cntCellsForLoad >= 5){
nerit 0:1e09cd7d66b4 1368 stopCicloTB=1;
nerit 0:1e09cd7d66b4 1369 cntCellsForLoad=0;
nerit 0:1e09cd7d66b4 1370 }
nerit 0:1e09cd7d66b4 1371 }else{
nerit 0:1e09cd7d66b4 1372 cntCellsForLoad=0;
nerit 0:1e09cd7d66b4 1373 }
nerit 0:1e09cd7d66b4 1374 if (trigSD==0){
nerit 0:1e09cd7d66b4 1375 inhibit=1;
nerit 0:1e09cd7d66b4 1376 trigTB=1;
nerit 0:1e09cd7d66b4 1377 }else{
nerit 0:1e09cd7d66b4 1378 inhibit=0;
nerit 0:1e09cd7d66b4 1379 trigTB=0;
nerit 0:1e09cd7d66b4 1380 trigSD=0;
nerit 0:1e09cd7d66b4 1381 }
nerit 0:1e09cd7d66b4 1382 // torna indietro per sbloccare il tamburo
nerit 7:c9fd242538d9 1383 if ((TBmotorDirecti==0)&&(erroreTamburo==1)){
nerit 0:1e09cd7d66b4 1384 cntCellsForReload++;
nerit 6:3fca0ca1949e 1385 if (cntCellsForReload >= cellsCountSet){
nerit 7:c9fd242538d9 1386 TBmotorDirecti=1;
nerit 0:1e09cd7d66b4 1387 erroreTamburo=0;
nerit 0:1e09cd7d66b4 1388 }
nerit 0:1e09cd7d66b4 1389 }
nerit 7:c9fd242538d9 1390 #if defined(seedSensor)
nerit 7:c9fd242538d9 1391 resetDelay();
nerit 7:c9fd242538d9 1392 delaySeedCheck.start();
nerit 7:c9fd242538d9 1393 #endif
nerit 0:1e09cd7d66b4 1394 }
nerit 0:1e09cd7d66b4 1395 if ((double)TBactualPosition > ((TBgiroStep/cellsNumber)*1.2f)){
nerit 0:1e09cd7d66b4 1396 if (firstStart==0){
nerit 5:3b95bbfe2dc9 1397 if (cntTbError>2){
nerit 5:3b95bbfe2dc9 1398 all_cellSignal=1;
nerit 7:c9fd242538d9 1399 #if defined(seedSensor)
nerit 7:c9fd242538d9 1400 resetDelay();
nerit 7:c9fd242538d9 1401 #endif
nerit 5:3b95bbfe2dc9 1402 }
nerit 0:1e09cd7d66b4 1403 }
nerit 0:1e09cd7d66b4 1404 if (erroreTamburo==0){
nerit 0:1e09cd7d66b4 1405 erroreTamburo=1;
nerit 7:c9fd242538d9 1406 TBmotorDirecti=0;
nerit 0:1e09cd7d66b4 1407 cntCellsForReload=0;
nerit 0:1e09cd7d66b4 1408 cntTbError++;
nerit 7:c9fd242538d9 1409 #if defined(seedSensor)
nerit 7:c9fd242538d9 1410 resetDelay();
nerit 7:c9fd242538d9 1411 #endif
nerit 0:1e09cd7d66b4 1412 }
nerit 0:1e09cd7d66b4 1413 }
nerit 5:3b95bbfe2dc9 1414 if (((double)TBactualPosition > ((TBgiroStep/cellsNumber)*3.0f))||(cntTbError>4)){
nerit 0:1e09cd7d66b4 1415 if (firstStart==0){
nerit 0:1e09cd7d66b4 1416 all_noStepRota=1;
nerit 7:c9fd242538d9 1417 #if defined(seedSensor)
nerit 7:c9fd242538d9 1418 resetDelay();
nerit 7:c9fd242538d9 1419 #endif
nerit 0:1e09cd7d66b4 1420 }
nerit 0:1e09cd7d66b4 1421 cntTbError=0;
nerit 0:1e09cd7d66b4 1422 }
nerit 0:1e09cd7d66b4 1423 // ----------------------------------------
nerit 0:1e09cd7d66b4 1424 // read and manage joystick
nerit 0:1e09cd7d66b4 1425 // WARNING - ENABLE CYCLE IS SOFTWARE ALWAYS ACTIVE
nerit 0:1e09cd7d66b4 1426 if (enableCycle==1){
nerit 0:1e09cd7d66b4 1427 if(runRequestBuf==1){
nerit 0:1e09cd7d66b4 1428 if (OldStartCycle!=runRequestBuf){
nerit 0:1e09cd7d66b4 1429 if((startCycle==0)&&(zeroCycleEnd==1)){
nerit 0:1e09cd7d66b4 1430 startCycle=1;
nerit 0:1e09cd7d66b4 1431 OldStartCycle = runRequestBuf;
nerit 0:1e09cd7d66b4 1432 oldZeroCycle=0;
nerit 0:1e09cd7d66b4 1433 }
nerit 0:1e09cd7d66b4 1434 }
nerit 0:1e09cd7d66b4 1435 }else{
nerit 0:1e09cd7d66b4 1436 startCycle=0;
nerit 0:1e09cd7d66b4 1437 pntMedia=0;
nerit 0:1e09cd7d66b4 1438 }
nerit 0:1e09cd7d66b4 1439 if (azzeraDaCan==1){
nerit 0:1e09cd7d66b4 1440 if (tractorSpeed_MtS_timed==0.0f){
nerit 0:1e09cd7d66b4 1441 zeroRequestBuf=1;
nerit 0:1e09cd7d66b4 1442 oldZeroCycle=0;
nerit 0:1e09cd7d66b4 1443 }
nerit 0:1e09cd7d66b4 1444 azzeraDaCan=0;
nerit 0:1e09cd7d66b4 1445 }
nerit 0:1e09cd7d66b4 1446 if (loadDaCan==1){
nerit 0:1e09cd7d66b4 1447 if (tractorSpeed_MtS_timed==0.0f){
nerit 0:1e09cd7d66b4 1448 ciclaTB();
nerit 0:1e09cd7d66b4 1449 }
nerit 0:1e09cd7d66b4 1450 }
nerit 0:1e09cd7d66b4 1451 if ((zeroRequestBuf==1)){
nerit 0:1e09cd7d66b4 1452 if (oldZeroCycle!=zeroRequestBuf){
nerit 0:1e09cd7d66b4 1453 zeroCycle=1;
nerit 0:1e09cd7d66b4 1454 zeroCycleEnd=0;
nerit 0:1e09cd7d66b4 1455 SDzeroed=0;
nerit 0:1e09cd7d66b4 1456 TBzeroed=0;
nerit 0:1e09cd7d66b4 1457 zeroTrigger=0;
nerit 0:1e09cd7d66b4 1458 oldZeroCycle = zeroRequestBuf;
nerit 0:1e09cd7d66b4 1459 }
nerit 0:1e09cd7d66b4 1460 }
nerit 0:1e09cd7d66b4 1461 }else{
nerit 0:1e09cd7d66b4 1462 startCycle=0;
nerit 0:1e09cd7d66b4 1463 zeroCycle=0;
nerit 0:1e09cd7d66b4 1464 }
nerit 0:1e09cd7d66b4 1465
nerit 0:1e09cd7d66b4 1466 //***************************************************************************************************
nerit 0:1e09cd7d66b4 1467 // pulseRised define the event of speed wheel pulse occurs
nerit 0:1e09cd7d66b4 1468 //
nerit 0:1e09cd7d66b4 1469 double maxInterval = pulseDistance/minWorkSpeed;
nerit 0:1e09cd7d66b4 1470 double minIntervalPulse = pulseDistance/maxWorkSpeed;
nerit 0:1e09cd7d66b4 1471 if (pulseRised==1){
nerit 0:1e09cd7d66b4 1472 if (enableSpeed<10){enableSpeed++;}
nerit 5:3b95bbfe2dc9 1473 pulseRised=0;
nerit 5:3b95bbfe2dc9 1474 pulseRised1=1;
nerit 5:3b95bbfe2dc9 1475 speedMediaCalc();
nerit 0:1e09cd7d66b4 1476 // calcola velocità trattore
nerit 0:1e09cd7d66b4 1477 if(enableSpeed>=2){
nerit 0:1e09cd7d66b4 1478 if ((pulseSpeedInterval>=0.0f)){ //minIntervalPulse)&&(pulseSpeedInterval<maxInterval)){
nerit 8:0e643ea7834f 1479 if((quincCnt<3)||(speedFromMaster==0.0f)||(enableSimula==1)){
nerit 7:c9fd242538d9 1480 tractorSpeed_MtS_timed = ((pulseDistance / pulseSpeedInterval)); // tractor speed (unit= Mt/S) from pulse time interval
nerit 7:c9fd242538d9 1481 }
nerit 7:c9fd242538d9 1482 /*#if !defined(speedMaster)
nerit 5:3b95bbfe2dc9 1483 if (quincCnt>=5){
nerit 5:3b95bbfe2dc9 1484 if (speedFromMaster>0.0f){
nerit 5:3b95bbfe2dc9 1485 tractorSpeed_MtS_timed = speedFromMaster + percento;
nerit 5:3b95bbfe2dc9 1486 }
nerit 5:3b95bbfe2dc9 1487 }
nerit 7:c9fd242538d9 1488 #endif*/
nerit 0:1e09cd7d66b4 1489 if (checkSDrotation==0){
nerit 0:1e09cd7d66b4 1490 checkSDrotation=1;
nerit 0:1e09cd7d66b4 1491 SDwheelTimer.start();
nerit 0:1e09cd7d66b4 1492 }
nerit 0:1e09cd7d66b4 1493 }
nerit 0:1e09cd7d66b4 1494 }
nerit 0:1e09cd7d66b4 1495 speedTimeOut.reset();
nerit 0:1e09cd7d66b4 1496 }else{
nerit 0:1e09cd7d66b4 1497 double oldLastPr = (double)oldLastPulseRead*1.7f;
nerit 0:1e09cd7d66b4 1498 if((double)speedTimeOut.read_us()> (oldLastPr)){
nerit 0:1e09cd7d66b4 1499 tractorSpeed_MtS_timed = 0.0f;
nerit 7:c9fd242538d9 1500 #if defined(seedSensor)
nerit 7:c9fd242538d9 1501 resetDelay();
nerit 7:c9fd242538d9 1502 #endif
nerit 0:1e09cd7d66b4 1503 pntMedia=0;
nerit 0:1e09cd7d66b4 1504 speedTimeOut.reset();
nerit 0:1e09cd7d66b4 1505 enableSpeed=0;
nerit 0:1e09cd7d66b4 1506 quincCnt=0;
nerit 0:1e09cd7d66b4 1507 }
nerit 0:1e09cd7d66b4 1508 }
nerit 7:c9fd242538d9 1509
nerit 7:c9fd242538d9 1510 #if defined(seedSensor)
nerit 7:c9fd242538d9 1511 if (seedSensorEnable==true){
nerit 7:c9fd242538d9 1512 if (delaySeedCheck.read_ms()>100){
nerit 7:c9fd242538d9 1513 if (seedSee==0){
nerit 7:c9fd242538d9 1514 all_noSeedOnCe=1;
nerit 7:c9fd242538d9 1515 }
nerit 7:c9fd242538d9 1516 resetDelay();
nerit 7:c9fd242538d9 1517 }
nerit 7:c9fd242538d9 1518 }
nerit 7:c9fd242538d9 1519 #endif
nerit 0:1e09cd7d66b4 1520 // esegue il controllo di velocità minima
nerit 7:c9fd242538d9 1521 /*if ((double)speedTimer.read_ms()>=maxInterval){
nerit 4:d32258ec411f 1522 tractorSpeed_MtS_timed = 0.0f;
nerit 0:1e09cd7d66b4 1523 enableSpeed=0;
nerit 7:c9fd242538d9 1524 }*/
nerit 6:3fca0ca1949e 1525 // esegue il controllo di velocità massima
nerit 7:c9fd242538d9 1526 /*if ((double)speedTimer.read_ms()<=minIntervalPulse){
nerit 6:3fca0ca1949e 1527 tractorSpeed_MtS_timed = 4.5f;
nerit 7:c9fd242538d9 1528 }*/
nerit 0:1e09cd7d66b4 1529 //***************************************************************************************************************
nerit 0:1e09cd7d66b4 1530 // cycle logic control section
nerit 0:1e09cd7d66b4 1531 //***************************************************************************************************************
nerit 0:1e09cd7d66b4 1532 if (enableSimula==1){if(simOk==0){tractorSpeed_MtS_timed=0.0f;}}
nerit 0:1e09cd7d66b4 1533 if ((tractorSpeed_MtS_timed>0.01f)){
nerit 0:1e09cd7d66b4 1534 cycleStopRequest=1;
nerit 0:1e09cd7d66b4 1535 // calcola il tempo teorico di passaggio becchi sulla base della velocità del trattore
nerit 0:1e09cd7d66b4 1536 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)
nerit 5:3b95bbfe2dc9 1537 if (encoder==false){
nerit 5:3b95bbfe2dc9 1538 if (speedFromPick==1) {
nerit 5:3b95bbfe2dc9 1539 tempoTraBecchi_mS = (tempoGiroSD / pickNumber)*1000.0f; // tempo tra due impulsi dai becchi in millisecondi ( circa 157mS a 4,5Km/h)
nerit 5:3b95bbfe2dc9 1540 }else{
nerit 5:3b95bbfe2dc9 1541 tempoTraBecchi_mS = (tempoGiroSD / 25.0f)*1000.0f; // tempo tra due impulsi dai becchi in millisecondi ( circa 157mS a 4,5Km/h)
nerit 5:3b95bbfe2dc9 1542 }
nerit 0:1e09cd7d66b4 1543 }else{
nerit 5:3b95bbfe2dc9 1544 tempoTraBecchi_mS = (tempoGiroSD / (SDreductionRatio*25.5f))*1000.0f; // tempo tra due impulsi dai becchi in millisecondi ( circa 157mS a 4,5Km/h)
nerit 5:3b95bbfe2dc9 1545 double pippo=0.0f;
nerit 5:3b95bbfe2dc9 1546 #if !defined(speedMaster)
nerit 5:3b95bbfe2dc9 1547 pippo = seedPerimeter / speedFromMaster;
nerit 5:3b95bbfe2dc9 1548 #endif
nerit 5:3b95bbfe2dc9 1549 tempoBecchiPerQuinc = (pippo / pickNumber)*1000.0f;
nerit 0:1e09cd7d66b4 1550 }
nerit 0:1e09cd7d66b4 1551 //*******************************************
nerit 0:1e09cd7d66b4 1552 // segue calcolo duty cycle comando motore DC per allinearsi con la velocità del trattore
nerit 0:1e09cd7d66b4 1553 double dutyTeorico = 0.00;
nerit 0:1e09cd7d66b4 1554 if ((tractorSpeed_MtS_timed>0.0)&&(tractorSpeed_MtS_timed<tabSpeed[0])){dutyTeorico = tabComan[0];}
nerit 0:1e09cd7d66b4 1555 for (int ii = 0;ii<16;ii++){
nerit 0:1e09cd7d66b4 1556 if ((tractorSpeed_MtS_timed>=tabSpeed[ii])&&(tractorSpeed_MtS_timed<tabSpeed[ii+1])){
nerit 0:1e09cd7d66b4 1557 dutyTeorico = tabComan[ii+1];
nerit 0:1e09cd7d66b4 1558 }
nerit 0:1e09cd7d66b4 1559 }
nerit 6:3fca0ca1949e 1560 if (tractorSpeed_MtS_timed > tabSpeed[16]){dutyTeorico=tractorSpeed_MtS_timed/maxWorkSpeed;}
nerit 2:d9c7430ae953 1561 #if !defined(speedMaster)
nerit 4:d32258ec411f 1562 quinCalc();
nerit 2:d9c7430ae953 1563 #endif
nerit 5:3b95bbfe2dc9 1564 if ((enableSpeed>3)&&(pulseRised2==1)&&(quincCnt>=2)){
nerit 0:1e09cd7d66b4 1565 double erroreTempo = 0.0f;
nerit 5:3b95bbfe2dc9 1566 if(encoder==false){
nerit 5:3b95bbfe2dc9 1567 if(speedFromPick==1){
nerit 3:c0f11ca4df02 1568 erroreTempo = (double)timeIntraPick - tempoTraBecchi_mS;
nerit 5:3b95bbfe2dc9 1569 }else{
nerit 5:3b95bbfe2dc9 1570 erroreTempo = (double)memoTimeHole - tempoTraBecchi_mS; // errore tra il tempo previsto ed il tempo reale ( >0 se sto andando più piano del previsto)
nerit 5:3b95bbfe2dc9 1571 }
nerit 0:1e09cd7d66b4 1572 }else{
nerit 5:3b95bbfe2dc9 1573 erroreTempo = ((double)memoTimeHole/1000.0f) - tempoTraBecchi_mS; // errore tra il tempo previsto ed il tempo reale ( >0 se sto andando più piano del previsto)
nerit 0:1e09cd7d66b4 1574 }
nerit 0:1e09cd7d66b4 1575 double errorePercentuale = erroreTempo / tempoTraBecchi_mS;
nerit 1:e88bf5011af6 1576 double k3=0.0f;
nerit 1:e88bf5011af6 1577 double k4=0.0f;
nerit 1:e88bf5011af6 1578 double k5=0.0f;
nerit 4:d32258ec411f 1579 double k6=0.0f;
nerit 4:d32258ec411f 1580 /*if (tractorSpeed_MtS_timed <= minSeedSpeed){
nerit 4:d32258ec411f 1581 k3=1.030f;
nerit 4:d32258ec411f 1582 k4=5.103f;
nerit 4:d32258ec411f 1583 k5=10.00f;
nerit 4:d32258ec411f 1584 k6=20.50f;
nerit 4:d32258ec411f 1585 }else{*/
nerit 8:0e643ea7834f 1586 #if defined(speedMaster)
nerit 8:0e643ea7834f 1587 k3=0.010f;
nerit 8:0e643ea7834f 1588 #else
nerit 8:0e643ea7834f 1589 k3=0.050f;
nerit 8:0e643ea7834f 1590 #endif
nerit 7:c9fd242538d9 1591 k4=1.103f;
nerit 7:c9fd242538d9 1592 k5=10.00f;
nerit 7:c9fd242538d9 1593 k6=20.50f;
nerit 4:d32258ec411f 1594 //}
nerit 7:c9fd242538d9 1595 double L1 = 0.045f;
nerit 7:c9fd242538d9 1596 double L_1=-0.045f;
nerit 7:c9fd242538d9 1597 double L2 = 0.150f;
nerit 7:c9fd242538d9 1598 double L_2=-0.150f;
nerit 7:c9fd242538d9 1599 double L3 = 0.301f;
nerit 7:c9fd242538d9 1600 double L_3=-0.301f;
nerit 4:d32258ec411f 1601 double k1=0.0f;
nerit 4:d32258ec411f 1602 if ((errorePercentuale > L3)||(errorePercentuale < L_3)){
nerit 4:d32258ec411f 1603 k1=errorePercentuale*k6;
nerit 1:e88bf5011af6 1604 }
nerit 4:d32258ec411f 1605 if (((errorePercentuale >= L2)&&(errorePercentuale<=L3))||((errorePercentuale <= L_2)&&(errorePercentuale>=L_3))){
nerit 1:e88bf5011af6 1606 k1=errorePercentuale*k5;
nerit 1:e88bf5011af6 1607 }
nerit 4:d32258ec411f 1608 if (((errorePercentuale < L2)&&(errorePercentuale>L1))||((errorePercentuale > L_2)&&(errorePercentuale<L_1))){
nerit 1:e88bf5011af6 1609 k1=errorePercentuale*k4;
nerit 1:e88bf5011af6 1610 }
nerit 4:d32258ec411f 1611 if ((errorePercentuale < L1)||(errorePercentuale > L_1)){
nerit 1:e88bf5011af6 1612 k1=errorePercentuale*k3;
nerit 1:e88bf5011af6 1613 }
nerit 4:d32258ec411f 1614 double memoCorrezione = k1;
nerit 4:d32258ec411f 1615 if (quincCnt >= 2){
nerit 4:d32258ec411f 1616 correzione = correzione + memoCorrezione;
nerit 4:d32258ec411f 1617 if (correzione > (1.0f - dutyTeorico)){correzione = (1.0f - dutyTeorico);}
nerit 4:d32258ec411f 1618 if ((correzione < 0.0f)&&(dutyTeorico+correzione<0.0f)){correzione = -1.0f*dutyTeorico;}
nerit 4:d32258ec411f 1619 }
nerit 4:d32258ec411f 1620 pulseRised1=0;
nerit 4:d32258ec411f 1621 pulseRised2=0;
nerit 1:e88bf5011af6 1622 #if defined(pcSerial)
nerit 4:d32258ec411f 1623 #if defined(Qnca)
nerit 5:3b95bbfe2dc9 1624 pc.printf("ErTem: %f K1: %f Corr: %f MemoCorr:%f DutyTeo: %f \n",erroreTempo, k1,correzione, memoCorrezione, dutyTeorico);
nerit 4:d32258ec411f 1625 pc.printf("TsP: %f SpW: %f InPic: %f TBec: %f EPerc: %f Duty:%f \n",tractorSpeed_MtS_timed,speedOfSeedWheel,timeIntraPick, tempoTraBecchi_mS,errorePercentuale, dcActualDuty);
nerit 1:e88bf5011af6 1626 #endif
nerit 1:e88bf5011af6 1627 #endif
nerit 4:d32258ec411f 1628 #if defined(pcSerial)
nerit 4:d32258ec411f 1629 #if defined(Qncb)
nerit 4:d32258ec411f 1630 pc.printf("TsP: %f SpW: %f InPic: %f EPerc: %f Duty:%f \n",tractorSpeed_MtS_timed,speedOfSeedWheel,timeIntraPick, errorePercentuale, dcActualDuty);
nerit 4:d32258ec411f 1631 #endif
nerit 4:d32258ec411f 1632 #endif
nerit 0:1e09cd7d66b4 1633 }
nerit 0:1e09cd7d66b4 1634 // introduce il controllo di corrente
nerit 6:3fca0ca1949e 1635 if (currentCheckEnable==true){
nerit 6:3fca0ca1949e 1636 if (incrementCurrent){
nerit 6:3fca0ca1949e 1637 boostDcOut +=0.005f;
nerit 6:3fca0ca1949e 1638 }
nerit 6:3fca0ca1949e 1639 if (reduceCurrent){
nerit 6:3fca0ca1949e 1640 boostDcOut -=0.005f;
nerit 6:3fca0ca1949e 1641 }
nerit 6:3fca0ca1949e 1642 if (boostDcOut >= 0.2f){
nerit 6:3fca0ca1949e 1643 boostDcOut=0.2f;
nerit 6:3fca0ca1949e 1644 all_genericals=1;
nerit 6:3fca0ca1949e 1645 }
nerit 6:3fca0ca1949e 1646 if (boostDcOut <=-0.2f){
nerit 6:3fca0ca1949e 1647 boostDcOut=-0.2f;
nerit 6:3fca0ca1949e 1648 all_genericals=1;
nerit 6:3fca0ca1949e 1649 }
nerit 6:3fca0ca1949e 1650 correzione += boostDcOut;
nerit 0:1e09cd7d66b4 1651 }
nerit 1:e88bf5011af6 1652 DC_brake=0;
nerit 1:e88bf5011af6 1653 DC_forward=1;
nerit 1:e88bf5011af6 1654 DC_prepare();
nerit 0:1e09cd7d66b4 1655
nerit 0:1e09cd7d66b4 1656 // il semiperiodoreale è calcolato sulla lettura del passaggio becchi reale
nerit 5:3b95bbfe2dc9 1657 seedWheelPeriod = semiPeriodoReale;
nerit 0:1e09cd7d66b4 1658 if (seedWheelPeriod < 180.0f){seedWheelPeriod = 180.0f;}
nerit 0:1e09cd7d66b4 1659 if ((oldSeedWheelPeriod!=seedWheelPeriod)&&(seedWheelPeriod >=180.0f )){
nerit 0:1e09cd7d66b4 1660 SDticker.attach_us(&step_SDPulseOut,seedWheelPeriod); // clock time are milliseconds and attach seed motor stepper controls
nerit 0:1e09cd7d66b4 1661 oldSeedWheelPeriod=seedWheelPeriod;
nerit 0:1e09cd7d66b4 1662 }
nerit 0:1e09cd7d66b4 1663
nerit 4:d32258ec411f 1664 if((quincCnt>=3)){
nerit 0:1e09cd7d66b4 1665 if (correzioneAttiva==1){
nerit 0:1e09cd7d66b4 1666 dcActualDuty = dutyTeorico + correzione;
nerit 4:d32258ec411f 1667 }else{
nerit 4:d32258ec411f 1668 dcActualDuty = dutyTeorico;
nerit 0:1e09cd7d66b4 1669 }
nerit 0:1e09cd7d66b4 1670 }else{
nerit 0:1e09cd7d66b4 1671 dcActualDuty = dutyTeorico;
nerit 0:1e09cd7d66b4 1672 }
nerit 7:c9fd242538d9 1673 if (dcActualDuty <=0.0f){dcActualDuty=0.05f;}
nerit 7:c9fd242538d9 1674 if (dcActualDuty > 0.95f){dcActualDuty = 0.95f;}//dcMaxSpeed;}
nerit 4:d32258ec411f 1675 if (olddcActualDuty!=dcActualDuty){
nerit 7:c9fd242538d9 1676 SDmotorPWM.write(1.0f-dcActualDuty);
nerit 4:d32258ec411f 1677 olddcActualDuty=dcActualDuty;
nerit 4:d32258ec411f 1678 }
nerit 0:1e09cd7d66b4 1679 // allarme
nerit 0:1e09cd7d66b4 1680 if (SDwheelTimer.read_ms()>4000){
nerit 0:1e09cd7d66b4 1681 if (firstStart==0){
nerit 0:1e09cd7d66b4 1682 all_noDcRotati=1;
nerit 0:1e09cd7d66b4 1683 }
nerit 0:1e09cd7d66b4 1684 #if defined(pcSerial)
nerit 0:1e09cd7d66b4 1685 #if defined(VediAllarmi)
nerit 0:1e09cd7d66b4 1686 pc.printf("allarme no DC rotation");
nerit 0:1e09cd7d66b4 1687 #endif
nerit 0:1e09cd7d66b4 1688 #endif
nerit 0:1e09cd7d66b4 1689
nerit 0:1e09cd7d66b4 1690 }
nerit 0:1e09cd7d66b4 1691
nerit 0:1e09cd7d66b4 1692 //***************************************************************************************************************
nerit 0:1e09cd7d66b4 1693 // CONTROLLA TAMBURO
nerit 0:1e09cd7d66b4 1694 //***************************************************************************************************************
nerit 0:1e09cd7d66b4 1695 if(lowSpeed==0){
nerit 0:1e09cd7d66b4 1696 if (syncroCheck==1){
nerit 0:1e09cd7d66b4 1697 syncroCheck=0;
nerit 0:1e09cd7d66b4 1698 lockStart=1;
nerit 0:1e09cd7d66b4 1699 periodo = TBperiod;
nerit 0:1e09cd7d66b4 1700 if (aspettaStart==0){cambiaTB(periodo);}
nerit 0:1e09cd7d66b4 1701 }
nerit 0:1e09cd7d66b4 1702 // controllo di stop
nerit 0:1e09cd7d66b4 1703 double memoIntraP = (double)memoIntraPick*1.8f;
nerit 0:1e09cd7d66b4 1704 if ((double)rotationTimeOut.read_ms()> (memoIntraP)){
nerit 0:1e09cd7d66b4 1705 syncroCheck=0;
nerit 0:1e09cd7d66b4 1706 aspettaStart=1;
nerit 0:1e09cd7d66b4 1707 countCicli=0;
nerit 0:1e09cd7d66b4 1708 if (TBzeroCyclePulse==1){TBticker.detach();}
nerit 0:1e09cd7d66b4 1709 }
nerit 0:1e09cd7d66b4 1710 }else{ // fine ciclo fuori da low speed
nerit 0:1e09cd7d66b4 1711 syncroCheck=0;
nerit 0:1e09cd7d66b4 1712 lockStart=0;
nerit 0:1e09cd7d66b4 1713 if (beccoPronto==1){
nerit 0:1e09cd7d66b4 1714 if (tamburoStandard==1){
nerit 0:1e09cd7d66b4 1715 double ritardoMassimo = 0.0f;
nerit 5:3b95bbfe2dc9 1716 if (encoder==false){
nerit 5:3b95bbfe2dc9 1717 if(speedFromPick==1){
nerit 5:3b95bbfe2dc9 1718 ritardoMassimo = (double)timeIntraPick;
nerit 5:3b95bbfe2dc9 1719 }else{
nerit 5:3b95bbfe2dc9 1720 ritardoMassimo = (double)memoTimeHole;
nerit 5:3b95bbfe2dc9 1721 }
nerit 5:3b95bbfe2dc9 1722 }else{
nerit 0:1e09cd7d66b4 1723 ritardoMassimo = (double)timeIntraPick;
nerit 0:1e09cd7d66b4 1724 }
nerit 6:3fca0ca1949e 1725 int tempoDiSincro = (int)((double)(ritardoMassimo - ((tempoBecco/2.0f)+((speedOfSeedWheel/maxWorkSpeed)*ritardoMassimo)))); //
nerit 0:1e09cd7d66b4 1726 if (tempoDiSincro <= 1){tempoDiSincro=1;}
nerit 0:1e09cd7d66b4 1727 if ((sincroTimer.read_ms()>= tempoDiSincro)){
nerit 0:1e09cd7d66b4 1728 if (tractorSpeed_MtS_timed >= minWorkSpeed){startCicloTB=1;}
nerit 0:1e09cd7d66b4 1729 beccoPronto=0;
nerit 0:1e09cd7d66b4 1730 }
nerit 0:1e09cd7d66b4 1731 }else{
nerit 0:1e09cd7d66b4 1732 // tamburo per zucca
nerit 0:1e09cd7d66b4 1733 if (speedOfSeedWheel >= minWorkSpeed){startCicloTB=1;}
nerit 0:1e09cd7d66b4 1734 beccoPronto=0;
nerit 0:1e09cd7d66b4 1735 }
nerit 0:1e09cd7d66b4 1736 }
nerit 0:1e09cd7d66b4 1737 ciclaTB();
nerit 0:1e09cd7d66b4 1738 }
nerit 0:1e09cd7d66b4 1739 //*************************************************************
nerit 0:1e09cd7d66b4 1740 }else{ // fine ciclo con velocita maggiore di 0
nerit 0:1e09cd7d66b4 1741 SDwheelTimer.stop();
nerit 0:1e09cd7d66b4 1742 SDwheelTimer.reset();
nerit 7:c9fd242538d9 1743 #if defined(seedSensor)
nerit 7:c9fd242538d9 1744 resetDelay();
nerit 7:c9fd242538d9 1745 #endif
nerit 0:1e09cd7d66b4 1746 checkSDrotation=0;
nerit 0:1e09cd7d66b4 1747 oldFaseLavoro=0;
nerit 0:1e09cd7d66b4 1748 aspettaStart=1;
nerit 0:1e09cd7d66b4 1749 countCicli=0;
nerit 0:1e09cd7d66b4 1750 startCycle=0;
nerit 0:1e09cd7d66b4 1751 oldSeedWheelPeriod=0.0f;
nerit 0:1e09cd7d66b4 1752 oldPeriodoTB=0.0f;
nerit 0:1e09cd7d66b4 1753 correzione=0.0f;
nerit 0:1e09cd7d66b4 1754 OLDpulseSpeedInterval=1000.01f;
nerit 0:1e09cd7d66b4 1755 cicloTbinCorso=0;
nerit 0:1e09cd7d66b4 1756 cntTbError=0;
nerit 0:1e09cd7d66b4 1757 if (cycleStopRequest==1){
nerit 0:1e09cd7d66b4 1758 zeroDelay.reset();
nerit 0:1e09cd7d66b4 1759 zeroDelay.start();
nerit 0:1e09cd7d66b4 1760 runRequestBuf=0;
nerit 0:1e09cd7d66b4 1761 zeroRequestBuf=1;
nerit 0:1e09cd7d66b4 1762 cycleStopRequest=0;
nerit 0:1e09cd7d66b4 1763 SDzeroCyclePulse=0;
nerit 0:1e09cd7d66b4 1764 TBzeroCyclePulse=0;
nerit 0:1e09cd7d66b4 1765 zeroCycleEnd=0;
nerit 0:1e09cd7d66b4 1766 zeroCycle=1;
nerit 0:1e09cd7d66b4 1767 zeroTrigger=0;
nerit 8:0e643ea7834f 1768 noSDzeroRequest=1;
nerit 0:1e09cd7d66b4 1769 }
nerit 0:1e09cd7d66b4 1770 }
nerit 0:1e09cd7d66b4 1771 //*************************************************************************************************
nerit 0:1e09cd7d66b4 1772 // ciclo di azzeramento motori
nerit 0:1e09cd7d66b4 1773 if ((zeroCycleEnd==0)&&(zeroCycle==1)){//&&(zeroDelay.read_ms()>10000)){
nerit 0:1e09cd7d66b4 1774 if (zeroTrigger==0){
nerit 0:1e09cd7d66b4 1775 TBticker.attach_us(&step_TBPulseOut,1000.0f); // clock time are seconds and attach seed motor stepper controls
nerit 0:1e09cd7d66b4 1776 DC_forward=1;
nerit 0:1e09cd7d66b4 1777 DC_brake=1;
nerit 0:1e09cd7d66b4 1778 DC_prepare();
nerit 0:1e09cd7d66b4 1779 frenata=0;
nerit 0:1e09cd7d66b4 1780 zeroTrigger=1;
nerit 0:1e09cd7d66b4 1781 ritardoStop.reset();
nerit 0:1e09cd7d66b4 1782 ritardoComandoStop.reset();
nerit 0:1e09cd7d66b4 1783 ritardoComandoStop.start();
nerit 0:1e09cd7d66b4 1784 timeOutZeroSD.start();
nerit 0:1e09cd7d66b4 1785 quincTimeSet.reset();
nerit 0:1e09cd7d66b4 1786 }
nerit 0:1e09cd7d66b4 1787 int tempoFrenata=300;
nerit 0:1e09cd7d66b4 1788 if (((ritardoStop.read_ms()>tempoFrenata)&&(SDzeroDebounced==0))||(accensione==1)||(ritardoComandoStop.read_ms()>400)){
nerit 0:1e09cd7d66b4 1789 accensione=0;
nerit 0:1e09cd7d66b4 1790 avvicinamento=1;
nerit 0:1e09cd7d66b4 1791 avvicinamentoOn.reset();
nerit 0:1e09cd7d66b4 1792 avvicinamentoOn.start();
nerit 7:c9fd242538d9 1793 SDmotorPWM.write(1.0f-0.32f); // duty cycle = 60% of power
nerit 0:1e09cd7d66b4 1794 DC_forward=1;
nerit 0:1e09cd7d66b4 1795 DC_brake=0;
nerit 0:1e09cd7d66b4 1796 DC_prepare();
nerit 0:1e09cd7d66b4 1797 ritardoComandoStop.reset();
nerit 0:1e09cd7d66b4 1798 ritardoComandoStop.stop();
nerit 0:1e09cd7d66b4 1799 }
nerit 0:1e09cd7d66b4 1800 if (avvicinamento==1){
nerit 0:1e09cd7d66b4 1801 if(avvicinamentoOn.read_ms()>300){
nerit 7:c9fd242538d9 1802 SDmotorPWM.write(1.0f-0.7f);
nerit 0:1e09cd7d66b4 1803 avvicinamentoOn.reset();
nerit 0:1e09cd7d66b4 1804 avvicinamentoOff.reset();
nerit 0:1e09cd7d66b4 1805 avvicinamentoOff.start();
nerit 0:1e09cd7d66b4 1806 }
nerit 0:1e09cd7d66b4 1807 if(avvicinamentoOff.read_ms()>100){
nerit 7:c9fd242538d9 1808 SDmotorPWM.write(1.0f-0.32f);
nerit 0:1e09cd7d66b4 1809 avvicinamentoOff.reset();
nerit 0:1e09cd7d66b4 1810 avvicinamentoOff.stop();
nerit 0:1e09cd7d66b4 1811 avvicinamentoOn.start();
nerit 0:1e09cd7d66b4 1812 }
nerit 0:1e09cd7d66b4 1813 }else{
nerit 0:1e09cd7d66b4 1814 avvicinamentoOn.stop();
nerit 0:1e09cd7d66b4 1815 avvicinamentoOff.stop();
nerit 0:1e09cd7d66b4 1816 avvicinamentoOn.reset();
nerit 0:1e09cd7d66b4 1817 avvicinamentoOff.reset();
nerit 0:1e09cd7d66b4 1818 }
nerit 0:1e09cd7d66b4 1819 if (frenata==0){
nerit 0:1e09cd7d66b4 1820 if (SDzeroCyclePulse==1){
nerit 0:1e09cd7d66b4 1821 SDticker.detach();
nerit 0:1e09cd7d66b4 1822 frenata=1;
nerit 0:1e09cd7d66b4 1823 quincTimeSet.reset();
nerit 0:1e09cd7d66b4 1824 quincTimeSet.start();
nerit 0:1e09cd7d66b4 1825 ritardoStop.start();
nerit 0:1e09cd7d66b4 1826 //ritardoComandoStop.reset();
nerit 0:1e09cd7d66b4 1827 //ritardoComandoStop.stop();
nerit 0:1e09cd7d66b4 1828 }
nerit 0:1e09cd7d66b4 1829 }else{
nerit 7:c9fd242538d9 1830 #if defined(mezzo)
nerit 7:c9fd242538d9 1831 if (quinconceActive==0){
nerit 7:c9fd242538d9 1832 if (SDzeroCyclePulse==1){
nerit 7:c9fd242538d9 1833 avvicinamento=0;
nerit 7:c9fd242538d9 1834 DC_brake=1;
nerit 7:c9fd242538d9 1835 DC_prepare();
nerit 7:c9fd242538d9 1836 SDzeroed=1;
nerit 7:c9fd242538d9 1837 ritardoStop.reset();
nerit 7:c9fd242538d9 1838 ritardoStop.stop();
nerit 7:c9fd242538d9 1839 }
nerit 7:c9fd242538d9 1840 }else{
nerit 7:c9fd242538d9 1841 if (quincTimeSet.read_ms()>700){
nerit 7:c9fd242538d9 1842 avvicinamento=0;
nerit 7:c9fd242538d9 1843 DC_brake=1;
nerit 7:c9fd242538d9 1844 DC_prepare();
nerit 7:c9fd242538d9 1845 SDzeroed=1;
nerit 7:c9fd242538d9 1846 ritardoStop.reset();
nerit 7:c9fd242538d9 1847 ritardoStop.stop();
nerit 7:c9fd242538d9 1848 quincTimeSet.stop();
nerit 7:c9fd242538d9 1849 }
nerit 7:c9fd242538d9 1850 }
nerit 7:c9fd242538d9 1851 #else
nerit 0:1e09cd7d66b4 1852 if (SDzeroCyclePulse==1){
nerit 0:1e09cd7d66b4 1853 avvicinamento=0;
nerit 0:1e09cd7d66b4 1854 DC_brake=1;
nerit 0:1e09cd7d66b4 1855 DC_prepare();
nerit 0:1e09cd7d66b4 1856 SDzeroed=1;
nerit 0:1e09cd7d66b4 1857 ritardoStop.reset();
nerit 0:1e09cd7d66b4 1858 ritardoStop.stop();
nerit 0:1e09cd7d66b4 1859 }
nerit 7:c9fd242538d9 1860 #endif
nerit 0:1e09cd7d66b4 1861 }
nerit 0:1e09cd7d66b4 1862 // azzera tutto in time out
nerit 8:0e643ea7834f 1863 if ((timeOutZeroSD.read_ms()>=10000)||(noSDzeroRequest==1)){
nerit 8:0e643ea7834f 1864 if ((firstStart==0)&&(noSDzeroRequest==0)){
nerit 0:1e09cd7d66b4 1865 all_no_Zeroing=1;
nerit 0:1e09cd7d66b4 1866 }
nerit 0:1e09cd7d66b4 1867 avvicinamento=0;
nerit 0:1e09cd7d66b4 1868 DC_brake=1;
nerit 0:1e09cd7d66b4 1869 DC_prepare();
nerit 0:1e09cd7d66b4 1870 SDzeroed=1;
nerit 0:1e09cd7d66b4 1871 ritardoStop.reset();
nerit 0:1e09cd7d66b4 1872 ritardoStop.stop();
nerit 0:1e09cd7d66b4 1873 avvicinamentoOn.stop();
nerit 0:1e09cd7d66b4 1874 avvicinamentoOff.stop();
nerit 0:1e09cd7d66b4 1875 avvicinamentoOn.reset();
nerit 0:1e09cd7d66b4 1876 avvicinamentoOff.reset();
nerit 0:1e09cd7d66b4 1877 ritardoComandoStop.reset();
nerit 0:1e09cd7d66b4 1878 ritardoComandoStop.stop();
nerit 0:1e09cd7d66b4 1879 timeOutZeroSD.stop();
nerit 0:1e09cd7d66b4 1880 timeOutZeroSD.reset();
nerit 8:0e643ea7834f 1881 noSDzeroRequest=0;
nerit 0:1e09cd7d66b4 1882 }
nerit 0:1e09cd7d66b4 1883 if (TBzeroCyclePulse==1){
nerit 0:1e09cd7d66b4 1884 TBticker.detach();
nerit 0:1e09cd7d66b4 1885 TBzeroed=1;
nerit 0:1e09cd7d66b4 1886 }
nerit 0:1e09cd7d66b4 1887 if ((SDzeroed==1)&&(TBzeroed==1)){
nerit 0:1e09cd7d66b4 1888 avvicinamentoOn.stop();
nerit 0:1e09cd7d66b4 1889 avvicinamentoOff.stop();
nerit 0:1e09cd7d66b4 1890 ritardoComandoStop.stop();
nerit 0:1e09cd7d66b4 1891 ritardoStop.stop();
nerit 0:1e09cd7d66b4 1892 zeroCycleEnd=1;
nerit 0:1e09cd7d66b4 1893 zeroCycle=0;
nerit 0:1e09cd7d66b4 1894 zeroTrigger=0;
nerit 0:1e09cd7d66b4 1895 runRequestBuf=1;
nerit 0:1e09cd7d66b4 1896 zeroRequestBuf=0;
nerit 0:1e09cd7d66b4 1897 cycleStopRequest=0;
nerit 0:1e09cd7d66b4 1898 SDzeroed=0;
nerit 0:1e09cd7d66b4 1899 TBzeroed=0;
nerit 0:1e09cd7d66b4 1900 timeOutZeroSD.stop();
nerit 0:1e09cd7d66b4 1901 timeOutZeroSD.reset();
nerit 0:1e09cd7d66b4 1902 }
nerit 0:1e09cd7d66b4 1903 }
nerit 0:1e09cd7d66b4 1904
nerit 0:1e09cd7d66b4 1905 //*************************************************************************************************
nerit 0:1e09cd7d66b4 1906 if (enableCycle==0){
nerit 0:1e09cd7d66b4 1907 zeroTrigger=0;
nerit 0:1e09cd7d66b4 1908 SDmotorPWM=0;
nerit 0:1e09cd7d66b4 1909 SDmotorInA=0;
nerit 0:1e09cd7d66b4 1910 SDmotorInB=0;
nerit 0:1e09cd7d66b4 1911 }
nerit 0:1e09cd7d66b4 1912 SDzeroCyclePulse=0;
nerit 0:1e09cd7d66b4 1913 TBzeroCyclePulse=0;
nerit 0:1e09cd7d66b4 1914 //*************************************************************************************************
nerit 0:1e09cd7d66b4 1915 }else{//end ciclo normale
nerit 0:1e09cd7d66b4 1916 //*************************************************************************************************
nerit 0:1e09cd7d66b4 1917 // task di prova della scheda
nerit 0:1e09cd7d66b4 1918 //*************************************************************************************************
nerit 0:1e09cd7d66b4 1919 #if defined(provaScheda)
nerit 0:1e09cd7d66b4 1920 clocca++;
nerit 0:1e09cd7d66b4 1921 //led = !led;
nerit 0:1e09cd7d66b4 1922 //txMsg.clear();
nerit 0:1e09cd7d66b4 1923 //txMsg << clocca;
nerit 0:1e09cd7d66b4 1924 //test.printf("aogs \n");
nerit 0:1e09cd7d66b4 1925 //if(can1.write(txMsg)){
nerit 0:1e09cd7d66b4 1926 #if defined(pcSerial)
nerit 0:1e09cd7d66b4 1927 pc.printf("Can write OK \n");
nerit 0:1e09cd7d66b4 1928 #endif
nerit 0:1e09cd7d66b4 1929 //}
nerit 0:1e09cd7d66b4 1930 switch (clocca){
nerit 0:1e09cd7d66b4 1931 case 1:
nerit 0:1e09cd7d66b4 1932 TBmotorStepOut=1; // define step command for up down motor driver
nerit 0:1e09cd7d66b4 1933 break;
nerit 0:1e09cd7d66b4 1934 case 2:
nerit 0:1e09cd7d66b4 1935 SDmotorPWM=1; // define step command for seeding whell motor driver
nerit 0:1e09cd7d66b4 1936 break;
nerit 0:1e09cd7d66b4 1937 case 3:
nerit 0:1e09cd7d66b4 1938 speedClock=1; // define input of
nerit 0:1e09cd7d66b4 1939 break;
nerit 0:1e09cd7d66b4 1940 case 4:
nerit 0:1e09cd7d66b4 1941 break;
nerit 0:1e09cd7d66b4 1942 case 5:
nerit 0:1e09cd7d66b4 1943 SDmotorInA=1;
nerit 0:1e09cd7d66b4 1944 SDmotorInB=0;
nerit 0:1e09cd7d66b4 1945 break;
nerit 0:1e09cd7d66b4 1946 case 6:
nerit 0:1e09cd7d66b4 1947 break;
nerit 0:1e09cd7d66b4 1948 case 7:
nerit 0:1e09cd7d66b4 1949 break;
nerit 0:1e09cd7d66b4 1950 case 8:
nerit 0:1e09cd7d66b4 1951 break;
nerit 0:1e09cd7d66b4 1952 case 9:
nerit 0:1e09cd7d66b4 1953 break;
nerit 0:1e09cd7d66b4 1954 case 10:
nerit 0:1e09cd7d66b4 1955 break;
nerit 0:1e09cd7d66b4 1956 case 11:
nerit 0:1e09cd7d66b4 1957 break;
nerit 0:1e09cd7d66b4 1958 case 12:
nerit 0:1e09cd7d66b4 1959 break;
nerit 0:1e09cd7d66b4 1960 case 13:
nerit 0:1e09cd7d66b4 1961 break;
nerit 0:1e09cd7d66b4 1962 case 14:
nerit 0:1e09cd7d66b4 1963 SDmotorPWM=1; // power mosfet 2 command out
nerit 0:1e09cd7d66b4 1964 break;
nerit 0:1e09cd7d66b4 1965 case 15:
nerit 0:1e09cd7d66b4 1966 break;
nerit 0:1e09cd7d66b4 1967 case 16:
nerit 0:1e09cd7d66b4 1968 case 17:
nerit 0:1e09cd7d66b4 1969 break;
nerit 0:1e09cd7d66b4 1970 case 18:
nerit 0:1e09cd7d66b4 1971 TBmotorStepOut=0; // define step command for up down motor driver
nerit 0:1e09cd7d66b4 1972 SDmotorPWM=0; // define step command for seeding whell motor driver
nerit 0:1e09cd7d66b4 1973 speedClock=0; // define input of
nerit 0:1e09cd7d66b4 1974 SDmotorInA=0;
nerit 0:1e09cd7d66b4 1975 SDmotorInB=0;
nerit 0:1e09cd7d66b4 1976 SDmotorPWM=0; // power mosfet 2 command out
nerit 0:1e09cd7d66b4 1977 break;
nerit 0:1e09cd7d66b4 1978 default:
nerit 0:1e09cd7d66b4 1979 clocca=0;
nerit 0:1e09cd7d66b4 1980 break;
nerit 0:1e09cd7d66b4 1981 }
nerit 0:1e09cd7d66b4 1982 wait_ms(100);
nerit 0:1e09cd7d66b4 1983 #endif // end prova scheda
nerit 0:1e09cd7d66b4 1984
nerit 0:1e09cd7d66b4 1985 #if defined(provaDC)
nerit 0:1e09cd7d66b4 1986 int rampa=1000;
nerit 0:1e09cd7d66b4 1987 int pausa=3000;
nerit 0:1e09cd7d66b4 1988 switch (clocca){
nerit 0:1e09cd7d66b4 1989 case 0:
nerit 0:1e09cd7d66b4 1990 DC_brake=0;
nerit 0:1e09cd7d66b4 1991 DC_forward=1;
nerit 0:1e09cd7d66b4 1992 duty_DC+=0.01f;
nerit 7:c9fd242538d9 1993 if (duty_DC>=1.0f){
nerit 7:c9fd242538d9 1994 duty_DC=1.0f;
nerit 0:1e09cd7d66b4 1995 clocca=1;
nerit 0:1e09cd7d66b4 1996 }
nerit 0:1e09cd7d66b4 1997 wait_ms(rampa);
nerit 0:1e09cd7d66b4 1998 break;
nerit 0:1e09cd7d66b4 1999 case 1:
nerit 0:1e09cd7d66b4 2000 wait_ms(pausa*4);
nerit 0:1e09cd7d66b4 2001 clocca=2;
nerit 0:1e09cd7d66b4 2002 break;
nerit 0:1e09cd7d66b4 2003 case 2:
nerit 0:1e09cd7d66b4 2004 DC_brake=0;
nerit 0:1e09cd7d66b4 2005 DC_forward=1;
nerit 0:1e09cd7d66b4 2006 duty_DC-=0.01f;
nerit 0:1e09cd7d66b4 2007 if (duty_DC<=0.0f){
nerit 0:1e09cd7d66b4 2008 duty_DC=0.0f;
nerit 0:1e09cd7d66b4 2009 clocca=3;
nerit 0:1e09cd7d66b4 2010 }
nerit 0:1e09cd7d66b4 2011 wait_ms(rampa);
nerit 0:1e09cd7d66b4 2012 break;
nerit 0:1e09cd7d66b4 2013 case 3:
nerit 0:1e09cd7d66b4 2014 wait_ms(pausa);
nerit 0:1e09cd7d66b4 2015 clocca=4;
nerit 0:1e09cd7d66b4 2016 break;
nerit 0:1e09cd7d66b4 2017 case 4:
nerit 0:1e09cd7d66b4 2018 DC_brake=0;
nerit 0:1e09cd7d66b4 2019 DC_forward=1;
nerit 0:1e09cd7d66b4 2020 duty_DC+=0.01f;
nerit 0:1e09cd7d66b4 2021 if (duty_DC>=1.0f){
nerit 0:1e09cd7d66b4 2022 duty_DC=1.0f;
nerit 0:1e09cd7d66b4 2023 clocca=5;
nerit 0:1e09cd7d66b4 2024 }
nerit 0:1e09cd7d66b4 2025 wait_ms(rampa);
nerit 0:1e09cd7d66b4 2026 break;
nerit 0:1e09cd7d66b4 2027 case 5:
nerit 0:1e09cd7d66b4 2028 wait_ms(pausa);
nerit 0:1e09cd7d66b4 2029 clocca=6;
nerit 0:1e09cd7d66b4 2030 break;
nerit 0:1e09cd7d66b4 2031 case 6:
nerit 0:1e09cd7d66b4 2032 DC_brake=0;
nerit 0:1e09cd7d66b4 2033 DC_forward=1;
nerit 0:1e09cd7d66b4 2034 duty_DC-=0.01f;
nerit 0:1e09cd7d66b4 2035 if (duty_DC<=0.0f){
nerit 0:1e09cd7d66b4 2036 duty_DC=0.0f;
nerit 0:1e09cd7d66b4 2037 clocca=7;
nerit 0:1e09cd7d66b4 2038 }
nerit 0:1e09cd7d66b4 2039 wait_ms(rampa);
nerit 0:1e09cd7d66b4 2040 break;
nerit 0:1e09cd7d66b4 2041 case 7:
nerit 0:1e09cd7d66b4 2042 wait_ms(pausa);
nerit 0:1e09cd7d66b4 2043 clocca=0;
nerit 0:1e09cd7d66b4 2044 break;
nerit 0:1e09cd7d66b4 2045 default:
nerit 0:1e09cd7d66b4 2046 break;
nerit 0:1e09cd7d66b4 2047 }
nerit 0:1e09cd7d66b4 2048 if (oldDuty_DC != duty_DC){
nerit 7:c9fd242538d9 2049 SDmotorPWM.write(1.0f-duty_DC); // duty cycle = stop
nerit 0:1e09cd7d66b4 2050 oldDuty_DC=duty_DC;
nerit 0:1e09cd7d66b4 2051 DC_prepare();
nerit 0:1e09cd7d66b4 2052 }
nerit 0:1e09cd7d66b4 2053 #endif
nerit 0:1e09cd7d66b4 2054 }//end in prova
nerit 0:1e09cd7d66b4 2055 wd.Service(); // kick the dog before the timeout
nerit 0:1e09cd7d66b4 2056 } // end while
nerit 0:1e09cd7d66b4 2057 }// end main
nerit 0:1e09cd7d66b4 2058