tr

Dependencies:   mbed CANMsg

Committer:
nerit
Date:
Fri Jun 15 07:01:13 2018 +0000
Revision:
1:e88bf5011af6
Parent:
0:1e09cd7d66b4
Child:
2:d9c7430ae953
update prima del quinconcio

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 0:1e09cd7d66b4 94 // rise of seed speed 25 pulse sensor
nerit 0:1e09cd7d66b4 95 void sd25Fall(){
nerit 0:1e09cd7d66b4 96 timeHole=metalTimer.read_ms();
nerit 0:1e09cd7d66b4 97 int memo_TimeHole= (memoTimeHole + timeHole)/ (int)2;
nerit 0:1e09cd7d66b4 98 memoTimeHole = timeHole;
nerit 0:1e09cd7d66b4 99 metalTimer.reset();
nerit 0:1e09cd7d66b4 100 if (speedFromPick==0){
nerit 0:1e09cd7d66b4 101 speedOfSeedWheel=((seedPerimeter/25.0f)/(double)memo_TimeHole)*1000.0f; //mtS
nerit 0:1e09cd7d66b4 102 }
nerit 0:1e09cd7d66b4 103 }
nerit 0:1e09cd7d66b4 104 //**************************************************
nerit 0:1e09cd7d66b4 105 // generate speed clock when speed is simulated from Tritecnica display
nerit 0:1e09cd7d66b4 106 void speedSimulationClock(){
nerit 0:1e09cd7d66b4 107 lastPulseRead=speedTimer.read_us();
nerit 0:1e09cd7d66b4 108 oldLastPulseRead=lastPulseRead;
nerit 0:1e09cd7d66b4 109 speedTimer.reset();
nerit 0:1e09cd7d66b4 110 pulseRised=1;
nerit 0:1e09cd7d66b4 111 }
nerit 0:1e09cd7d66b4 112 //*******************************************************
nerit 0:1e09cd7d66b4 113 // interrupt task for tractor speed reading
nerit 0:1e09cd7d66b4 114 //*******************************************************
nerit 0:1e09cd7d66b4 115 void tractorReadSpeed(){
nerit 0:1e09cd7d66b4 116 if ((oldTractorSpeedRead==0)){
nerit 0:1e09cd7d66b4 117 lastPulseRead=speedTimer.read_us();
nerit 0:1e09cd7d66b4 118 oldLastPulseRead=lastPulseRead;
nerit 0:1e09cd7d66b4 119 speedTimer.reset();
nerit 0:1e09cd7d66b4 120 pulseRised=1;
nerit 0:1e09cd7d66b4 121 oldTractorSpeedRead=1;
nerit 0:1e09cd7d66b4 122 }
nerit 0:1e09cd7d66b4 123 speedFilter.reset();
nerit 0:1e09cd7d66b4 124 speedClock=1;
nerit 0:1e09cd7d66b4 125 }
nerit 0:1e09cd7d66b4 126 //*******************************************************
nerit 0:1e09cd7d66b4 127 void speedMediaCalc(){
nerit 0:1e09cd7d66b4 128 double lastPd=(double) lastPulseRead/1000.0f;
nerit 0:1e09cd7d66b4 129 pulseSpeedInterval = (mediaSpeed[0]+lastPd)/2.0f;
nerit 0:1e09cd7d66b4 130 if (enableSimula==1){
nerit 0:1e09cd7d66b4 131 double TMT = (double)(speedSimula) * 100.0f /3600.0f;
nerit 0:1e09cd7d66b4 132 pulseSpeedInterval = pulseDistance / TMT;
nerit 0:1e09cd7d66b4 133 }
nerit 0:1e09cd7d66b4 134 mediaSpeed[0]=lastPd;
nerit 0:1e09cd7d66b4 135 OLDpulseSpeedInterval=pulseSpeedInterval;
nerit 0:1e09cd7d66b4 136 }
nerit 0:1e09cd7d66b4 137
nerit 0:1e09cd7d66b4 138 //*******************************************************
nerit 0:1e09cd7d66b4 139 // clocked task for manage virtual encoder of seed wheel i/o
nerit 0:1e09cd7d66b4 140 //*******************************************************
nerit 0:1e09cd7d66b4 141 //*******************************************************
nerit 0:1e09cd7d66b4 142 void step_SDPulseOut(){
nerit 0:1e09cd7d66b4 143 SDactualPosition++;
nerit 0:1e09cd7d66b4 144 prePosSD++;
nerit 0:1e09cd7d66b4 145 }
nerit 0:1e09cd7d66b4 146 //*******************************************************
nerit 0:1e09cd7d66b4 147 void step_TBPulseOut(){
nerit 0:1e09cd7d66b4 148 TBmotorStepOut=!TBmotorStepOut;
nerit 0:1e09cd7d66b4 149 if (TBmotorStepOut==0){
nerit 0:1e09cd7d66b4 150 TBactualPosition++;
nerit 0:1e09cd7d66b4 151 }
nerit 0:1e09cd7d66b4 152 }
nerit 0:1e09cd7d66b4 153 //*******************************************************
nerit 0:1e09cd7d66b4 154 // aggiornamento parametri di lavoro fissi e da Tritecnica
nerit 0:1e09cd7d66b4 155 void aggiornaParametri(){
nerit 0:1e09cd7d66b4 156 speedPerimeter = Pi * speedWheelDiameter ; // perimeter of speed wheel
nerit 0:1e09cd7d66b4 157 pulseDistance = (speedPerimeter / speedWheelPulse)*1000.0f; // linear space between speed wheel pulse
nerit 0:1e09cd7d66b4 158 seedPerimeter = Pi * (seedWheelDiameter-(deepOfSeed*2.0f)); // perimeter of seed wheel
nerit 0:1e09cd7d66b4 159 intraPickDistance = seedPerimeter/pickNumber;
nerit 0:1e09cd7d66b4 160 K_WheelRPM = 60.0f/seedPerimeter; // calcola il K per i giri al minuto della ruota di semina
nerit 0:1e09cd7d66b4 161 K_WhellFrequency = (seedWheelMotorSteps*SDreductionRatio)/60.0f; // calcola il K per la frequenza di comando del motore di semina
nerit 0:1e09cd7d66b4 162 rapportoRuote = pickNumber/cellsNumber; // calcola il rapporto tra il numero di becchi ed il numero di celle
nerit 0:1e09cd7d66b4 163 K_TBfrequency = (TBmotorSteps*TBreductionRatio)/240.0f;
nerit 0:1e09cd7d66b4 164 K_percentuale = TBmotorSteps*TBreductionRatio;
nerit 0:1e09cd7d66b4 165 SDsectorStep = (double)fixedStepGiroSD / (double)pickNumber;
nerit 0:1e09cd7d66b4 166 TBsectorStep = (TBmotorSteps*TBreductionRatio)/cellsNumber;
nerit 0:1e09cd7d66b4 167 KcorT = (SDsectorStep/TBsectorStep);///2.0f;
nerit 0:1e09cd7d66b4 168 angoloFase=angoloPh;
nerit 0:1e09cd7d66b4 169 avvioGradi=angoloAv;
nerit 0:1e09cd7d66b4 170 stepGrado=fixedStepGiroSD/360.0f;
nerit 0:1e09cd7d66b4 171 TBdeltaStep=(fixedStepGiroSD/pickNumber)+(stepGrado*avvioGradi);
nerit 0:1e09cd7d66b4 172 TBfaseStep = (stepGrado*angoloFase);
nerit 0:1e09cd7d66b4 173 K_WheelRPM = 60.0f/seedPerimeter; // calcola il K per i giri al minuto della ruota di semina 25.4 25,40
nerit 0:1e09cd7d66b4 174 if (speedFromPick==1) {
nerit 0:1e09cd7d66b4 175 intraPickDistance = seedPerimeter/pickNumber;
nerit 0:1e09cd7d66b4 176 }else{
nerit 0:1e09cd7d66b4 177 intraPickDistance = seedPerimeter/25.0f; // 25 è il numero di fori presenti nel disco di semina
nerit 0:1e09cd7d66b4 178 }
nerit 0:1e09cd7d66b4 179 if (anticipoMax > ((360.0f/pickNumber)-(avvioGradi+16.0f))){
nerit 0:1e09cd7d66b4 180 anticipoMax=(360.0f/pickNumber)-(avvioGradi+16.0f);
nerit 0:1e09cd7d66b4 181 }
nerit 0:1e09cd7d66b4 182 }
nerit 0:1e09cd7d66b4 183 //*******************************************************
nerit 0:1e09cd7d66b4 184 void cambiaTB(double perio){
nerit 0:1e09cd7d66b4 185 // update TB frequency
nerit 0:1e09cd7d66b4 186 double TBper=0.0f;
nerit 0:1e09cd7d66b4 187 if (aspettaStart==0){
nerit 0:1e09cd7d66b4 188 if (perio<250.0f){perio=500.0f;}
nerit 0:1e09cd7d66b4 189 double scala =0.0f;
nerit 0:1e09cd7d66b4 190 if (lowSpeed==1){
nerit 0:1e09cd7d66b4 191 scala =2.0f;
nerit 0:1e09cd7d66b4 192 }else{
nerit 0:1e09cd7d66b4 193 scala =1.8f;
nerit 0:1e09cd7d66b4 194 }
nerit 0:1e09cd7d66b4 195 TBper=perio/scala;
nerit 0:1e09cd7d66b4 196 if (oldPeriodoTB!=TBper){
nerit 0:1e09cd7d66b4 197 if (TBper >= 250.0f){
nerit 0:1e09cd7d66b4 198 TBticker.attach_us(&step_TBPulseOut,TBper); // clock time are milliseconds and attach seed motor stepper controls
nerit 0:1e09cd7d66b4 199 }else{
nerit 0:1e09cd7d66b4 200 TBticker.detach();
nerit 0:1e09cd7d66b4 201 }
nerit 0:1e09cd7d66b4 202 oldPeriodoTB=TBper;
nerit 0:1e09cd7d66b4 203 }
nerit 0:1e09cd7d66b4 204 }
nerit 0:1e09cd7d66b4 205 }
nerit 0:1e09cd7d66b4 206 //*******************************************************
nerit 0:1e09cd7d66b4 207 void seedCorrect(){
nerit 0:1e09cd7d66b4 208 /*
nerit 0:1e09cd7d66b4 209 posError determina la posizione relativa di TB rispetto ad SD
nerit 0:1e09cd7d66b4 210 la reale posizione di SD viene modificata in funzione della velocità per
nerit 0:1e09cd7d66b4 211 traslare la posizione relativa di TB. All'aumentare della velocità la posizione
nerit 0:1e09cd7d66b4 212 di SD viene incrementata così che TB acceleri per raggiungerla in modo da rilasciare il seme prima
nerit 0:1e09cd7d66b4 213 La taratura del sistema avviene determinando prima il valore di angoloFase alla minima velocità,
nerit 0:1e09cd7d66b4 214 poi, alla massima velocità, dovrebbe spostarsi la posizione relativa con una variabile proporzionale alla velocità, ma c'è un però.
nerit 0:1e09cd7d66b4 215 Il problema è che il momento di avvio determina una correzione dell'angolo di partenza del tamburo
nerit 0:1e09cd7d66b4 216 angolo che viene rideterminato ogni volta che il sensore becchi legge un transito.
nerit 0:1e09cd7d66b4 217 Di fatto c'è una concorrenza tra l'angolo di avvio determinato e la correzione di posizione relativa
nerit 0:1e09cd7d66b4 218 del tamburo. E' molto probabile che convenga modificare solo la posizione relativa e non anche l'angolo di avvio
nerit 0:1e09cd7d66b4 219 Ancora di più se viene eliminata la parte gestita da ciclata.
nerit 0:1e09cd7d66b4 220 In questo modo dovrebbe esserci solo un andamento in accelerazione di TB che viene poi eventualmente decelerato
nerit 0:1e09cd7d66b4 221 dal passaggio sul sensore di TB. Funzione corretta perchè il sincronismo tra i sensori genera l'inibizione della correzione
nerit 0:1e09cd7d66b4 222 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 223 Appena il sensore di TB viene impegnato allora viene abilitato il controllo di fase del tamburo.
nerit 0:1e09cd7d66b4 224 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 225 il tamburo non risenta della condizione di reset della posizione di SD mentre lui è ancora nella fase precedente. Si fermerebbe.
nerit 0:1e09cd7d66b4 226
nerit 0:1e09cd7d66b4 227 // La considerazione finale è che mantenendo l'angolo di avvio fisso e regolato sulla bassa velocità, intervenendo solo sulla correzione
nerit 0:1e09cd7d66b4 228 // di posizione in questa routine, dovrebbe essere possibile seminare correttamente a tutte le velocità regolando solo 2 parametri.
nerit 0:1e09cd7d66b4 229 */
nerit 0:1e09cd7d66b4 230 /*
nerit 0:1e09cd7d66b4 231 SDsectorStep = (double)fixedStepGiroSD / (double)pickNumber;
nerit 0:1e09cd7d66b4 232 TBsectorStep = (TBmotorSteps*TBreductionRatio)/cellsNumber;
nerit 0:1e09cd7d66b4 233 KcorT = (SDsectorStep/TBsectorStep);
nerit 0:1e09cd7d66b4 234 angoloFase=angoloPh;
nerit 0:1e09cd7d66b4 235 stepGrado=fixedStepGiroSD/360.0f;
nerit 0:1e09cd7d66b4 236 avvioGradi = costante da terminale tritecnica
nerit 0:1e09cd7d66b4 237 TBdeltaStep=(fixedStepGiroSD/pickNumber)-(stepGrado*avvioGradi);
nerit 0:1e09cd7d66b4 238 TBfaseStep = (stepGrado*angoloFase);
nerit 0:1e09cd7d66b4 239 anticipoMax = valore in gradi del massimo anticipo proporzionale alla velocità della ruota di semina
nerit 0:1e09cd7d66b4 240 */
nerit 0:1e09cd7d66b4 241
nerit 0:1e09cd7d66b4 242 if ((tractorSpeed_MtS_timed>0.01f)){
nerit 0:1e09cd7d66b4 243 if (inhibit==0){
nerit 0:1e09cd7d66b4 244 double posError =0.0f;
nerit 0:1e09cd7d66b4 245 double posSD=((double)SDactualPosition)/KcorT;
nerit 0:1e09cd7d66b4 246 posError = posSD - (double)TBactualPosition;
nerit 0:1e09cd7d66b4 247 // interviene sulla velocità di TB per raggiungere la corretta posizione relativa
nerit 0:1e09cd7d66b4 248 if((lowSpeed==0)&&(aspettaStart==0)){
nerit 0:1e09cd7d66b4 249 if (posError>50.0f){posError=50.0f;}
nerit 0:1e09cd7d66b4 250 if (posError<-50.0f){posError=-50.0f;}
nerit 0:1e09cd7d66b4 251 if ((posError >=1.0f)||(posError<=-1.0f)){
nerit 0:1e09cd7d66b4 252 ePpos = periodo /(1.0f+ ((posError/100.0f)));
nerit 0:1e09cd7d66b4 253 cambiaTB(ePpos);
nerit 0:1e09cd7d66b4 254 }
nerit 0:1e09cd7d66b4 255 }
nerit 0:1e09cd7d66b4 256 }
nerit 0:1e09cd7d66b4 257 #if defined(pcSerial)
nerit 0:1e09cd7d66b4 258 #if defined(SDreset)
nerit 0:1e09cd7d66b4 259 pc.printf("Fase: %d",fase);
nerit 0:1e09cd7d66b4 260 pc.printf(" PrePosSD: %d",prePosSD);
nerit 0:1e09cd7d66b4 261 pc.printf(" PosSD: %d",SDactualPosition);
nerit 0:1e09cd7d66b4 262 pc.printf(" speed: %f",tractorSpeed_MtS_timed);
nerit 0:1e09cd7d66b4 263 pc.printf(" Trigger: %d \n", trigRepos);
nerit 0:1e09cd7d66b4 264 #endif
nerit 0:1e09cd7d66b4 265 #endif
nerit 0:1e09cd7d66b4 266 }
nerit 0:1e09cd7d66b4 267 }
nerit 0:1e09cd7d66b4 268 //*******************************************************
nerit 0:1e09cd7d66b4 269 void videoUpdate(){
nerit 0:1e09cd7d66b4 270 for(int aa=0;aa<4;aa++){speedForDisplay[aa]=speedForDisplay[aa+1];}
nerit 0:1e09cd7d66b4 271 speedForDisplay[4]=tractorSpeed_MtS_timed;
nerit 0:1e09cd7d66b4 272 totalSpeed=0.0f;
nerit 0:1e09cd7d66b4 273 for (int aa=0; aa<5; aa++){totalSpeed += speedForDisplay[aa];}
nerit 0:1e09cd7d66b4 274 totalSpeed = totalSpeed / 5.0f;
nerit 0:1e09cd7d66b4 275 }
nerit 0:1e09cd7d66b4 276 //*******************************************************
nerit 0:1e09cd7d66b4 277 void ciclaTB(){
nerit 0:1e09cd7d66b4 278 if ((startCicloTB==1)&&(cicloTbinCorso==0)){
nerit 0:1e09cd7d66b4 279 TBticker.attach_us(&step_TBPulseOut,TBperiod/2.5f); // clock time are milliseconds and attach seed motor stepper controls
nerit 0:1e09cd7d66b4 280 cicloTbinCorso = 1;
nerit 0:1e09cd7d66b4 281 startCicloTB=0;
nerit 0:1e09cd7d66b4 282 }
nerit 0:1e09cd7d66b4 283 if ((loadDaCan==1)&&(loadDaCanInCorso==0)){
nerit 0:1e09cd7d66b4 284 TBticker.attach_us(&step_TBPulseOut,1000.0f); // clock time are milliseconds and attach seed motor stepper controls
nerit 0:1e09cd7d66b4 285 loadDaCanInCorso=1;
nerit 0:1e09cd7d66b4 286 stopCicloTB=0;
nerit 0:1e09cd7d66b4 287 }
nerit 0:1e09cd7d66b4 288 if ((stopCicloTB==1)&&(TBactualPosition>5)){
nerit 0:1e09cd7d66b4 289 TBticker.detach();
nerit 0:1e09cd7d66b4 290 cicloTbinCorso = 0;
nerit 0:1e09cd7d66b4 291 stopCicloTB=0;
nerit 0:1e09cd7d66b4 292 loadDaCanInCorso=0;
nerit 0:1e09cd7d66b4 293 loadDaCan=0;
nerit 0:1e09cd7d66b4 294 }
nerit 0:1e09cd7d66b4 295 }
nerit 0:1e09cd7d66b4 296 //*******************************************************
nerit 0:1e09cd7d66b4 297 void allarmi(){
nerit 0:1e09cd7d66b4 298 uint8_t alarmLowRegister1=0x00;
nerit 0:1e09cd7d66b4 299 alarmLowRegister=0x00;
nerit 0:1e09cd7d66b4 300 alarmHighRegister=0x80;
nerit 0:1e09cd7d66b4 301
nerit 0:1e09cd7d66b4 302 //alarmLowRegister=alarmLowRegister+(all_semiFiniti*0x01); // manca il sensore
nerit 0:1e09cd7d66b4 303 alarmLowRegister=alarmLowRegister+(all_pickSignal*0x02); // fatto
nerit 0:1e09cd7d66b4 304 alarmLowRegister=alarmLowRegister+(all_cellSignal*0x04); // fatto
nerit 0:1e09cd7d66b4 305 alarmLowRegister=alarmLowRegister+(all_lowBattery*0x08); // fatto
nerit 0:1e09cd7d66b4 306 alarmLowRegister=alarmLowRegister+(all_overCurrDC*0x10); // fatto
nerit 0:1e09cd7d66b4 307 alarmLowRegister=alarmLowRegister+(all_stopSistem*0x20); // verificarne la necessità
nerit 0:1e09cd7d66b4 308 //alarmLowRegister=alarmLowRegister+(all_upElements*0x40); // manca il sensore
nerit 0:1e09cd7d66b4 309 //alarmLowRegister=alarmLowRegister+(all_noSeedOnCe*0x80); // manca il sensore
nerit 0:1e09cd7d66b4 310
nerit 0:1e09cd7d66b4 311 //alarmLowRegister1=alarmLowRegister1+(all_cfgnErrors*0x01); // da scrivere
nerit 0:1e09cd7d66b4 312 alarmLowRegister1=alarmLowRegister1+(all_noDcRotati*0x02); // fatto
nerit 0:1e09cd7d66b4 313 alarmLowRegister1=alarmLowRegister1+(all_noStepRota*0x04); // fatto
nerit 0:1e09cd7d66b4 314 alarmLowRegister1=alarmLowRegister1+(all_speedError*0x08); // fatto
nerit 0:1e09cd7d66b4 315 alarmLowRegister1=alarmLowRegister1+(all_noSpeedSen*0x10); // fatto
nerit 0:1e09cd7d66b4 316 alarmLowRegister1=alarmLowRegister1+(all_no_Zeroing*0x20); // fatto
nerit 0:1e09cd7d66b4 317 alarmLowRegister1=alarmLowRegister1+(all_genericals*0x40);
nerit 0:1e09cd7d66b4 318 if (alarmLowRegister1 > 0){
nerit 0:1e09cd7d66b4 319 alarmHighRegister = 0x81;
nerit 0:1e09cd7d66b4 320 alarmLowRegister = alarmLowRegister1;
nerit 0:1e09cd7d66b4 321 }
nerit 0:1e09cd7d66b4 322
nerit 0:1e09cd7d66b4 323 #if defined(pcSerial)
nerit 0:1e09cd7d66b4 324 #if defined(VediAllarmi)
nerit 0:1e09cd7d66b4 325 if (all_pickSignal==1){pc.printf("AllarmeBecchi\n");}
nerit 0:1e09cd7d66b4 326 if (all_cellSignal==1){pc.printf("AllarmeCelle\n");}
nerit 0:1e09cd7d66b4 327 if (all_lowBattery==1){pc.printf("AllarmeBassaCorrente\n");}
nerit 0:1e09cd7d66b4 328 if (all_overCurrDC==1){pc.printf("AllarmeAltaCorrente\n");}
nerit 0:1e09cd7d66b4 329 if (all_stopSistem==1){pc.printf("AllarmeStop\n");}
nerit 0:1e09cd7d66b4 330 if (all_noDcRotati==1){pc.printf("AllarmeDCnoRotation\n");}
nerit 0:1e09cd7d66b4 331 if (all_noStepRota==1){pc.printf("AllarmeNoStepRotation\n");}
nerit 0:1e09cd7d66b4 332 if (all_speedError==1){pc.printf("AllarmeSpeedError\n");}
nerit 0:1e09cd7d66b4 333 if (all_noSpeedSen==1){pc.printf("AllarmeNoSpeedSensor\n");}
nerit 0:1e09cd7d66b4 334 if (all_no_Zeroing==1){pc.printf("AllarmeNoZero\n");}
nerit 0:1e09cd7d66b4 335 if (all_genericals==1){pc.printf("AllarmeGenerico\n");}
nerit 0:1e09cd7d66b4 336 pc.printf("Code: 0x%x%x\n",alarmHighRegister,alarmLowRegister);
nerit 0:1e09cd7d66b4 337 #endif
nerit 0:1e09cd7d66b4 338 #endif
nerit 0:1e09cd7d66b4 339 all_semiFiniti=0;
nerit 0:1e09cd7d66b4 340 all_pickSignal=0;
nerit 0:1e09cd7d66b4 341 all_cellSignal=0;
nerit 0:1e09cd7d66b4 342 all_lowBattery=0;
nerit 0:1e09cd7d66b4 343 all_overCurrDC=0;
nerit 0:1e09cd7d66b4 344 all_stopSistem=0;
nerit 0:1e09cd7d66b4 345 all_upElements=0;
nerit 0:1e09cd7d66b4 346 all_noSeedOnCe=0;
nerit 0:1e09cd7d66b4 347 all_cfgnErrors=0;
nerit 0:1e09cd7d66b4 348 all_noDcRotati=0;
nerit 0:1e09cd7d66b4 349 all_noStepRota=0;
nerit 0:1e09cd7d66b4 350 all_speedError=0;
nerit 0:1e09cd7d66b4 351 all_noSpeedSen=0;
nerit 0:1e09cd7d66b4 352 all_no_Zeroing=0;
nerit 0:1e09cd7d66b4 353 all_genericals=0;
nerit 0:1e09cd7d66b4 354 }
nerit 0:1e09cd7d66b4 355 //*******************************************************
nerit 0:1e09cd7d66b4 356 #if defined(speedMaster)
nerit 0:1e09cd7d66b4 357 void upDateSincro(){
nerit 0:1e09cd7d66b4 358 char val1[8]={0,0,0,0,0,0,0,0};
nerit 0:1e09cd7d66b4 359 val1[0]=(prePosSD /0x00FF0000)&0x000000FF;
nerit 0:1e09cd7d66b4 360 val1[1]=(prePosSD /0x0000FF00)&0x000000FF;
nerit 0:1e09cd7d66b4 361 val1[2]=(prePosSD /0x000000FF)&0x000000FF;
nerit 0:1e09cd7d66b4 362 val1[3]=prePosSD & 0x000000FF;
nerit 0:1e09cd7d66b4 363 #if defined(canbusActive)
nerit 0:1e09cd7d66b4 364 #if defined(speedMaster)
nerit 0:1e09cd7d66b4 365 if(can1.write(CANMessage(TX_SI, *&val1,8))){
nerit 0:1e09cd7d66b4 366 checkState=0;
nerit 0:1e09cd7d66b4 367 }
nerit 0:1e09cd7d66b4 368 #endif
nerit 0:1e09cd7d66b4 369 #endif
nerit 0:1e09cd7d66b4 370 }
nerit 0:1e09cd7d66b4 371 #endif
nerit 0:1e09cd7d66b4 372 //*******************************************************
nerit 0:1e09cd7d66b4 373 void upDateSpeed(){
nerit 0:1e09cd7d66b4 374 /*
nerit 0:1e09cd7d66b4 375 aggiorna dati OPUSA3
nerit 0:1e09cd7d66b4 376 val1[0] contiene il dato di velocità
nerit 0:1e09cd7d66b4 377 val1[1] contiene il byte basso della tabella allarmi
nerit 0:1e09cd7d66b4 378 uint8_t all_semiFiniti = 0; // semi finiti (generato dal relativo sensore)
nerit 0:1e09cd7d66b4 379 uint8_t all_pickSignal = 0; // errore segnale becchi (generato dal tempo tra un becco ed il successivo)
nerit 0:1e09cd7d66b4 380 uint8_t all_cellSignal = 0; // errore segnale celle (generato dal sensore tamburo )
nerit 0:1e09cd7d66b4 381 uint8_t all_lowBattery = 0; // allarme batteria (valore interno di tritecnica)
nerit 0:1e09cd7d66b4 382 uint8_t all_overCurrDC = 0; // sovracorrente motore DC (generato dal sensore di corrente)
nerit 0:1e09cd7d66b4 383 uint8_t all_stopSistem = 0; // sistema in stop (a tempo con ruota ferma)
nerit 0:1e09cd7d66b4 384 uint8_t all_upElements = 0; // elementi sollevati (generato dal relativo sensore)
nerit 0:1e09cd7d66b4 385 uint8_t all_noSeedOnCe = 0; // fallanza di semina (manca il seme nella cella)
nerit 0:1e09cd7d66b4 386 uint8_t all_cfgnErrors = 0; // errore di configurazione (incongruenza dei parametri impostati)
nerit 0:1e09cd7d66b4 387 uint8_t all_noDcRotati = 0; // ruota di semina bloccata (arriva la velocità ma non i becchi)
nerit 0:1e09cd7d66b4 388 uint8_t all_noStepRota = 0; // tamburo di semina bloccato (comando il tamburo ma non ricevo il sensore)
nerit 0:1e09cd7d66b4 389 uint8_t all_speedError = 0; // errore sensore velocità (tempo impulsi non costante)
nerit 0:1e09cd7d66b4 390 uint8_t all_noSpeedSen = 0; // mancanza segnale di velocità (sento i becchi ma non la velocita)
nerit 0:1e09cd7d66b4 391 uint8_t all_no_Zeroing = 0; // mancato azzeramento sistema (generato dal timeout del comando motore DC)
nerit 0:1e09cd7d66b4 392 uint8_t all_genericals = 0; // allarme generico
nerit 0:1e09cd7d66b4 393 val1[2] contiene il byte alto della tabella di allarmi
nerit 0:1e09cd7d66b4 394 val1[3] contiene i segnali per la diagnostica
nerit 0:1e09cd7d66b4 395 bit 0= sensore ruota fonica
nerit 0:1e09cd7d66b4 396 bit 1= sensore celle
nerit 0:1e09cd7d66b4 397 bit 2= sensore posizione
nerit 0:1e09cd7d66b4 398 bit 3= sensore becchi
nerit 0:1e09cd7d66b4 399 bit 4= motore DC
nerit 0:1e09cd7d66b4 400 bit 5= controller
nerit 0:1e09cd7d66b4 401 bit 6= motore stepper
nerit 0:1e09cd7d66b4 402 */
nerit 0:1e09cd7d66b4 403 char val1[8]={0,0,0,0,0,0,0,0};
nerit 0:1e09cd7d66b4 404
nerit 0:1e09cd7d66b4 405 val1[3]=0;
nerit 0:1e09cd7d66b4 406 val1[3]=val1[3]+(tractorSpeedRead*0x01);
nerit 0:1e09cd7d66b4 407 val1[3]=val1[3]+(TBzeroPinInputRev*0x02);
nerit 0:1e09cd7d66b4 408 val1[3]=val1[3]+(ElementPosition*0x04);
nerit 0:1e09cd7d66b4 409 val1[3]=val1[3]+(seedWheelZeroPinInputRev*0x08);
nerit 0:1e09cd7d66b4 410 #if defined(simulaPerCan)
nerit 0:1e09cd7d66b4 411 if (buttonUser==0){
nerit 0:1e09cd7d66b4 412 val1[1]=0x02;
nerit 0:1e09cd7d66b4 413 }else{
nerit 0:1e09cd7d66b4 414 val1[1]=0x00;
nerit 0:1e09cd7d66b4 415 }
nerit 0:1e09cd7d66b4 416 val1[3]=valore;
nerit 0:1e09cd7d66b4 417 valore+=1;
nerit 0:1e09cd7d66b4 418 if(valore>50){
nerit 0:1e09cd7d66b4 419 valore=0;
nerit 0:1e09cd7d66b4 420 }
nerit 0:1e09cd7d66b4 421 tractorSpeed_MtS_timed=valore;
nerit 0:1e09cd7d66b4 422 #endif
nerit 0:1e09cd7d66b4 423 allarmi();
nerit 0:1e09cd7d66b4 424 valore = totalSpeed*36.0f; // tractorSpeed_MtS_timed*36.0f;
nerit 0:1e09cd7d66b4 425 val1[0]=valore;
nerit 0:1e09cd7d66b4 426 val1[1]=alarmHighRegister; // registro alto allarmi. Parte sempre da 0x80
nerit 0:1e09cd7d66b4 427 val1[2]=alarmLowRegister; // registro basso allarmi
nerit 0:1e09cd7d66b4 428 //val1[3]=val1[3];// registro di stato degli ingressi
nerit 0:1e09cd7d66b4 429 val1[4]=resetComandi;
nerit 0:1e09cd7d66b4 430 val1[5]=cellsCounterLow;
nerit 0:1e09cd7d66b4 431 val1[6]=cellsCounterHig;
nerit 0:1e09cd7d66b4 432 #if defined(canbusActive)
nerit 0:1e09cd7d66b4 433 if(can1.write(CANMessage(TX_ID, *&val1,8))){
nerit 0:1e09cd7d66b4 434 checkState=0;
nerit 0:1e09cd7d66b4 435 }
nerit 0:1e09cd7d66b4 436 #endif
nerit 0:1e09cd7d66b4 437 }
nerit 0:1e09cd7d66b4 438 //*******************************************************
nerit 0:1e09cd7d66b4 439 void leggiCAN(){
nerit 0:1e09cd7d66b4 440 #if defined(canbusActive)
nerit 0:1e09cd7d66b4 441 if(can1.read(rxMsg)) {
nerit 0:1e09cd7d66b4 442 if (firstStart==1){
nerit 0:1e09cd7d66b4 443 #if defined(speedMaster)
nerit 0:1e09cd7d66b4 444 sincUpdate.attach(&upDateSincro,0.01f);
nerit 0:1e09cd7d66b4 445 #endif
nerit 0:1e09cd7d66b4 446 canUpdate.attach(&upDateSpeed,0.11f);
nerit 0:1e09cd7d66b4 447 firstStart=0;
nerit 0:1e09cd7d66b4 448 }
nerit 0:1e09cd7d66b4 449
nerit 0:1e09cd7d66b4 450 if (rxMsg.id==RX_ID){
nerit 0:1e09cd7d66b4 451 #if defined(pcSerial)
nerit 0:1e09cd7d66b4 452 #if defined(canDataReceived)
nerit 0:1e09cd7d66b4 453 pc.printf("Messaggio ricevuto\n");
nerit 0:1e09cd7d66b4 454 #endif
nerit 0:1e09cd7d66b4 455 #endif
nerit 0:1e09cd7d66b4 456 }
nerit 0:1e09cd7d66b4 457 if (rxMsg.id==RX_Broadcast){
nerit 0:1e09cd7d66b4 458 #if defined(pcSerial)
nerit 0:1e09cd7d66b4 459 #if defined(canDataReceived)
nerit 0:1e09cd7d66b4 460 pc.printf("BroadCast ricevuto\n");
nerit 0:1e09cd7d66b4 461 #endif
nerit 0:1e09cd7d66b4 462 #endif
nerit 0:1e09cd7d66b4 463 enableSimula= rxMsg.data[0];
nerit 0:1e09cd7d66b4 464 speedSimula = rxMsg.data[1];
nerit 0:1e09cd7d66b4 465 avviaSimula = rxMsg.data[2];
nerit 0:1e09cd7d66b4 466 selezionato = rxMsg.data[3];
nerit 0:1e09cd7d66b4 467 comandiDaCan = rxMsg.data[4];
nerit 0:1e09cd7d66b4 468 #if defined(pcSerial)
nerit 0:1e09cd7d66b4 469 #if defined(canDataReceived)
nerit 0:1e09cd7d66b4 470 pc.printf("Comandi: %x\n",comandiDaCan);
nerit 0:1e09cd7d66b4 471 #endif
nerit 0:1e09cd7d66b4 472 #endif
nerit 0:1e09cd7d66b4 473 switch (comandiDaCan){
nerit 0:1e09cd7d66b4 474 case 1:
nerit 0:1e09cd7d66b4 475 case 3:
nerit 0:1e09cd7d66b4 476 azzeraDaCan=1;
nerit 0:1e09cd7d66b4 477 resetComandi=0x01;
nerit 0:1e09cd7d66b4 478 comandiDaCan=0;
nerit 0:1e09cd7d66b4 479 break;
nerit 0:1e09cd7d66b4 480 case 2:
nerit 0:1e09cd7d66b4 481 loadDaCan=1;
nerit 0:1e09cd7d66b4 482 resetComandi=0x02;
nerit 0:1e09cd7d66b4 483 comandiDaCan=0;
nerit 0:1e09cd7d66b4 484 break;
nerit 0:1e09cd7d66b4 485 default:
nerit 0:1e09cd7d66b4 486 comandiDaCan=0;
nerit 0:1e09cd7d66b4 487 resetComandi=0xFF;
nerit 0:1e09cd7d66b4 488 break;
nerit 0:1e09cd7d66b4 489 }
nerit 0:1e09cd7d66b4 490 #if defined(pcSerial)
nerit 0:1e09cd7d66b4 491 #if defined(canDataReceived)
nerit 0:1e09cd7d66b4 492 pc.printf("Comandi: %x\n",resetComandi);
nerit 0:1e09cd7d66b4 493 #endif
nerit 0:1e09cd7d66b4 494 #endif
nerit 0:1e09cd7d66b4 495 if (speedSimula>45){
nerit 0:1e09cd7d66b4 496 speedSimula=45;
nerit 0:1e09cd7d66b4 497 }
nerit 0:1e09cd7d66b4 498 // modulo 1
nerit 0:1e09cd7d66b4 499 if (RX_ID==0x100){
nerit 0:1e09cd7d66b4 500 if ((selezionato&0x01)==0x01){
nerit 0:1e09cd7d66b4 501 simOk=1;
nerit 0:1e09cd7d66b4 502 }else{
nerit 0:1e09cd7d66b4 503 simOk=0;
nerit 0:1e09cd7d66b4 504 }
nerit 0:1e09cd7d66b4 505 }
nerit 0:1e09cd7d66b4 506 // modulo 2
nerit 0:1e09cd7d66b4 507 if (RX_ID==0x102){
nerit 0:1e09cd7d66b4 508 if ((selezionato&0x02)==0x02){
nerit 0:1e09cd7d66b4 509 simOk=1;
nerit 0:1e09cd7d66b4 510 }else{
nerit 0:1e09cd7d66b4 511 simOk=0;
nerit 0:1e09cd7d66b4 512 }
nerit 0:1e09cd7d66b4 513 }
nerit 0:1e09cd7d66b4 514 // modulo 3
nerit 0:1e09cd7d66b4 515 if (RX_ID==0x104){
nerit 0:1e09cd7d66b4 516 if ((selezionato&0x04)==0x04){
nerit 0:1e09cd7d66b4 517 simOk=1;
nerit 0:1e09cd7d66b4 518 }else{
nerit 0:1e09cd7d66b4 519 simOk=0;
nerit 0:1e09cd7d66b4 520 }
nerit 0:1e09cd7d66b4 521 }
nerit 0:1e09cd7d66b4 522 // modulo 4
nerit 0:1e09cd7d66b4 523 if (RX_ID==0x106){
nerit 0:1e09cd7d66b4 524 if ((selezionato&0x08)==0x08){
nerit 0:1e09cd7d66b4 525 simOk=1;
nerit 0:1e09cd7d66b4 526 }else{
nerit 0:1e09cd7d66b4 527 simOk=0;
nerit 0:1e09cd7d66b4 528 }
nerit 0:1e09cd7d66b4 529 }
nerit 0:1e09cd7d66b4 530 // modulo 5
nerit 0:1e09cd7d66b4 531 if (RX_ID==0x108){
nerit 0:1e09cd7d66b4 532 if ((selezionato&0x10)==0x10){
nerit 0:1e09cd7d66b4 533 simOk=1;
nerit 0:1e09cd7d66b4 534 }else{
nerit 0:1e09cd7d66b4 535 simOk=0;
nerit 0:1e09cd7d66b4 536 }
nerit 0:1e09cd7d66b4 537 }
nerit 0:1e09cd7d66b4 538
nerit 0:1e09cd7d66b4 539 }
nerit 0:1e09cd7d66b4 540 if (rxMsg.id==RX_Settings){
nerit 0:1e09cd7d66b4 541 if (tractorSpeed_MtS_timed==0.0f){
nerit 0:1e09cd7d66b4 542 pickNumber = rxMsg.data[0]; // numero di becchi installati sulla ruota di semina
nerit 0:1e09cd7d66b4 543 cellsNumber = rxMsg.data[1]; // numero di celle del tamburo
nerit 0:1e09cd7d66b4 544 deepOfSeed=(rxMsg.data[2]/10000.0f); // deep of seeding
nerit 0:1e09cd7d66b4 545 seedWheelDiameter = ((rxMsg.data[4]*0xFF)+rxMsg.data[3])/10000.0f; // seed wheel diameter setting
nerit 0:1e09cd7d66b4 546 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 547 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 548 aggiornaParametri();
nerit 0:1e09cd7d66b4 549 #if defined(pcSerial)
nerit 0:1e09cd7d66b4 550 #if defined(canDataReceived)
nerit 0:1e09cd7d66b4 551 pc.printf("Seed wheel diameter %f \n",seedWheelDiameter);
nerit 0:1e09cd7d66b4 552 pc.printf("Speed wheel diameter %f \n",speedWheelDiameter);
nerit 0:1e09cd7d66b4 553 #endif
nerit 0:1e09cd7d66b4 554 #endif
nerit 0:1e09cd7d66b4 555 }
nerit 0:1e09cd7d66b4 556 }
nerit 0:1e09cd7d66b4 557 if (rxMsg.id==RX_AngoloPh){
nerit 0:1e09cd7d66b4 558 if (tractorSpeed_MtS_timed==0.0f){
nerit 0:1e09cd7d66b4 559 #if defined(M1)
nerit 0:1e09cd7d66b4 560 angoloPh = (double) rxMsg.data[0] ;
nerit 0:1e09cd7d66b4 561 aggiornaParametri();
nerit 0:1e09cd7d66b4 562 #endif
nerit 0:1e09cd7d66b4 563 #if defined(M2)
nerit 0:1e09cd7d66b4 564 angoloPh = (double) rxMsg.data[1] ;
nerit 0:1e09cd7d66b4 565 aggiornaParametri();
nerit 0:1e09cd7d66b4 566 #endif
nerit 0:1e09cd7d66b4 567 #if defined(M3)
nerit 0:1e09cd7d66b4 568 angoloPh = (double) rxMsg.data[2] ;
nerit 0:1e09cd7d66b4 569 aggiornaParametri();
nerit 0:1e09cd7d66b4 570 #endif
nerit 0:1e09cd7d66b4 571 #if defined(M4)
nerit 0:1e09cd7d66b4 572 angoloPh = (double) rxMsg.data[3] ;
nerit 0:1e09cd7d66b4 573 aggiornaParametri();
nerit 0:1e09cd7d66b4 574 #endif
nerit 0:1e09cd7d66b4 575 #if defined(M5)
nerit 0:1e09cd7d66b4 576 angoloPh = (double) rxMsg.data[4] ;
nerit 0:1e09cd7d66b4 577 aggiornaParametri();
nerit 0:1e09cd7d66b4 578 #endif
nerit 0:1e09cd7d66b4 579 #if defined(M6)
nerit 0:1e09cd7d66b4 580 angoloPh = (double) rxMsg.data[5] ;
nerit 0:1e09cd7d66b4 581 aggiornaParametri();
nerit 0:1e09cd7d66b4 582 #endif
nerit 0:1e09cd7d66b4 583 }
nerit 0:1e09cd7d66b4 584 }
nerit 0:1e09cd7d66b4 585 if (rxMsg.id==RX_AngoloAv){
nerit 0:1e09cd7d66b4 586 if (tractorSpeed_MtS_timed==0.0f){
nerit 0:1e09cd7d66b4 587 #if defined(M1)
nerit 0:1e09cd7d66b4 588 angoloAv = (double) rxMsg.data[0] ;
nerit 0:1e09cd7d66b4 589 aggiornaParametri();
nerit 0:1e09cd7d66b4 590 #endif
nerit 0:1e09cd7d66b4 591 #if defined(M2)
nerit 0:1e09cd7d66b4 592 angoloAv = (double) rxMsg.data[1] ;
nerit 0:1e09cd7d66b4 593 aggiornaParametri();
nerit 0:1e09cd7d66b4 594 #endif
nerit 0:1e09cd7d66b4 595 #if defined(M3)
nerit 0:1e09cd7d66b4 596 angoloAv = (double) rxMsg.data[2] ;
nerit 0:1e09cd7d66b4 597 aggiornaParametri();
nerit 0:1e09cd7d66b4 598 #endif
nerit 0:1e09cd7d66b4 599 #if defined(M4)
nerit 0:1e09cd7d66b4 600 angoloAv = (double) rxMsg.data[3] ;
nerit 0:1e09cd7d66b4 601 aggiornaParametri();
nerit 0:1e09cd7d66b4 602 #endif
nerit 0:1e09cd7d66b4 603 #if defined(M5)
nerit 0:1e09cd7d66b4 604 angoloAv = (double) rxMsg.data[4] ;
nerit 0:1e09cd7d66b4 605 aggiornaParametri();
nerit 0:1e09cd7d66b4 606 #endif
nerit 0:1e09cd7d66b4 607 #if defined(M6)
nerit 0:1e09cd7d66b4 608 angoloAv = (double) rxMsg.data[5] ;
nerit 0:1e09cd7d66b4 609 aggiornaParametri();
nerit 0:1e09cd7d66b4 610 #endif
nerit 0:1e09cd7d66b4 611 }
nerit 0:1e09cd7d66b4 612 }
nerit 0:1e09cd7d66b4 613 if (rxMsg.id==RX_Quinconce){
nerit 0:1e09cd7d66b4 614 if (tractorSpeed_MtS_timed==0.0f){
nerit 0:1e09cd7d66b4 615 quinconceActive = (uint8_t) rxMsg.data[0];
nerit 0:1e09cd7d66b4 616 aggiornaParametri();
nerit 0:1e09cd7d66b4 617 }
nerit 0:1e09cd7d66b4 618 }
nerit 0:1e09cd7d66b4 619 if (rxMsg.id==RX_QuincSinc){
nerit 0:1e09cd7d66b4 620 masterSinc = (uint32_t) rxMsg.data[0] * 0x00FF0000;
nerit 0:1e09cd7d66b4 621 masterSinc = masterSinc + ((uint32_t) rxMsg.data[1] * 0x0000FF00);
nerit 0:1e09cd7d66b4 622 masterSinc = masterSinc + ((uint32_t) rxMsg.data[2] * 0x000000FF);
nerit 0:1e09cd7d66b4 623 masterSinc = masterSinc + ((uint32_t) rxMsg.data[3]);
nerit 0:1e09cd7d66b4 624 quincVerify=1;
nerit 0:1e09cd7d66b4 625 //pc.printf("pippo");
nerit 0:1e09cd7d66b4 626 }
nerit 0:1e09cd7d66b4 627 }
nerit 0:1e09cd7d66b4 628 #endif
nerit 0:1e09cd7d66b4 629 #if defined(overWriteCanSimulation)
nerit 0:1e09cd7d66b4 630 enableSimula=1;
nerit 0:1e09cd7d66b4 631 speedSimula=25;
nerit 0:1e09cd7d66b4 632 avviaSimula=1;
nerit 0:1e09cd7d66b4 633 simOk=1;
nerit 0:1e09cd7d66b4 634 #endif
nerit 0:1e09cd7d66b4 635
nerit 0:1e09cd7d66b4 636 }
nerit 0:1e09cd7d66b4 637
nerit 0:1e09cd7d66b4 638 //*******************************************************
nerit 0:1e09cd7d66b4 639 void DC_CheckCurrent(){
nerit 0:1e09cd7d66b4 640
nerit 0:1e09cd7d66b4 641 // TODO: tabella di riferimento assorbimenti alle varie velocità al fine di gestire
nerit 0:1e09cd7d66b4 642 // gli allarmi e le correzioni di velocità
nerit 0:1e09cd7d66b4 643
nerit 0:1e09cd7d66b4 644 //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 645 //int SD_analogIndex[10]={0,0,0,0,0,0,0,0,0,0};
nerit 0:1e09cd7d66b4 646 // Analog reading
nerit 0:1e09cd7d66b4 647 //number = floor(number * 100) / 100;
nerit 0:1e09cd7d66b4 648 //if (pwmCheck==1){
nerit 0:1e09cd7d66b4 649 timeout.detach();
nerit 0:1e09cd7d66b4 650 for (int ii=1;ii<20;ii++){
nerit 0:1e09cd7d66b4 651 SD_analogMatrix[ii]=SD_analogMatrix[ii+1];
nerit 0:1e09cd7d66b4 652 }
nerit 0:1e09cd7d66b4 653 SD_CurrentAnalog = floor(SDcurrent.read()*100)/100; // valore in ingresso compreso tra 0.00 e 1.00
nerit 0:1e09cd7d66b4 654 SD_analogMatrix[20]=SD_CurrentAnalog;
nerit 0:1e09cd7d66b4 655 if (SDmotorPWM==0.0f){
nerit 0:1e09cd7d66b4 656 SD_CurrentStart=SD_CurrentAnalog;
nerit 0:1e09cd7d66b4 657 }
nerit 0:1e09cd7d66b4 658 float sommaTutto=0.0f;
nerit 0:1e09cd7d66b4 659 for (int ii=1;ii<21;ii++){
nerit 0:1e09cd7d66b4 660 sommaTutto=sommaTutto+SD_analogMatrix[ii];
nerit 0:1e09cd7d66b4 661 }
nerit 0:1e09cd7d66b4 662 float SD_CurrentAnalogica=sommaTutto/20.0f;
nerit 0:1e09cd7d66b4 663 SD_CurrentScaled = floor(( (SD_CurrentAnalogica - SD_CurrentStart)*3.3f) / (SD_CurrentFactor/1000.0f)*10)/10;
nerit 0:1e09cd7d66b4 664 #if defined(pcSerial)
nerit 0:1e09cd7d66b4 665 #if defined(correnteAssorbita)
nerit 0:1e09cd7d66b4 666 pc.printf("CorrenteStart: %f ",SD_CurrentStart);
nerit 0:1e09cd7d66b4 667 pc.printf(" CorrenteScaled: %f ",SD_CurrentScaled);
nerit 0:1e09cd7d66b4 668 pc.printf(" CorrenteMedia: %f \n",SD_CurrentAnalogica);
nerit 0:1e09cd7d66b4 669 #endif
nerit 0:1e09cd7d66b4 670 #endif
nerit 0:1e09cd7d66b4 671 reduceCurrent=false;
nerit 0:1e09cd7d66b4 672 incrementCurrent=false;
nerit 0:1e09cd7d66b4 673 /*
nerit 0:1e09cd7d66b4 674 if (SD_CurrentScaled < 3.0f){
nerit 0:1e09cd7d66b4 675 #if defined(canbusActive)
nerit 0:1e09cd7d66b4 676 all_lowBattery=1;
nerit 0:1e09cd7d66b4 677 #endif
nerit 0:1e09cd7d66b4 678 incrementCurrent=true;
nerit 0:1e09cd7d66b4 679 }
nerit 0:1e09cd7d66b4 680 */
nerit 0:1e09cd7d66b4 681 if (SD_CurrentScaled > 13.0f){
nerit 0:1e09cd7d66b4 682 #if defined(canbusActive)
nerit 0:1e09cd7d66b4 683 all_overCurrDC=1;
nerit 0:1e09cd7d66b4 684 #endif
nerit 0:1e09cd7d66b4 685 reduceCurrent=true;
nerit 0:1e09cd7d66b4 686 }
nerit 0:1e09cd7d66b4 687 //}
nerit 0:1e09cd7d66b4 688 }
nerit 0:1e09cd7d66b4 689 //*******************************************************
nerit 0:1e09cd7d66b4 690 void DC_prepare(){
nerit 0:1e09cd7d66b4 691 // direction or brake preparation
nerit 0:1e09cd7d66b4 692 if (DC_brake==1){SDmotorInA=1;SDmotorInB=1;}else{if (DC_forward==1){SDmotorInA=1;SDmotorInB=0;}else{SDmotorInA=0;SDmotorInB=1;}}
nerit 0:1e09cd7d66b4 693 // fault reading
nerit 0:1e09cd7d66b4 694 if (SDmotorInA==1){SD_faultA=1;}else{SD_faultA=0;}
nerit 0:1e09cd7d66b4 695 if (SDmotorInB==1){SD_faultB=1;}else{SD_faultB=0;}
nerit 0:1e09cd7d66b4 696 }
nerit 0:1e09cd7d66b4 697 //*******************************************************
nerit 0:1e09cd7d66b4 698 void startDelay(){
nerit 0:1e09cd7d66b4 699 int ritardo =0;
nerit 0:1e09cd7d66b4 700 ritardo = (int)((float)(dcActualDuty*800.0f));
nerit 0:1e09cd7d66b4 701 timeout.attach_us(&DC_CheckCurrent,ritardo);
nerit 0:1e09cd7d66b4 702 }
nerit 0:1e09cd7d66b4 703 //*******************************************************
nerit 0:1e09cd7d66b4 704 void quincTrigga(){
nerit 0:1e09cd7d66b4 705 if (quinconceIn==1){
nerit 0:1e09cd7d66b4 706 quincClock=true;
nerit 0:1e09cd7d66b4 707 }else{
nerit 0:1e09cd7d66b4 708 quincClock=false;
nerit 0:1e09cd7d66b4 709 }
nerit 0:1e09cd7d66b4 710 }
nerit 0:1e09cd7d66b4 711 //*******************************************************
nerit 0:1e09cd7d66b4 712 void quinCalc(double parametro){
nerit 1:e88bf5011af6 713 //double riferimento=tempoTraBecchi_mS/parametro;
nerit 0:1e09cd7d66b4 714 // riceve l'impulso di sincro dal master e fa partire il timer di verifica dell'errore
nerit 0:1e09cd7d66b4 715 if (quinconceActive==0){
nerit 0:1e09cd7d66b4 716 if ((quincClock==true)&&(oldQuincIn==0)){
nerit 0:1e09cd7d66b4 717 oldQuincIn=1;
nerit 1:e88bf5011af6 718 oldQuincTimeSD = (double) quincTimeSD.read_ms();
nerit 0:1e09cd7d66b4 719 if (trigSlave==false){
nerit 0:1e09cd7d66b4 720 quincInibit=true;
nerit 0:1e09cd7d66b4 721 trigDaMaster=true;
nerit 0:1e09cd7d66b4 722 }else{
nerit 0:1e09cd7d66b4 723 quincInibit=false;
nerit 0:1e09cd7d66b4 724 trigDaMaster=false;
nerit 0:1e09cd7d66b4 725 trigSlave=false;
nerit 0:1e09cd7d66b4 726 }
nerit 0:1e09cd7d66b4 727 if (quincStart==0){
nerit 0:1e09cd7d66b4 728 quincTime.reset();
nerit 0:1e09cd7d66b4 729 quincStart=1;
nerit 0:1e09cd7d66b4 730 }
nerit 0:1e09cd7d66b4 731 }
nerit 0:1e09cd7d66b4 732 if( quincClock==false){
nerit 0:1e09cd7d66b4 733 oldQuincIn=0;
nerit 0:1e09cd7d66b4 734 }
nerit 0:1e09cd7d66b4 735 }else{
nerit 0:1e09cd7d66b4 736 if ((quincClock==false)&&(oldQuincIn==0)){
nerit 0:1e09cd7d66b4 737 oldQuincIn=1;
nerit 1:e88bf5011af6 738 oldQuincTimeSD = (double) quincTimeSD.read_ms();
nerit 0:1e09cd7d66b4 739 if (trigSlave==false){
nerit 0:1e09cd7d66b4 740 quincInibit=true;
nerit 0:1e09cd7d66b4 741 trigDaMaster=true;
nerit 0:1e09cd7d66b4 742 }else{
nerit 0:1e09cd7d66b4 743 quincInibit=false;
nerit 0:1e09cd7d66b4 744 trigDaMaster=false;
nerit 0:1e09cd7d66b4 745 trigSlave=false;
nerit 0:1e09cd7d66b4 746 }
nerit 0:1e09cd7d66b4 747 if (quincStart==0){
nerit 0:1e09cd7d66b4 748 quincTime.reset();
nerit 0:1e09cd7d66b4 749 quincStart=1;
nerit 0:1e09cd7d66b4 750 }
nerit 0:1e09cd7d66b4 751 }
nerit 0:1e09cd7d66b4 752 if( quincClock==true){
nerit 0:1e09cd7d66b4 753 oldQuincIn=0;
nerit 0:1e09cd7d66b4 754 }
nerit 0:1e09cd7d66b4 755 }
nerit 0:1e09cd7d66b4 756 // riceve l'impulso di passaggio del becco locale e determina il valore dell'errore
nerit 0:1e09cd7d66b4 757 // controllo di linea e di quinconcio
nerit 0:1e09cd7d66b4 758 if ((quinconceActive==0)||(quinconceActive==1)){
nerit 0:1e09cd7d66b4 759 if ((quincStart==1)&&(SDzeroDebounced==1)){
nerit 1:e88bf5011af6 760 if (quincMemoEnable==false){
nerit 0:1e09cd7d66b4 761 memoDuty=correzione;
nerit 1:e88bf5011af6 762 oldCorrezione=correzione;
nerit 1:e88bf5011af6 763 //quincMemoEnable=true;
nerit 1:e88bf5011af6 764 }
nerit 1:e88bf5011af6 765 quincStart=0;
nerit 1:e88bf5011af6 766 percento=1.0f;
nerit 1:e88bf5011af6 767 scostamento = (double)quincTime.read_ms();
nerit 1:e88bf5011af6 768 scostamento2 = oldQuincTimeSD;
nerit 1:e88bf5011af6 769 /*if ((scostamento <= scostamento2)&&(scostamento>10.0f)&&(scostamento <200.0f)){
nerit 1:e88bf5011af6 770 quincError = scostamento;
nerit 1:e88bf5011af6 771 percento = 1.0f + (abs(quincError)/100.0f);
nerit 0:1e09cd7d66b4 772 }
nerit 1:e88bf5011af6 773 if ((scostamento > scostamento2)&&(scostamento2>10.0f)&&(scostamento2 <200.0f)){
nerit 1:e88bf5011af6 774 quincError = scostamento2;
nerit 1:e88bf5011af6 775 percento = 1.0f - (abs(quincError)/100.0f);
nerit 1:e88bf5011af6 776 }*/
nerit 1:e88bf5011af6 777 double pippo = (tractorSpeed_MtS_timed/1.25f)*0.05f;
nerit 1:e88bf5011af6 778 double halfPippo= tempoTraBecchi_mS/2.0f;
nerit 1:e88bf5011af6 779 if ((scostamento > 5.0f)&&(scostamento<halfPippo)){
nerit 1:e88bf5011af6 780 percento = 1.0f-pippo;
nerit 1:e88bf5011af6 781 }
nerit 1:e88bf5011af6 782 if ((scostamento2 > 5.0f)&&(scostamento2<halfPippo)){
nerit 1:e88bf5011af6 783 percento = 1.0f+pippo;
nerit 1:e88bf5011af6 784 }
nerit 1:e88bf5011af6 785 if (abs(percento) > 1.15f){percento=1.15f;}
nerit 1:e88bf5011af6 786 if (abs(percento) < 0.80f){percento=0.80f;}
nerit 1:e88bf5011af6 787 #if defined(pcSerial)
nerit 1:e88bf5011af6 788 #if defined(Qnc)
nerit 1:e88bf5011af6 789 pc.printf(" s1: %f", scostamento);
nerit 1:e88bf5011af6 790 pc.printf(" s2: %f", scostamento2);
nerit 1:e88bf5011af6 791 pc.printf(" er: %f", quincError);
nerit 1:e88bf5011af6 792 pc.printf(" pc: %f", percento);
nerit 1:e88bf5011af6 793 pc.printf(" cr: %f\n",correzione);
nerit 1:e88bf5011af6 794 #endif
nerit 1:e88bf5011af6 795 #endif
nerit 0:1e09cd7d66b4 796 }
nerit 1:e88bf5011af6 797 /*if ((abs(percento) != 1.0f)&&(abs(percento) > 0.0f)){
nerit 0:1e09cd7d66b4 798 correzione = memoDuty* abs(percento);
nerit 0:1e09cd7d66b4 799 correggiSD=true;
nerit 1:e88bf5011af6 800 cntQuinc=0;
nerit 0:1e09cd7d66b4 801 }else{
nerit 0:1e09cd7d66b4 802 correzione = memoDuty;
nerit 0:1e09cd7d66b4 803 correggiSD=false;
nerit 1:e88bf5011af6 804 if (cntQuinc>=2){
nerit 1:e88bf5011af6 805 quincMemoEnable=false;
nerit 1:e88bf5011af6 806 }
nerit 1:e88bf5011af6 807 }*/
nerit 0:1e09cd7d66b4 808 }
nerit 0:1e09cd7d66b4 809 /*
nerit 0:1e09cd7d66b4 810 // gestione con confronto encoder
nerit 0:1e09cd7d66b4 811 // serve l'encoder fisico sugli elementi slave perchè altrimenti la ruota cambia velocità ma non cambia
nerit 0:1e09cd7d66b4 812 // il clock dell'encoder virtuale e quindi l'errore non può essere rilevato correttamente
nerit 0:1e09cd7d66b4 813 if ((quinconceActive==0)&&(quincEnable==1)){
nerit 0:1e09cd7d66b4 814 // da encoder master
nerit 0:1e09cd7d66b4 815 if (quincVerify==1){
nerit 0:1e09cd7d66b4 816 quincVerify=0;
nerit 0:1e09cd7d66b4 817 quincStart=0;
nerit 0:1e09cd7d66b4 818 memoDuty=dcActualDuty;
nerit 0:1e09cd7d66b4 819 if (quincInibit==false){
nerit 0:1e09cd7d66b4 820 percento=0.0f;
nerit 0:1e09cd7d66b4 821 quincError = 0.0f;
nerit 0:1e09cd7d66b4 822 if (masterSinc > prePosSD){
nerit 0:1e09cd7d66b4 823 quincError = masterSinc - prePosSD;
nerit 0:1e09cd7d66b4 824 percento = 1.0f + ((abs(quincError)/SDsectorStep));
nerit 0:1e09cd7d66b4 825 }
nerit 0:1e09cd7d66b4 826 if (masterSinc < prePosSD){
nerit 0:1e09cd7d66b4 827 quincError = prePosSD -masterSinc;
nerit 0:1e09cd7d66b4 828 percento = 1.0f - ((abs(quincError)/SDsectorStep));
nerit 0:1e09cd7d66b4 829 }
nerit 0:1e09cd7d66b4 830 // se quinconce richiesto deve andare al valore di riferimento
nerit 0:1e09cd7d66b4 831 if (abs(percento) > 1.55f){percento=1.55f;}
nerit 0:1e09cd7d66b4 832 if (abs(percento) < 0.40f){percento=0.40f;}
nerit 0:1e09cd7d66b4 833 if (abs(percento) >0.0f){
nerit 0:1e09cd7d66b4 834 if (abs(percento)>1.0f){
nerit 0:1e09cd7d66b4 835 correzione=correzione+(abs(percento)*0.01f);
nerit 0:1e09cd7d66b4 836 }else{
nerit 0:1e09cd7d66b4 837 correzione=correzione-(abs(percento)*0.01f);
nerit 0:1e09cd7d66b4 838 }
nerit 0:1e09cd7d66b4 839 //dcActualDuty = memoDuty * abs(percento);
nerit 0:1e09cd7d66b4 840 }else{
nerit 0:1e09cd7d66b4 841 correzione=correzione;
nerit 0:1e09cd7d66b4 842 //dcActualDuty = memoDuty;
nerit 0:1e09cd7d66b4 843 }
nerit 0:1e09cd7d66b4 844 }
nerit 0:1e09cd7d66b4 845 #if defined(pcSerial)
nerit 0:1e09cd7d66b4 846 #if defined(Qnc)
nerit 0:1e09cd7d66b4 847 pc.printf(" trigMaster: %d", trigDaMaster);
nerit 0:1e09cd7d66b4 848 pc.printf(" trigSlave: %d", trigSlave);
nerit 0:1e09cd7d66b4 849 pc.printf(" quincinibit: %d", quincInibit);
nerit 0:1e09cd7d66b4 850 pc.printf(" masterSinc: %d", masterSinc);
nerit 0:1e09cd7d66b4 851 pc.printf(" prePosSD: %d",prePosSD);
nerit 0:1e09cd7d66b4 852 pc.printf(" percento: %f",percento);
nerit 0:1e09cd7d66b4 853 pc.printf(" correzione: %f\n",correzione);
nerit 0:1e09cd7d66b4 854 #endif
nerit 0:1e09cd7d66b4 855 #endif
nerit 0:1e09cd7d66b4 856 }
nerit 0:1e09cd7d66b4 857 }
nerit 0:1e09cd7d66b4 858 */
nerit 0:1e09cd7d66b4 859 }
nerit 0:1e09cd7d66b4 860 // ------------------------------------------------------------------------------------------------------------------------------------------------------------------
nerit 0:1e09cd7d66b4 861 // MAIN SECTION
nerit 0:1e09cd7d66b4 862 // ------------------------------------------------------------------------------------------------------------------------------------------------------------------
nerit 0:1e09cd7d66b4 863
nerit 0:1e09cd7d66b4 864 int main()
nerit 0:1e09cd7d66b4 865 {
nerit 0:1e09cd7d66b4 866 wd.Configure(2); //watchdog set at xx seconds
nerit 0:1e09cd7d66b4 867
nerit 0:1e09cd7d66b4 868 for (int a=0; a<5;a++){
nerit 0:1e09cd7d66b4 869 mediaSpeed[a]=0;
nerit 0:1e09cd7d66b4 870 }
nerit 0:1e09cd7d66b4 871
nerit 0:1e09cd7d66b4 872 // Stepper driver init and set
nerit 0:1e09cd7d66b4 873 TBmotorRst=0; // reset stepper driver
nerit 0:1e09cd7d66b4 874 TBmotorDirecti=0; // reset stepper direction
nerit 0:1e09cd7d66b4 875 // M1 M2 M3 RESOLUTION
nerit 0:1e09cd7d66b4 876 // 0 0 0 1/2
nerit 0:1e09cd7d66b4 877 // 1 0 0 1/8
nerit 0:1e09cd7d66b4 878 // 0 1 0 1/16
nerit 0:1e09cd7d66b4 879 // 1 1 0 1/32
nerit 0:1e09cd7d66b4 880 // 0 0 1 1/64
nerit 0:1e09cd7d66b4 881 // 1 0 1 1/128
nerit 0:1e09cd7d66b4 882 // 0 1 1 1/10
nerit 0:1e09cd7d66b4 883 // 1 1 1 1/20
nerit 0:1e09cd7d66b4 884 if (TBmotorSteps==400){
nerit 0:1e09cd7d66b4 885 TBmotor_M1=0;
nerit 0:1e09cd7d66b4 886 TBmotor_M2=0;
nerit 0:1e09cd7d66b4 887 TBmotor_M3=0;
nerit 0:1e09cd7d66b4 888 }else if (TBmotorSteps==1600){
nerit 0:1e09cd7d66b4 889 TBmotor_M1=1;
nerit 0:1e09cd7d66b4 890 TBmotor_M2=0;
nerit 0:1e09cd7d66b4 891 TBmotor_M3=0;
nerit 0:1e09cd7d66b4 892 }else if (TBmotorSteps==3200){
nerit 0:1e09cd7d66b4 893 TBmotor_M1=0;
nerit 0:1e09cd7d66b4 894 TBmotor_M2=1;
nerit 0:1e09cd7d66b4 895 TBmotor_M3=0;
nerit 0:1e09cd7d66b4 896 }else if (TBmotorSteps==6400){
nerit 0:1e09cd7d66b4 897 TBmotor_M1=1;
nerit 0:1e09cd7d66b4 898 TBmotor_M2=1;
nerit 0:1e09cd7d66b4 899 TBmotor_M3=0;
nerit 0:1e09cd7d66b4 900 }else if (TBmotorSteps==12800){
nerit 0:1e09cd7d66b4 901 TBmotor_M1=0;
nerit 0:1e09cd7d66b4 902 TBmotor_M2=0;
nerit 0:1e09cd7d66b4 903 TBmotor_M3=1;
nerit 0:1e09cd7d66b4 904 }else if (TBmotorSteps==25600){
nerit 0:1e09cd7d66b4 905 TBmotor_M1=1;
nerit 0:1e09cd7d66b4 906 TBmotor_M2=0;
nerit 0:1e09cd7d66b4 907 TBmotor_M3=1;
nerit 0:1e09cd7d66b4 908 }else if (TBmotorSteps==2000){
nerit 0:1e09cd7d66b4 909 TBmotor_M1=0;
nerit 0:1e09cd7d66b4 910 TBmotor_M2=1;
nerit 0:1e09cd7d66b4 911 TBmotor_M3=1;
nerit 0:1e09cd7d66b4 912 }else if (TBmotorSteps==4000){
nerit 0:1e09cd7d66b4 913 TBmotor_M1=1;
nerit 0:1e09cd7d66b4 914 TBmotor_M2=1;
nerit 0:1e09cd7d66b4 915 TBmotor_M3=1;
nerit 0:1e09cd7d66b4 916 }
nerit 0:1e09cd7d66b4 917 TBmotorRst=1;
nerit 0:1e09cd7d66b4 918
nerit 0:1e09cd7d66b4 919 // DC reset ad set
nerit 0:1e09cd7d66b4 920 int decima = 100;
nerit 0:1e09cd7d66b4 921 wait_ms(200);
nerit 0:1e09cd7d66b4 922 SD_CurrentStart=floor(SDcurrent.read()*decima)/decima;
nerit 0:1e09cd7d66b4 923 wait_ms(2);
nerit 0:1e09cd7d66b4 924 SD_CurrentStart+=floor(SDcurrent.read()*decima)/decima;
nerit 0:1e09cd7d66b4 925 wait_ms(1);
nerit 0:1e09cd7d66b4 926 SD_CurrentStart+=floor(SDcurrent.read()*decima)/decima;
nerit 0:1e09cd7d66b4 927 wait_ms(3);
nerit 0:1e09cd7d66b4 928 SD_CurrentStart+=floor(SDcurrent.read()*decima)/decima;
nerit 0:1e09cd7d66b4 929 SD_CurrentStart=(floor((SD_CurrentStart/4.0f)*decima)/decima)-0.01f;
nerit 0:1e09cd7d66b4 930 wait_ms(100);
nerit 0:1e09cd7d66b4 931 DC_prepare();
nerit 0:1e09cd7d66b4 932
nerit 0:1e09cd7d66b4 933 speedTimer.start(); // speed pulse timer
nerit 0:1e09cd7d66b4 934 intraPickTimer.start();
nerit 0:1e09cd7d66b4 935 speedTimeOut.start();
nerit 0:1e09cd7d66b4 936 speedFilter.start();
nerit 0:1e09cd7d66b4 937 seedFilter.start();
nerit 0:1e09cd7d66b4 938 TBfilter.start();
nerit 0:1e09cd7d66b4 939 sincroTimer.start();
nerit 0:1e09cd7d66b4 940 rotationTimeOut.start();
nerit 0:1e09cd7d66b4 941 metalTimer.start();
nerit 0:1e09cd7d66b4 942 quincTime.start();
nerit 1:e88bf5011af6 943 quincTimeSD.start();
nerit 0:1e09cd7d66b4 944
nerit 0:1e09cd7d66b4 945 //*******************************************************
nerit 0:1e09cd7d66b4 946 // controls for check DC motor current
nerit 0:1e09cd7d66b4 947 //currentCheck.attach(&DC_CheckCurrent, 0.10f);
nerit 0:1e09cd7d66b4 948 pwmCheck.rise(&startDelay);
nerit 0:1e09cd7d66b4 949 //tbCorrection.attach(&correction,0.1f;
nerit 0:1e09cd7d66b4 950
nerit 0:1e09cd7d66b4 951 if (inProva==0){
nerit 0:1e09cd7d66b4 952 tractorSpeedRead.rise(&tractorReadSpeed);
nerit 0:1e09cd7d66b4 953 #if !defined(speedMaster)
nerit 0:1e09cd7d66b4 954 quinconceIn.rise(&quincTrigga);
nerit 0:1e09cd7d66b4 955 quinconceIn.fall(&quincTrigga);
nerit 0:1e09cd7d66b4 956 #endif
nerit 0:1e09cd7d66b4 957 tftUpdate.attach(&videoUpdate,0.50f);
nerit 0:1e09cd7d66b4 958 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 0:1e09cd7d66b4 959 if (speedFromPick==0){
nerit 0:1e09cd7d66b4 960 ElementPosition.rise(&sd25Fall);
nerit 0:1e09cd7d66b4 961 }
nerit 0:1e09cd7d66b4 962 }else{
nerit 0:1e09cd7d66b4 963 tftUpdate.attach(&videoUpdate,0.125f);
nerit 0:1e09cd7d66b4 964 }
nerit 0:1e09cd7d66b4 965
nerit 0:1e09cd7d66b4 966 aggiornaParametri();
nerit 0:1e09cd7d66b4 967
nerit 0:1e09cd7d66b4 968 SDmotorPWM.period_us(periodoSD); // frequency 1KHz pilotaggio motore DC
nerit 0:1e09cd7d66b4 969 SDmotorPWM.write(0.0f); // duty cycle = stop
nerit 0:1e09cd7d66b4 970 TBmotorDirecti=0; // tb motor direction set
nerit 0:1e09cd7d66b4 971
nerit 0:1e09cd7d66b4 972 #if defined(provaStepper)
nerit 0:1e09cd7d66b4 973 TBmotorRst=1;
nerit 0:1e09cd7d66b4 974 TBticker.attach_us(&step_TBPulseOut,3000.0f); // clock time are seconds and attach seed motor stepper controls
nerit 0:1e09cd7d66b4 975 #endif // end prova stepper
nerit 0:1e09cd7d66b4 976
nerit 0:1e09cd7d66b4 977 #if defined(canbusActive)
nerit 0:1e09cd7d66b4 978 can1.attach(&leggiCAN, CAN::RxIrq);
nerit 0:1e09cd7d66b4 979 #endif
nerit 0:1e09cd7d66b4 980
nerit 0:1e09cd7d66b4 981 //**************************************************************************************************************
nerit 0:1e09cd7d66b4 982 // MAIN LOOP
nerit 0:1e09cd7d66b4 983 //**************************************************************************************************************
nerit 0:1e09cd7d66b4 984 while (true){
nerit 0:1e09cd7d66b4 985 // ripetitore segnale di velocità
nerit 0:1e09cd7d66b4 986 if (tractorSpeedRead==0){speedClock=0;}
nerit 0:1e09cd7d66b4 987
nerit 0:1e09cd7d66b4 988 // inversione segnali ingressi
nerit 0:1e09cd7d66b4 989 TBzeroPinInput = !TBzeroPinInputRev;
nerit 0:1e09cd7d66b4 990 seedWheelZeroPinInput = !seedWheelZeroPinInputRev;
nerit 0:1e09cd7d66b4 991
nerit 0:1e09cd7d66b4 992 // se quinconce attivo ed unita' master invia segnale di sincro
nerit 0:1e09cd7d66b4 993 #if defined(speedMaster)
nerit 0:1e09cd7d66b4 994 if (seedWheelZeroPinInput==1){
nerit 0:1e09cd7d66b4 995 quinconceOut=1;
nerit 0:1e09cd7d66b4 996 }
nerit 0:1e09cd7d66b4 997 if ((double)SDactualPosition >= (SDsectorStep/2.0f)){
nerit 0:1e09cd7d66b4 998 quinconceOut=0;
nerit 0:1e09cd7d66b4 999 }
nerit 0:1e09cd7d66b4 1000 #endif
nerit 0:1e09cd7d66b4 1001
nerit 0:1e09cd7d66b4 1002 #if defined(overWriteCanSimulation)
nerit 0:1e09cd7d66b4 1003 leggiCAN();
nerit 0:1e09cd7d66b4 1004 #endif
nerit 0:1e09cd7d66b4 1005
nerit 0:1e09cd7d66b4 1006 // simulazione velocita
nerit 0:1e09cd7d66b4 1007 if (enableSimula==1){
nerit 0:1e09cd7d66b4 1008 double TMT = 0.0f;
nerit 0:1e09cd7d66b4 1009 if (speedSimula > 0){
nerit 0:1e09cd7d66b4 1010 TMT = (double)(speedSimula) * 100.0f /3600.0f;
nerit 0:1e09cd7d66b4 1011 pulseSpeedInterval = pulseDistance / TMT;
nerit 0:1e09cd7d66b4 1012 }else{
nerit 0:1e09cd7d66b4 1013 pulseSpeedInterval = 10000.0f;
nerit 0:1e09cd7d66b4 1014 }
nerit 0:1e09cd7d66b4 1015 if (avviaSimula==1){
nerit 0:1e09cd7d66b4 1016 if(oldSimulaSpeed!=pulseSpeedInterval){
nerit 0:1e09cd7d66b4 1017 spedSimclock.attach_us(&speedSimulationClock,pulseSpeedInterval);
nerit 0:1e09cd7d66b4 1018 oldSimulaSpeed=pulseSpeedInterval;
nerit 0:1e09cd7d66b4 1019 }
nerit 0:1e09cd7d66b4 1020 }else{
nerit 0:1e09cd7d66b4 1021 oldSimulaSpeed=10000.0f;
nerit 0:1e09cd7d66b4 1022 spedSimclock.detach();
nerit 0:1e09cd7d66b4 1023 }
nerit 0:1e09cd7d66b4 1024 }else{
nerit 0:1e09cd7d66b4 1025 spedSimclock.detach();
nerit 0:1e09cd7d66b4 1026 }
nerit 0:1e09cd7d66b4 1027
nerit 0:1e09cd7d66b4 1028 //*******************************************************
nerit 0:1e09cd7d66b4 1029 // determina se sono in bassa velocità per il controllo di TB
nerit 0:1e09cd7d66b4 1030 if (speedOfSeedWheel<=minSeedSpeed){
nerit 0:1e09cd7d66b4 1031 if (lowSpeedRequired==0){
nerit 0:1e09cd7d66b4 1032 ritardaLowSpeed.reset();
nerit 0:1e09cd7d66b4 1033 ritardaLowSpeed.start();
nerit 0:1e09cd7d66b4 1034 }
nerit 0:1e09cd7d66b4 1035 lowSpeedRequired=1;
nerit 0:1e09cd7d66b4 1036 }else{
nerit 0:1e09cd7d66b4 1037 if (lowSpeedRequired==1){
nerit 0:1e09cd7d66b4 1038 lowSpeedRequired=0;
nerit 0:1e09cd7d66b4 1039 ritardaLowSpeed.reset();
nerit 0:1e09cd7d66b4 1040 ritardaLowSpeed.stop();
nerit 0:1e09cd7d66b4 1041 }
nerit 0:1e09cd7d66b4 1042 }
nerit 0:1e09cd7d66b4 1043
nerit 0:1e09cd7d66b4 1044 if (ritardaLowSpeed.read_ms()> 2000){
nerit 0:1e09cd7d66b4 1045 lowSpeed=1;
nerit 0:1e09cd7d66b4 1046 }else{
nerit 0:1e09cd7d66b4 1047 lowSpeed=0;
nerit 0:1e09cd7d66b4 1048 }
nerit 0:1e09cd7d66b4 1049
nerit 0:1e09cd7d66b4 1050
nerit 0:1e09cd7d66b4 1051 //**************************************************************************************************************
nerit 0:1e09cd7d66b4 1052 //**************************************************************************************************************
nerit 0:1e09cd7d66b4 1053 // LOGICAL CONTROLS
nerit 0:1e09cd7d66b4 1054 //**************************************************************************************************************
nerit 0:1e09cd7d66b4 1055 //**************************************************************************************************************
nerit 0:1e09cd7d66b4 1056
nerit 0:1e09cd7d66b4 1057 if (inProva==0){
nerit 0:1e09cd7d66b4 1058 if ((startCycleSimulation==0)&&(enableSimula==0)){zeroRequestBuf=1;runRequestBuf=0;enableCycle=1;
nerit 0:1e09cd7d66b4 1059 }else{zeroRequestBuf=1;runRequestBuf=0;enableCycle=1;}
nerit 0:1e09cd7d66b4 1060 if ((tractorSpeedRead==0)&&(speedFilter.read_ms()>3)){oldTractorSpeedRead=0;}
nerit 0:1e09cd7d66b4 1061 // ----------------------------------------
nerit 0:1e09cd7d66b4 1062 // filtra il segnale dei becchi per misurare il tempo di intervallo tra loro
nerit 0:1e09cd7d66b4 1063 // ----------------------------------------
nerit 0:1e09cd7d66b4 1064 if (seedWheelZeroPinInput==0){
nerit 0:1e09cd7d66b4 1065 //trigSlave=false;
nerit 0:1e09cd7d66b4 1066 if(seedFilter.read_ms()>=4){
nerit 0:1e09cd7d66b4 1067 oldSeedWheelZeroPinInput=0;
nerit 0:1e09cd7d66b4 1068 SDzeroDebounced=0;
nerit 0:1e09cd7d66b4 1069 }
nerit 0:1e09cd7d66b4 1070 }
nerit 0:1e09cd7d66b4 1071 if ((seedWheelZeroPinInput==1)&&(oldSeedWheelZeroPinInput==0)){
nerit 0:1e09cd7d66b4 1072 timeIntraPick = (double)intraPickTimer.read_ms();
nerit 0:1e09cd7d66b4 1073 prePosSD=500; // preposizionamento SD
nerit 0:1e09cd7d66b4 1074 intraPickTimer.reset();
nerit 0:1e09cd7d66b4 1075 rotationTimeOut.reset();
nerit 0:1e09cd7d66b4 1076 seedFilter.reset();
nerit 0:1e09cd7d66b4 1077 sincroTimer.reset();
nerit 0:1e09cd7d66b4 1078 oldSeedWheelZeroPinInput=1;
nerit 1:e88bf5011af6 1079 quincTimeSD.reset();
nerit 0:1e09cd7d66b4 1080 SDzeroDebounced=1;
nerit 0:1e09cd7d66b4 1081 if (trigDaMaster==false){
nerit 0:1e09cd7d66b4 1082 quincInibit=true;
nerit 0:1e09cd7d66b4 1083 trigSlave=true;
nerit 0:1e09cd7d66b4 1084 }else{
nerit 0:1e09cd7d66b4 1085 quincInibit=false;
nerit 0:1e09cd7d66b4 1086 trigSlave=false;
nerit 0:1e09cd7d66b4 1087 trigDaMaster=false;
nerit 0:1e09cd7d66b4 1088 }
nerit 0:1e09cd7d66b4 1089 SDwheelTimer.reset();
nerit 0:1e09cd7d66b4 1090 if (quincCnt<10){
nerit 0:1e09cd7d66b4 1091 quincCnt++;
nerit 0:1e09cd7d66b4 1092 }
nerit 1:e88bf5011af6 1093 if (quincCnt >9){
nerit 0:1e09cd7d66b4 1094 quincEnable=1;
nerit 0:1e09cd7d66b4 1095 }else{
nerit 0:1e09cd7d66b4 1096 quincEnable=0;
nerit 0:1e09cd7d66b4 1097 }
nerit 1:e88bf5011af6 1098 if (cntQuinc<10){
nerit 1:e88bf5011af6 1099 cntQuinc++;
nerit 1:e88bf5011af6 1100 }
nerit 0:1e09cd7d66b4 1101 if ((aspettaStart==0)&&(lowSpeed==1)){
nerit 0:1e09cd7d66b4 1102 beccoPronto=1;
nerit 0:1e09cd7d66b4 1103 }
nerit 0:1e09cd7d66b4 1104 SDzeroCyclePulse=1;
nerit 0:1e09cd7d66b4 1105 lockStart=0;
nerit 0:1e09cd7d66b4 1106 double fase1=0.0f;
nerit 0:1e09cd7d66b4 1107 forzaFase=0;
nerit 0:1e09cd7d66b4 1108 double limite=fixedStepGiroSD/pickNumber;
nerit 0:1e09cd7d66b4 1109 if (pickCounterLow< 0xFF){
nerit 0:1e09cd7d66b4 1110 pickCounterLow++;
nerit 0:1e09cd7d66b4 1111 }else{
nerit 0:1e09cd7d66b4 1112 pickCounterHig++;
nerit 0:1e09cd7d66b4 1113 pickCounterLow=0;
nerit 0:1e09cd7d66b4 1114 }
nerit 0:1e09cd7d66b4 1115 if (tamburoStandard==0){
nerit 0:1e09cd7d66b4 1116 fase1=TBdeltaStep;
nerit 0:1e09cd7d66b4 1117 }else{
nerit 0:1e09cd7d66b4 1118 if(speedForCorrection >= speedOfSeedWheel){
nerit 0:1e09cd7d66b4 1119 fase1=TBdeltaStep;
nerit 0:1e09cd7d66b4 1120 }else{
nerit 0:1e09cd7d66b4 1121 //fase1=(TBdeltaStep)-(((speedOfSeedWheel-speedForCorrection)/1.25f)*(TBfaseStep));
nerit 0:1e09cd7d66b4 1122 fase1=(TBdeltaStep)-(((speedOfSeedWheel)/1.25f)*(TBfaseStep));
nerit 0:1e09cd7d66b4 1123 }
nerit 0:1e09cd7d66b4 1124 if (fase1 > limite){
nerit 0:1e09cd7d66b4 1125 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 1126 //forzaFase=1;
nerit 0:1e09cd7d66b4 1127 }
nerit 0:1e09cd7d66b4 1128 if ((fase1 > (limite -20.0f))&&(fase1<(limite +5.0f))){
nerit 0:1e09cd7d66b4 1129 fase1 = limite -20.0f; // se la fase è molto vicina al limite interpone uno spazio minimo di lavoro del sincronismo
nerit 0:1e09cd7d66b4 1130 forzaFase=1;
nerit 0:1e09cd7d66b4 1131 }
nerit 0:1e09cd7d66b4 1132 trigRepos=1;
nerit 0:1e09cd7d66b4 1133 }
nerit 0:1e09cd7d66b4 1134 fase = (uint32_t)fase1+500;
nerit 0:1e09cd7d66b4 1135 #if defined(pcSerial)
nerit 0:1e09cd7d66b4 1136 #if defined(inCorre)
nerit 0:1e09cd7d66b4 1137 pc.printf(" limite %f", limite);
nerit 0:1e09cd7d66b4 1138 pc.printf(" delta %f", TBdeltaStep);
nerit 0:1e09cd7d66b4 1139 pc.printf(" faseStep %f", TBfaseStep);
nerit 0:1e09cd7d66b4 1140 pc.printf(" fase %d",fase);
nerit 0:1e09cd7d66b4 1141 pc.printf(" forzaFase %d",forzaFase);
nerit 0:1e09cd7d66b4 1142 pc.printf(" trigRepos %d", trigRepos);
nerit 0:1e09cd7d66b4 1143 pc.printf(" ActualSD: %d",SDactualPosition);
nerit 0:1e09cd7d66b4 1144 pc.printf(" SpeedWheel: %f",speedOfSeedWheel);
nerit 0:1e09cd7d66b4 1145 pc.printf(" SPEED: %f \n",tractorSpeed_MtS_timed);
nerit 0:1e09cd7d66b4 1146 #endif
nerit 0:1e09cd7d66b4 1147 #endif
nerit 0:1e09cd7d66b4 1148 if (timeIntraPick >= (memoIntraPick*2)){
nerit 0:1e09cd7d66b4 1149 if ((aspettaStart==0)&&(zeroRequestBuf==0)){
nerit 0:1e09cd7d66b4 1150 if (firstStart==0){
nerit 0:1e09cd7d66b4 1151 all_pickSignal=1;
nerit 0:1e09cd7d66b4 1152 }
nerit 0:1e09cd7d66b4 1153 }
nerit 0:1e09cd7d66b4 1154 }
nerit 0:1e09cd7d66b4 1155 memoIntraPick = timeIntraPick;
nerit 0:1e09cd7d66b4 1156 if (speedFromPick==1){
nerit 0:1e09cd7d66b4 1157 speedOfSeedWheel=((seedPerimeter / pickNumber)/timeIntraPick)*1000.0f;
nerit 0:1e09cd7d66b4 1158 }
nerit 0:1e09cd7d66b4 1159 if ((tractorSpeed_MtS_timed==0.0f)&&(zeroCycle==0)&&(zeroCycleEnd==1)){
nerit 0:1e09cd7d66b4 1160 if ((firstStart==0)&&(simOk==0)&&(enableSimula==0)&&(zeroCycleEnd==1)){
nerit 0:1e09cd7d66b4 1161 all_noSpeedSen=1;
nerit 0:1e09cd7d66b4 1162 }
nerit 0:1e09cd7d66b4 1163 #if defined(pcSerial)
nerit 0:1e09cd7d66b4 1164 #if defined(canDataReceived)
nerit 0:1e09cd7d66b4 1165 pc.printf("allarme no speed sensor");
nerit 0:1e09cd7d66b4 1166 #endif
nerit 0:1e09cd7d66b4 1167 #endif
nerit 0:1e09cd7d66b4 1168 }
nerit 0:1e09cd7d66b4 1169 pulseRised2=1;
nerit 0:1e09cd7d66b4 1170 #if defined(speedMaster)
nerit 0:1e09cd7d66b4 1171 double oldLastPr = (double)oldLastPulseRead*1.2f;
nerit 0:1e09cd7d66b4 1172 if((double)speedTimeOut.read_us()> (oldLastPr)){
nerit 0:1e09cd7d66b4 1173 if ((firstStart==0)&&(simOk==0)&&(enableSimula==0)&&(zeroCycleEnd==1)){
nerit 0:1e09cd7d66b4 1174 all_speedError =1;
nerit 0:1e09cd7d66b4 1175 }
nerit 0:1e09cd7d66b4 1176 #if defined(pcSerial)
nerit 0:1e09cd7d66b4 1177 #if defined(canDataReceived)
nerit 0:1e09cd7d66b4 1178 pc.printf("allarme errore speed sensor");
nerit 0:1e09cd7d66b4 1179 #endif
nerit 0:1e09cd7d66b4 1180 #endif
nerit 0:1e09cd7d66b4 1181
nerit 0:1e09cd7d66b4 1182 }
nerit 0:1e09cd7d66b4 1183 #endif
nerit 0:1e09cd7d66b4 1184 //*******************************************
nerit 0:1e09cd7d66b4 1185 // esegue calcolo clock per la generazione della posizione teorica
nerit 0:1e09cd7d66b4 1186 // la realtà in base al segnale di presenza del becco
nerit 0:1e09cd7d66b4 1187 realSpeed = speedOfSeedWheel;
nerit 0:1e09cd7d66b4 1188 realGiroSD = seedPerimeter / realSpeed;
nerit 0:1e09cd7d66b4 1189 tempoBecco = (realGiroSD/360.0f)*16000.0f;
nerit 0:1e09cd7d66b4 1190 frequenzaReale = fixedStepGiroSD/realGiroSD;
nerit 0:1e09cd7d66b4 1191 semiPeriodoReale = (1000000.0f/frequenzaReale);
nerit 0:1e09cd7d66b4 1192 tempoTraBecchi_mS = 0.0f;
nerit 0:1e09cd7d66b4 1193 seedWheelRPM = (speedOfSeedWheel)*K_WheelRPM ; // calcola i giri al minuto della ruota di semina 7.37 31,75
nerit 0:1e09cd7d66b4 1194 rapportoRuote = pickNumber/cellsNumber; // 0.67 calcola il rapporto tra il numero di becchi ed il numero di celle 0.8 1,00
nerit 0:1e09cd7d66b4 1195 TBrpm = seedWheelRPM*rapportoRuote; // 5.896 31,75
nerit 0:1e09cd7d66b4 1196 TBgiroStep = TBmotorSteps*TBreductionRatio;
nerit 0:1e09cd7d66b4 1197 K_TBfrequency = TBgiroStep/60.0f; // 1600 * 1.65625f /60 = 44 44,00
nerit 0:1e09cd7d66b4 1198 TBfrequency = (TBrpm*K_TBfrequency); // 130Hz a 0,29Mts 1397,00 a 1,25mt/s con 15 becchi e 15 celle
nerit 0:1e09cd7d66b4 1199 TBperiod=1000000.0f/TBfrequency; // 715uS
nerit 0:1e09cd7d66b4 1200 }
nerit 0:1e09cd7d66b4 1201 // ----------------------------------------
nerit 0:1e09cd7d66b4 1202 // check SD fase
nerit 0:1e09cd7d66b4 1203 if ((prePosSD >= fase)||(forzaFase==1)){//&&(prePosSD < (fase +30))){
nerit 0:1e09cd7d66b4 1204 forzaFase=0;
nerit 0:1e09cd7d66b4 1205 if (trigRepos==1){
nerit 0:1e09cd7d66b4 1206 SDactualPosition=0;
nerit 0:1e09cd7d66b4 1207 if ((countCicli<30)&&(trigCicli==0)){countCicli++;trigCicli=1;}
nerit 0:1e09cd7d66b4 1208 if(countCicli>=cicliAspettaStart){aspettaStart=0;}
nerit 0:1e09cd7d66b4 1209 if ((lowSpeed==0)&&(aspettaStart==0)&&(lockStart==0)){syncroCheck=1;beccoPronto=0;}
nerit 0:1e09cd7d66b4 1210 if (trigTB==0){
nerit 0:1e09cd7d66b4 1211 inhibit=1;
nerit 0:1e09cd7d66b4 1212 trigSD=1;
nerit 0:1e09cd7d66b4 1213 }else{
nerit 0:1e09cd7d66b4 1214 inhibit=0;
nerit 0:1e09cd7d66b4 1215 trigTB=0;
nerit 0:1e09cd7d66b4 1216 trigSD=0;
nerit 0:1e09cd7d66b4 1217 }
nerit 0:1e09cd7d66b4 1218 trigRepos=0;
nerit 0:1e09cd7d66b4 1219 }
nerit 0:1e09cd7d66b4 1220 }else{
nerit 0:1e09cd7d66b4 1221 trigCicli=0;
nerit 0:1e09cd7d66b4 1222 }
nerit 0:1e09cd7d66b4 1223 // ----------------------------------------
nerit 0:1e09cd7d66b4 1224 // filtra il segnale del tamburo per lo stop in fase del tamburo stesso
nerit 0:1e09cd7d66b4 1225 if (TBzeroPinInput==0){if (TBfilter.read_ms()>=5){oldTBzeroPinInput=0;}}
nerit 0:1e09cd7d66b4 1226 if ((TBzeroPinInput==1)&&(oldTBzeroPinInput==0)){
nerit 0:1e09cd7d66b4 1227 oldTBzeroPinInput=1;
nerit 0:1e09cd7d66b4 1228 if (loadDaCanInCorso==0){
nerit 0:1e09cd7d66b4 1229 stopCicloTB=1;
nerit 0:1e09cd7d66b4 1230 startCicloTB=0;
nerit 0:1e09cd7d66b4 1231 }
nerit 0:1e09cd7d66b4 1232 TBfilter.reset();
nerit 0:1e09cd7d66b4 1233 TBzeroCyclePulse=1;
nerit 0:1e09cd7d66b4 1234 TBactualPosition=0;
nerit 0:1e09cd7d66b4 1235 if (cellsCounterLow < 0xFF){
nerit 0:1e09cd7d66b4 1236 cellsCounterLow++;
nerit 0:1e09cd7d66b4 1237 }else{
nerit 0:1e09cd7d66b4 1238 cellsCounterHig++;
nerit 0:1e09cd7d66b4 1239 cellsCounterLow=0;
nerit 0:1e09cd7d66b4 1240 }
nerit 0:1e09cd7d66b4 1241 if (loadDaCanInCorso==1){
nerit 0:1e09cd7d66b4 1242 cntCellsForLoad++;
nerit 0:1e09cd7d66b4 1243 if (cntCellsForLoad >= 5){
nerit 0:1e09cd7d66b4 1244 stopCicloTB=1;
nerit 0:1e09cd7d66b4 1245 cntCellsForLoad=0;
nerit 0:1e09cd7d66b4 1246 }
nerit 0:1e09cd7d66b4 1247 }else{
nerit 0:1e09cd7d66b4 1248 cntCellsForLoad=0;
nerit 0:1e09cd7d66b4 1249 }
nerit 0:1e09cd7d66b4 1250 if (trigSD==0){
nerit 0:1e09cd7d66b4 1251 inhibit=1;
nerit 0:1e09cd7d66b4 1252 trigTB=1;
nerit 0:1e09cd7d66b4 1253 }else{
nerit 0:1e09cd7d66b4 1254 inhibit=0;
nerit 0:1e09cd7d66b4 1255 trigTB=0;
nerit 0:1e09cd7d66b4 1256 trigSD=0;
nerit 0:1e09cd7d66b4 1257 }
nerit 0:1e09cd7d66b4 1258 // torna indietro per sbloccare il tamburo
nerit 0:1e09cd7d66b4 1259 if ((TBmotorDirecti==1)&&(erroreTamburo==1)){
nerit 0:1e09cd7d66b4 1260 cntCellsForReload++;
nerit 0:1e09cd7d66b4 1261 if (cntCellsForReload > 3){
nerit 0:1e09cd7d66b4 1262 TBmotorDirecti=0;
nerit 0:1e09cd7d66b4 1263 erroreTamburo=0;
nerit 0:1e09cd7d66b4 1264 }
nerit 0:1e09cd7d66b4 1265 }
nerit 0:1e09cd7d66b4 1266 }
nerit 0:1e09cd7d66b4 1267 if ((double)TBactualPosition > ((TBgiroStep/cellsNumber)*1.2f)){
nerit 0:1e09cd7d66b4 1268 if (firstStart==0){
nerit 0:1e09cd7d66b4 1269 all_cellSignal=1;
nerit 0:1e09cd7d66b4 1270 }
nerit 0:1e09cd7d66b4 1271 if (erroreTamburo==0){
nerit 0:1e09cd7d66b4 1272 erroreTamburo=1;
nerit 0:1e09cd7d66b4 1273 TBmotorDirecti=1;
nerit 0:1e09cd7d66b4 1274 cntCellsForReload=0;
nerit 0:1e09cd7d66b4 1275 cntTbError++;
nerit 0:1e09cd7d66b4 1276 }
nerit 0:1e09cd7d66b4 1277 #if defined(pcSerial)
nerit 0:1e09cd7d66b4 1278 #if defined(canDataReceived)
nerit 0:1e09cd7d66b4 1279 pc.printf("allarme error cells");
nerit 0:1e09cd7d66b4 1280 #endif
nerit 0:1e09cd7d66b4 1281 #endif
nerit 0:1e09cd7d66b4 1282 }
nerit 0:1e09cd7d66b4 1283 if (((double)TBactualPosition > ((TBgiroStep/cellsNumber)*3.0f))||(cntTbError>2)){
nerit 0:1e09cd7d66b4 1284 if (firstStart==0){
nerit 0:1e09cd7d66b4 1285 all_noStepRota=1;
nerit 0:1e09cd7d66b4 1286 }
nerit 0:1e09cd7d66b4 1287 cntTbError=0;
nerit 0:1e09cd7d66b4 1288 #if defined(pcSerial)
nerit 0:1e09cd7d66b4 1289 #if defined(canDataReceived)
nerit 0:1e09cd7d66b4 1290 pc.printf("allarme no cells sensor");
nerit 0:1e09cd7d66b4 1291 #endif
nerit 0:1e09cd7d66b4 1292 #endif
nerit 0:1e09cd7d66b4 1293 }
nerit 0:1e09cd7d66b4 1294 // ----------------------------------------
nerit 0:1e09cd7d66b4 1295 // read and manage joystick
nerit 0:1e09cd7d66b4 1296 // WARNING - ENABLE CYCLE IS SOFTWARE ALWAYS ACTIVE
nerit 0:1e09cd7d66b4 1297 if (enableCycle==1){
nerit 0:1e09cd7d66b4 1298 if(runRequestBuf==1){
nerit 0:1e09cd7d66b4 1299 if (OldStartCycle!=runRequestBuf){
nerit 0:1e09cd7d66b4 1300 if((startCycle==0)&&(zeroCycleEnd==1)){
nerit 0:1e09cd7d66b4 1301 startCycle=1;
nerit 0:1e09cd7d66b4 1302 OldStartCycle = runRequestBuf;
nerit 0:1e09cd7d66b4 1303 oldZeroCycle=0;
nerit 0:1e09cd7d66b4 1304 }
nerit 0:1e09cd7d66b4 1305 }
nerit 0:1e09cd7d66b4 1306 }else{
nerit 0:1e09cd7d66b4 1307 startCycle=0;
nerit 0:1e09cd7d66b4 1308 pntMedia=0;
nerit 0:1e09cd7d66b4 1309 }
nerit 0:1e09cd7d66b4 1310 if (azzeraDaCan==1){
nerit 0:1e09cd7d66b4 1311 if (tractorSpeed_MtS_timed==0.0f){
nerit 0:1e09cd7d66b4 1312 zeroRequestBuf=1;
nerit 0:1e09cd7d66b4 1313 oldZeroCycle=0;
nerit 0:1e09cd7d66b4 1314 }
nerit 0:1e09cd7d66b4 1315 azzeraDaCan=0;
nerit 0:1e09cd7d66b4 1316 }
nerit 0:1e09cd7d66b4 1317 if (loadDaCan==1){
nerit 0:1e09cd7d66b4 1318 if (tractorSpeed_MtS_timed==0.0f){
nerit 0:1e09cd7d66b4 1319 ciclaTB();
nerit 0:1e09cd7d66b4 1320 }
nerit 0:1e09cd7d66b4 1321 }
nerit 0:1e09cd7d66b4 1322 if ((zeroRequestBuf==1)){
nerit 0:1e09cd7d66b4 1323 if (oldZeroCycle!=zeroRequestBuf){
nerit 0:1e09cd7d66b4 1324 zeroCycle=1;
nerit 0:1e09cd7d66b4 1325 zeroCycleEnd=0;
nerit 0:1e09cd7d66b4 1326 SDzeroed=0;
nerit 0:1e09cd7d66b4 1327 TBzeroed=0;
nerit 0:1e09cd7d66b4 1328 zeroTrigger=0;
nerit 0:1e09cd7d66b4 1329 oldZeroCycle = zeroRequestBuf;
nerit 0:1e09cd7d66b4 1330 }
nerit 0:1e09cd7d66b4 1331 }
nerit 0:1e09cd7d66b4 1332 }else{
nerit 0:1e09cd7d66b4 1333 startCycle=0;
nerit 0:1e09cd7d66b4 1334 zeroCycle=0;
nerit 0:1e09cd7d66b4 1335 }
nerit 0:1e09cd7d66b4 1336
nerit 0:1e09cd7d66b4 1337 //***************************************************************************************************
nerit 0:1e09cd7d66b4 1338 // pulseRised define the event of speed wheel pulse occurs
nerit 0:1e09cd7d66b4 1339 //
nerit 0:1e09cd7d66b4 1340 double maxInterval = pulseDistance/minWorkSpeed;
nerit 0:1e09cd7d66b4 1341 double minIntervalPulse = pulseDistance/maxWorkSpeed;
nerit 0:1e09cd7d66b4 1342 if (pulseRised==1){
nerit 0:1e09cd7d66b4 1343 if (enableSpeed<10){enableSpeed++;}
nerit 0:1e09cd7d66b4 1344 pulseRised=0;pulseRised1=1;speedMediaCalc();
nerit 0:1e09cd7d66b4 1345 // calcola velocità trattore
nerit 0:1e09cd7d66b4 1346 if(enableSpeed>=2){
nerit 0:1e09cd7d66b4 1347 if ((pulseSpeedInterval>=0.0f)){ //minIntervalPulse)&&(pulseSpeedInterval<maxInterval)){
nerit 0:1e09cd7d66b4 1348 tractorSpeed_MtS_timed = ((pulseDistance / pulseSpeedInterval)); // tractor speed (unit= Mt/S) from pulse time interval
nerit 0:1e09cd7d66b4 1349 if (checkSDrotation==0){
nerit 0:1e09cd7d66b4 1350 checkSDrotation=1;
nerit 0:1e09cd7d66b4 1351 SDwheelTimer.start();
nerit 0:1e09cd7d66b4 1352 }
nerit 0:1e09cd7d66b4 1353 }
nerit 0:1e09cd7d66b4 1354 }
nerit 0:1e09cd7d66b4 1355 if ((pulseSpeedInterval>=minIntervalPulse)&&(pulseSpeedInterval<maxInterval)){
nerit 0:1e09cd7d66b4 1356 enableSDcontrol =1;
nerit 0:1e09cd7d66b4 1357 }
nerit 0:1e09cd7d66b4 1358 speedTimeOut.reset();
nerit 0:1e09cd7d66b4 1359 }else{
nerit 0:1e09cd7d66b4 1360 double oldLastPr = (double)oldLastPulseRead*1.7f;
nerit 0:1e09cd7d66b4 1361 if((double)speedTimeOut.read_us()> (oldLastPr)){
nerit 0:1e09cd7d66b4 1362 tractorSpeed_MtS_timed = 0.0f;
nerit 0:1e09cd7d66b4 1363 enableSDcontrol =0;
nerit 0:1e09cd7d66b4 1364 pntMedia=0;
nerit 0:1e09cd7d66b4 1365 speedTimeOut.reset();
nerit 0:1e09cd7d66b4 1366 enableSpeed=0;
nerit 0:1e09cd7d66b4 1367 quincCnt=0;
nerit 0:1e09cd7d66b4 1368 }
nerit 0:1e09cd7d66b4 1369 }
nerit 0:1e09cd7d66b4 1370 // esegue il controllo di velocità minima
nerit 0:1e09cd7d66b4 1371 if ((double)speedTimer.read_ms()>=maxInterval){tractorSpeed_MtS_timed = 0.0f;
nerit 0:1e09cd7d66b4 1372 enableSDcontrol =0;
nerit 0:1e09cd7d66b4 1373 enableSpeed=0;
nerit 0:1e09cd7d66b4 1374 }
nerit 0:1e09cd7d66b4 1375 //***************************************************************************************************************
nerit 0:1e09cd7d66b4 1376 // cycle logic control section
nerit 0:1e09cd7d66b4 1377 //***************************************************************************************************************
nerit 0:1e09cd7d66b4 1378 if (enableSimula==1){if(simOk==0){tractorSpeed_MtS_timed=0.0f;}}
nerit 0:1e09cd7d66b4 1379 if ((tractorSpeed_MtS_timed>0.01f)){
nerit 0:1e09cd7d66b4 1380 cycleStopRequest=1;
nerit 0:1e09cd7d66b4 1381 // calcola il tempo teorico di passaggio becchi sulla base della velocità del trattore
nerit 0:1e09cd7d66b4 1382 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 0:1e09cd7d66b4 1383 if (speedFromPick==1) {
nerit 0:1e09cd7d66b4 1384 tempoTraBecchi_mS = (tempoGiroSD / pickNumber)*1000.0f; // tempo tra due impulsi dai becchi in millisecondi ( circa 157mS a 4,5Km/h)
nerit 0:1e09cd7d66b4 1385 }else{
nerit 0:1e09cd7d66b4 1386 tempoTraBecchi_mS = (tempoGiroSD / 25.0f)*1000.0f; // tempo tra due impulsi dai becchi in millisecondi ( circa 157mS a 4,5Km/h)
nerit 0:1e09cd7d66b4 1387 }
nerit 0:1e09cd7d66b4 1388 //*******************************************
nerit 0:1e09cd7d66b4 1389 // segue calcolo duty cycle comando motore DC per allinearsi con la velocità del trattore
nerit 0:1e09cd7d66b4 1390 double dutyTeorico = 0.00;
nerit 0:1e09cd7d66b4 1391 if ((tractorSpeed_MtS_timed>0.0)&&(tractorSpeed_MtS_timed<tabSpeed[0])){dutyTeorico = tabComan[0];}
nerit 0:1e09cd7d66b4 1392 for (int ii = 0;ii<16;ii++){
nerit 0:1e09cd7d66b4 1393 if ((tractorSpeed_MtS_timed>=tabSpeed[ii])&&(tractorSpeed_MtS_timed<tabSpeed[ii+1])){
nerit 0:1e09cd7d66b4 1394 dutyTeorico = tabComan[ii+1];
nerit 0:1e09cd7d66b4 1395 }
nerit 0:1e09cd7d66b4 1396 }
nerit 0:1e09cd7d66b4 1397 if (tractorSpeed_MtS_timed > tabSpeed[16]){dutyTeorico=tractorSpeed_MtS_timed/1.25f;}
nerit 1:e88bf5011af6 1398 // if ((pulseRised1==1)&&(enableSpeed>3)&&(pulseRised2==1)&&(quincMemoEnable==false)){
nerit 1:e88bf5011af6 1399 #if !defined(speedMaster)
nerit 1:e88bf5011af6 1400 #if defined(mezzo)
nerit 1:e88bf5011af6 1401 if (quincEnable==1){
nerit 1:e88bf5011af6 1402 if (quinconceActive==1){
nerit 1:e88bf5011af6 1403 quinCalc(2.0f);
nerit 1:e88bf5011af6 1404 }else{
nerit 1:e88bf5011af6 1405 quinCalc(1.0f);
nerit 1:e88bf5011af6 1406 }
nerit 1:e88bf5011af6 1407 }else{
nerit 1:e88bf5011af6 1408 quincMemoEnable=false;
nerit 1:e88bf5011af6 1409 }
nerit 1:e88bf5011af6 1410 #else
nerit 1:e88bf5011af6 1411 quinCalc(1.0f);
nerit 1:e88bf5011af6 1412 #endif
nerit 1:e88bf5011af6 1413 #endif
nerit 0:1e09cd7d66b4 1414 if ((pulseRised1==1)&&(enableSpeed>3)&&(pulseRised2==1)){
nerit 0:1e09cd7d66b4 1415 double erroreTempo = 0.0f;
nerit 0:1e09cd7d66b4 1416 if(speedFromPick==1){
nerit 1:e88bf5011af6 1417 double giorgio = timeIntraPick/percento;
nerit 1:e88bf5011af6 1418 //erroreTempo = (double)timeIntraPick - tempoTraBecchi_mS; // errore tra il tempo previsto ed il tempo reale ( >0 se sto andando più piano del previsto)
nerit 1:e88bf5011af6 1419 erroreTempo = giorgio - tempoTraBecchi_mS; // errore tra il tempo previsto ed il tempo reale ( >0 se sto andando più piano del previsto)
nerit 0:1e09cd7d66b4 1420 }else{
nerit 0:1e09cd7d66b4 1421 erroreTempo = (double)memoTimeHole - tempoTraBecchi_mS; // errore tra il tempo previsto ed il tempo reale ( >0 se sto andando più piano del previsto)
nerit 0:1e09cd7d66b4 1422 }
nerit 0:1e09cd7d66b4 1423 double errorePercentuale = erroreTempo / tempoTraBecchi_mS;
nerit 1:e88bf5011af6 1424 double k1=errorePercentuale*0.265f;
nerit 1:e88bf5011af6 1425 double k3=0.0f;
nerit 1:e88bf5011af6 1426 double k4=0.0f;
nerit 1:e88bf5011af6 1427 double k5=0.0f;
nerit 1:e88bf5011af6 1428 if (tractorSpeed_MtS_timed <= minSeedSpeed){
nerit 1:e88bf5011af6 1429 k3=0.105;
nerit 1:e88bf5011af6 1430 k4=0.207;
nerit 1:e88bf5011af6 1431 k5=0.310;
nerit 1:e88bf5011af6 1432 }else{
nerit 1:e88bf5011af6 1433 k3=0.1075f;
nerit 1:e88bf5011af6 1434 k4=0.211f;
nerit 1:e88bf5011af6 1435 k5=0.315f;
nerit 1:e88bf5011af6 1436 }
nerit 1:e88bf5011af6 1437 if ((errorePercentuale > 25.0f)||(errorePercentuale < -25.0f)){
nerit 1:e88bf5011af6 1438 k1=errorePercentuale*k5;
nerit 1:e88bf5011af6 1439 }
nerit 1:e88bf5011af6 1440 if (((errorePercentuale >= 10.0f)&&(errorePercentuale<=25.0f))||((errorePercentuale <= -10.0f)&&(errorePercentuale>=-25.0f))){
nerit 1:e88bf5011af6 1441 k1=errorePercentuale*k4;
nerit 1:e88bf5011af6 1442 }
nerit 1:e88bf5011af6 1443 if ((errorePercentuale < 10.0f)||(errorePercentuale > -10.0f)){
nerit 1:e88bf5011af6 1444 k1=errorePercentuale*k3;
nerit 1:e88bf5011af6 1445 }
nerit 0:1e09cd7d66b4 1446 double memoCorrezione = (dutyTeorico *k1);
nerit 1:e88bf5011af6 1447 #if defined(pcSerial)
nerit 1:e88bf5011af6 1448 #if defined(Qnc)
nerit 1:e88bf5011af6 1449 pc.printf("IntraPick: %f TempoBecchi: %f ErrPerc: %f\n",timeIntraPick, tempoTraBecchi_mS,errorePercentuale);
nerit 1:e88bf5011af6 1450 #endif
nerit 1:e88bf5011af6 1451 #endif
nerit 0:1e09cd7d66b4 1452 correzione = correzione + memoCorrezione;
nerit 0:1e09cd7d66b4 1453 pulseRised1=0;
nerit 0:1e09cd7d66b4 1454 pulseRised2=0;
nerit 0:1e09cd7d66b4 1455 }
nerit 0:1e09cd7d66b4 1456 // introduce il controllo di corrente
nerit 0:1e09cd7d66b4 1457 if (incrementCurrent){
nerit 0:1e09cd7d66b4 1458 boostDcOut +=0.005f;
nerit 0:1e09cd7d66b4 1459 }
nerit 0:1e09cd7d66b4 1460 if (reduceCurrent){
nerit 0:1e09cd7d66b4 1461 boostDcOut -=0.005f;
nerit 0:1e09cd7d66b4 1462 }
nerit 0:1e09cd7d66b4 1463 if (boostDcOut >= 0.2f){
nerit 0:1e09cd7d66b4 1464 boostDcOut=0.2f;
nerit 0:1e09cd7d66b4 1465 all_genericals=1;
nerit 0:1e09cd7d66b4 1466 }
nerit 0:1e09cd7d66b4 1467 if (boostDcOut <=-0.2f){
nerit 0:1e09cd7d66b4 1468 boostDcOut=-0.2f;
nerit 0:1e09cd7d66b4 1469 all_genericals=1;
nerit 0:1e09cd7d66b4 1470 }
nerit 1:e88bf5011af6 1471 //correzione += boostDcOut;
nerit 0:1e09cd7d66b4 1472 #if defined(pcSerial)
nerit 0:1e09cd7d66b4 1473 #if defined(Qnca)
nerit 0:1e09cd7d66b4 1474 pc.printf(" DUTY PRE FINALE: %f",dcActualDuty);
nerit 0:1e09cd7d66b4 1475 #endif
nerit 0:1e09cd7d66b4 1476 #endif
nerit 0:1e09cd7d66b4 1477 // prova a mantenere il quinconce quando attivo
nerit 1:e88bf5011af6 1478 //dcActualDuty=dcActualDuty;
nerit 1:e88bf5011af6 1479 /*
nerit 1:e88bf5011af6 1480 #if !defined(speedMaster)
nerit 0:1e09cd7d66b4 1481 #if defined(mezzo)
nerit 0:1e09cd7d66b4 1482 if (quincEnable==1){
nerit 0:1e09cd7d66b4 1483 if (quinconceActive==1){
nerit 0:1e09cd7d66b4 1484 quinCalc(2.0f);
nerit 0:1e09cd7d66b4 1485 }else{
nerit 0:1e09cd7d66b4 1486 quinCalc(1.0f);
nerit 0:1e09cd7d66b4 1487 }
nerit 0:1e09cd7d66b4 1488 }else{
nerit 1:e88bf5011af6 1489 quincMemoEnable=false;
nerit 0:1e09cd7d66b4 1490 }
nerit 0:1e09cd7d66b4 1491 #else
nerit 0:1e09cd7d66b4 1492 quinCalc(1.0f);
nerit 0:1e09cd7d66b4 1493 #endif
nerit 0:1e09cd7d66b4 1494 #endif
nerit 1:e88bf5011af6 1495 */
nerit 1:e88bf5011af6 1496 DC_brake=0;
nerit 1:e88bf5011af6 1497 DC_forward=1;
nerit 1:e88bf5011af6 1498 DC_prepare();
nerit 0:1e09cd7d66b4 1499
nerit 0:1e09cd7d66b4 1500 // il semiperiodoreale è calcolato sulla lettura del passaggio becchi reale
nerit 0:1e09cd7d66b4 1501 if (correggiSD == true){
nerit 0:1e09cd7d66b4 1502 seedWheelPeriod = semiPeriodoReale / abs(percento);
nerit 0:1e09cd7d66b4 1503 }else{
nerit 0:1e09cd7d66b4 1504 seedWheelPeriod = semiPeriodoReale;
nerit 0:1e09cd7d66b4 1505 }
nerit 0:1e09cd7d66b4 1506 if (seedWheelPeriod < 180.0f){seedWheelPeriod = 180.0f;}
nerit 0:1e09cd7d66b4 1507 if ((oldSeedWheelPeriod!=seedWheelPeriod)&&(seedWheelPeriod >=180.0f )){
nerit 0:1e09cd7d66b4 1508 SDticker.attach_us(&step_SDPulseOut,seedWheelPeriod); // clock time are milliseconds and attach seed motor stepper controls
nerit 0:1e09cd7d66b4 1509 oldSeedWheelPeriod=seedWheelPeriod;
nerit 0:1e09cd7d66b4 1510 }
nerit 0:1e09cd7d66b4 1511
nerit 0:1e09cd7d66b4 1512 //if (correzione > (dutyTeorico*0.20)){correzione = dutyTeorico*0.20;}
nerit 1:e88bf5011af6 1513 if (correzione > 0.30f){correzione = 0.30f;}
nerit 1:e88bf5011af6 1514 if (correzione < -0.30f){correzione = -0.30f;}
nerit 0:1e09cd7d66b4 1515 if((enableSDcontrol==1)){
nerit 0:1e09cd7d66b4 1516 if (correzioneAttiva==1){
nerit 0:1e09cd7d66b4 1517 dcActualDuty = dutyTeorico + correzione;
nerit 1:e88bf5011af6 1518 //if (dcActualDuty<dcStarting){dcActualDuty=dcStarting;}
nerit 0:1e09cd7d66b4 1519 }
nerit 0:1e09cd7d66b4 1520 }else{
nerit 0:1e09cd7d66b4 1521 dcActualDuty = dutyTeorico;
nerit 0:1e09cd7d66b4 1522 }
nerit 0:1e09cd7d66b4 1523 if (dcActualDuty <0.0f){dcActualDuty=0.0f;}
nerit 0:1e09cd7d66b4 1524 if (dcActualDuty > 1.0f){dcActualDuty = dcMaxSpeed;}
nerit 0:1e09cd7d66b4 1525 #if defined(pcSerial)
nerit 0:1e09cd7d66b4 1526 #if defined(Qnca)
nerit 0:1e09cd7d66b4 1527 pc.printf(" DUTY FINALE: %f \n",dcActualDuty);
nerit 0:1e09cd7d66b4 1528 #endif
nerit 0:1e09cd7d66b4 1529 #endif
nerit 0:1e09cd7d66b4 1530 SDmotorPWM.write(dcActualDuty);
nerit 0:1e09cd7d66b4 1531 // allarme
nerit 0:1e09cd7d66b4 1532 if (SDwheelTimer.read_ms()>4000){
nerit 0:1e09cd7d66b4 1533 if (firstStart==0){
nerit 0:1e09cd7d66b4 1534 all_noDcRotati=1;
nerit 0:1e09cd7d66b4 1535 }
nerit 0:1e09cd7d66b4 1536 #if defined(pcSerial)
nerit 0:1e09cd7d66b4 1537 #if defined(VediAllarmi)
nerit 0:1e09cd7d66b4 1538 pc.printf("allarme no DC rotation");
nerit 0:1e09cd7d66b4 1539 #endif
nerit 0:1e09cd7d66b4 1540 #endif
nerit 0:1e09cd7d66b4 1541
nerit 0:1e09cd7d66b4 1542 }
nerit 0:1e09cd7d66b4 1543
nerit 0:1e09cd7d66b4 1544 //***************************************************************************************************************
nerit 0:1e09cd7d66b4 1545 // CONTROLLA TAMBURO
nerit 0:1e09cd7d66b4 1546 //***************************************************************************************************************
nerit 0:1e09cd7d66b4 1547 if(lowSpeed==0){
nerit 0:1e09cd7d66b4 1548 if (syncroCheck==1){
nerit 0:1e09cd7d66b4 1549 syncroCheck=0;
nerit 0:1e09cd7d66b4 1550 lockStart=1;
nerit 0:1e09cd7d66b4 1551 periodo = TBperiod;
nerit 0:1e09cd7d66b4 1552 if (aspettaStart==0){cambiaTB(periodo);}
nerit 0:1e09cd7d66b4 1553 }
nerit 0:1e09cd7d66b4 1554 // controllo di stop
nerit 0:1e09cd7d66b4 1555 double memoIntraP = (double)memoIntraPick*1.8f;
nerit 0:1e09cd7d66b4 1556 if ((double)rotationTimeOut.read_ms()> (memoIntraP)){
nerit 0:1e09cd7d66b4 1557 syncroCheck=0;
nerit 0:1e09cd7d66b4 1558 aspettaStart=1;
nerit 0:1e09cd7d66b4 1559 countCicli=0;
nerit 0:1e09cd7d66b4 1560 if (TBzeroCyclePulse==1){TBticker.detach();}
nerit 0:1e09cd7d66b4 1561 }
nerit 0:1e09cd7d66b4 1562 }else{ // fine ciclo fuori da low speed
nerit 0:1e09cd7d66b4 1563 syncroCheck=0;
nerit 0:1e09cd7d66b4 1564 lockStart=0;
nerit 0:1e09cd7d66b4 1565 if (beccoPronto==1){
nerit 0:1e09cd7d66b4 1566 if (tamburoStandard==1){
nerit 0:1e09cd7d66b4 1567 double ritardoMassimo = 0.0f;
nerit 0:1e09cd7d66b4 1568 if(speedFromPick==1){
nerit 0:1e09cd7d66b4 1569 ritardoMassimo = (double)timeIntraPick;
nerit 0:1e09cd7d66b4 1570 }else{
nerit 0:1e09cd7d66b4 1571 ritardoMassimo = (double)memoTimeHole;
nerit 0:1e09cd7d66b4 1572 }
nerit 0:1e09cd7d66b4 1573 int tempoDiSincro = (int)((double)(ritardoMassimo - ((tempoBecco/2.0f)+((speedOfSeedWheel/1.25f)*ritardoMassimo)))); //
nerit 0:1e09cd7d66b4 1574 if (tempoDiSincro <= 1){tempoDiSincro=1;}
nerit 0:1e09cd7d66b4 1575 if ((sincroTimer.read_ms()>= tempoDiSincro)){
nerit 0:1e09cd7d66b4 1576 if (tractorSpeed_MtS_timed >= minWorkSpeed){startCicloTB=1;}
nerit 0:1e09cd7d66b4 1577 beccoPronto=0;
nerit 0:1e09cd7d66b4 1578 }
nerit 0:1e09cd7d66b4 1579 }else{
nerit 0:1e09cd7d66b4 1580 // tamburo per zucca
nerit 0:1e09cd7d66b4 1581 if (speedOfSeedWheel >= minWorkSpeed){startCicloTB=1;}
nerit 0:1e09cd7d66b4 1582 beccoPronto=0;
nerit 0:1e09cd7d66b4 1583 }
nerit 0:1e09cd7d66b4 1584 }
nerit 0:1e09cd7d66b4 1585 ciclaTB();
nerit 0:1e09cd7d66b4 1586 }
nerit 0:1e09cd7d66b4 1587 //*************************************************************
nerit 0:1e09cd7d66b4 1588 }else{ // fine ciclo con velocita maggiore di 0
nerit 0:1e09cd7d66b4 1589 SDwheelTimer.stop();
nerit 0:1e09cd7d66b4 1590 SDwheelTimer.reset();
nerit 0:1e09cd7d66b4 1591 checkSDrotation=0;
nerit 0:1e09cd7d66b4 1592 oldFaseLavoro=0;
nerit 0:1e09cd7d66b4 1593 aspettaStart=1;
nerit 0:1e09cd7d66b4 1594 countCicli=0;
nerit 0:1e09cd7d66b4 1595 startCycle=0;
nerit 0:1e09cd7d66b4 1596 oldSeedWheelPeriod=0.0f;
nerit 0:1e09cd7d66b4 1597 oldPeriodoTB=0.0f;
nerit 0:1e09cd7d66b4 1598 correzione=0.0f;
nerit 0:1e09cd7d66b4 1599 OLDpulseSpeedInterval=1000.01f;
nerit 0:1e09cd7d66b4 1600 cicloTbinCorso=0;
nerit 0:1e09cd7d66b4 1601 cntTbError=0;
nerit 0:1e09cd7d66b4 1602 if (cycleStopRequest==1){
nerit 0:1e09cd7d66b4 1603 zeroDelay.reset();
nerit 0:1e09cd7d66b4 1604 zeroDelay.start();
nerit 0:1e09cd7d66b4 1605 runRequestBuf=0;
nerit 0:1e09cd7d66b4 1606 zeroRequestBuf=1;
nerit 0:1e09cd7d66b4 1607 cycleStopRequest=0;
nerit 0:1e09cd7d66b4 1608 SDzeroCyclePulse=0;
nerit 0:1e09cd7d66b4 1609 TBzeroCyclePulse=0;
nerit 0:1e09cd7d66b4 1610 zeroCycleEnd=0;
nerit 0:1e09cd7d66b4 1611 zeroCycle=1;
nerit 0:1e09cd7d66b4 1612 zeroTrigger=0;
nerit 0:1e09cd7d66b4 1613 }
nerit 0:1e09cd7d66b4 1614 }
nerit 0:1e09cd7d66b4 1615 //*************************************************************************************************
nerit 0:1e09cd7d66b4 1616 // ciclo di azzeramento motori
nerit 0:1e09cd7d66b4 1617 if ((zeroCycleEnd==0)&&(zeroCycle==1)){//&&(zeroDelay.read_ms()>10000)){
nerit 0:1e09cd7d66b4 1618 if (zeroTrigger==0){
nerit 0:1e09cd7d66b4 1619 TBticker.attach_us(&step_TBPulseOut,1000.0f); // clock time are seconds and attach seed motor stepper controls
nerit 0:1e09cd7d66b4 1620 DC_forward=1;
nerit 0:1e09cd7d66b4 1621 DC_brake=1;
nerit 0:1e09cd7d66b4 1622 DC_prepare();
nerit 0:1e09cd7d66b4 1623 frenata=0;
nerit 0:1e09cd7d66b4 1624 zeroTrigger=1;
nerit 0:1e09cd7d66b4 1625 ritardoStop.reset();
nerit 0:1e09cd7d66b4 1626 ritardoComandoStop.reset();
nerit 0:1e09cd7d66b4 1627 ritardoComandoStop.start();
nerit 0:1e09cd7d66b4 1628 timeOutZeroSD.start();
nerit 0:1e09cd7d66b4 1629 quincTimeSet.reset();
nerit 0:1e09cd7d66b4 1630 }
nerit 0:1e09cd7d66b4 1631 int tempoFrenata=300;
nerit 0:1e09cd7d66b4 1632 if (((ritardoStop.read_ms()>tempoFrenata)&&(SDzeroDebounced==0))||(accensione==1)||(ritardoComandoStop.read_ms()>400)){
nerit 0:1e09cd7d66b4 1633 accensione=0;
nerit 0:1e09cd7d66b4 1634 avvicinamento=1;
nerit 0:1e09cd7d66b4 1635 avvicinamentoOn.reset();
nerit 0:1e09cd7d66b4 1636 avvicinamentoOn.start();
nerit 0:1e09cd7d66b4 1637 SDmotorPWM.write(0.32f); // duty cycle = 60% of power
nerit 0:1e09cd7d66b4 1638 DC_forward=1;
nerit 0:1e09cd7d66b4 1639 DC_brake=0;
nerit 0:1e09cd7d66b4 1640 DC_prepare();
nerit 0:1e09cd7d66b4 1641 ritardoComandoStop.reset();
nerit 0:1e09cd7d66b4 1642 ritardoComandoStop.stop();
nerit 0:1e09cd7d66b4 1643 }
nerit 0:1e09cd7d66b4 1644 if (avvicinamento==1){
nerit 0:1e09cd7d66b4 1645 if(avvicinamentoOn.read_ms()>300){
nerit 0:1e09cd7d66b4 1646 SDmotorPWM.write(0.7f);
nerit 0:1e09cd7d66b4 1647 avvicinamentoOn.reset();
nerit 0:1e09cd7d66b4 1648 avvicinamentoOff.reset();
nerit 0:1e09cd7d66b4 1649 avvicinamentoOff.start();
nerit 0:1e09cd7d66b4 1650 }
nerit 0:1e09cd7d66b4 1651 if(avvicinamentoOff.read_ms()>100){
nerit 0:1e09cd7d66b4 1652 SDmotorPWM.write(0.32f);
nerit 0:1e09cd7d66b4 1653 avvicinamentoOff.reset();
nerit 0:1e09cd7d66b4 1654 avvicinamentoOff.stop();
nerit 0:1e09cd7d66b4 1655 avvicinamentoOn.start();
nerit 0:1e09cd7d66b4 1656 }
nerit 0:1e09cd7d66b4 1657 }else{
nerit 0:1e09cd7d66b4 1658 avvicinamentoOn.stop();
nerit 0:1e09cd7d66b4 1659 avvicinamentoOff.stop();
nerit 0:1e09cd7d66b4 1660 avvicinamentoOn.reset();
nerit 0:1e09cd7d66b4 1661 avvicinamentoOff.reset();
nerit 0:1e09cd7d66b4 1662 }
nerit 0:1e09cd7d66b4 1663 if (frenata==0){
nerit 0:1e09cd7d66b4 1664 if (SDzeroCyclePulse==1){
nerit 0:1e09cd7d66b4 1665 SDticker.detach();
nerit 0:1e09cd7d66b4 1666 frenata=1;
nerit 0:1e09cd7d66b4 1667 quincTimeSet.reset();
nerit 0:1e09cd7d66b4 1668 quincTimeSet.start();
nerit 0:1e09cd7d66b4 1669 ritardoStop.start();
nerit 0:1e09cd7d66b4 1670 //ritardoComandoStop.reset();
nerit 0:1e09cd7d66b4 1671 //ritardoComandoStop.stop();
nerit 0:1e09cd7d66b4 1672 }
nerit 0:1e09cd7d66b4 1673 }else{
nerit 0:1e09cd7d66b4 1674 if (quinconceActive==0){
nerit 0:1e09cd7d66b4 1675 if (SDzeroCyclePulse==1){
nerit 0:1e09cd7d66b4 1676 avvicinamento=0;
nerit 0:1e09cd7d66b4 1677 DC_brake=1;
nerit 0:1e09cd7d66b4 1678 DC_prepare();
nerit 0:1e09cd7d66b4 1679 SDzeroed=1;
nerit 0:1e09cd7d66b4 1680 ritardoStop.reset();
nerit 0:1e09cd7d66b4 1681 ritardoStop.stop();
nerit 0:1e09cd7d66b4 1682 }
nerit 0:1e09cd7d66b4 1683 }else{
nerit 0:1e09cd7d66b4 1684 if (quincTimeSet.read_ms()>500){
nerit 0:1e09cd7d66b4 1685 avvicinamento=0;
nerit 0:1e09cd7d66b4 1686 DC_brake=1;
nerit 0:1e09cd7d66b4 1687 DC_prepare();
nerit 0:1e09cd7d66b4 1688 SDzeroed=1;
nerit 0:1e09cd7d66b4 1689 ritardoStop.reset();
nerit 0:1e09cd7d66b4 1690 ritardoStop.stop();
nerit 0:1e09cd7d66b4 1691 quincTimeSet.stop();
nerit 0:1e09cd7d66b4 1692 }
nerit 0:1e09cd7d66b4 1693 }
nerit 0:1e09cd7d66b4 1694 }
nerit 0:1e09cd7d66b4 1695 // azzera tutto in time out
nerit 0:1e09cd7d66b4 1696 if (timeOutZeroSD.read_ms()>=10000){
nerit 0:1e09cd7d66b4 1697 #if defined(pcSerial)
nerit 0:1e09cd7d66b4 1698 #if defined(canDataReceived)
nerit 0:1e09cd7d66b4 1699 pc.printf("allarme no zero");
nerit 0:1e09cd7d66b4 1700 #endif
nerit 0:1e09cd7d66b4 1701 #endif
nerit 0:1e09cd7d66b4 1702 if (firstStart==0){
nerit 0:1e09cd7d66b4 1703 all_no_Zeroing=1;
nerit 0:1e09cd7d66b4 1704 }
nerit 0:1e09cd7d66b4 1705 avvicinamento=0;
nerit 0:1e09cd7d66b4 1706 DC_brake=1;
nerit 0:1e09cd7d66b4 1707 DC_prepare();
nerit 0:1e09cd7d66b4 1708 SDzeroed=1;
nerit 0:1e09cd7d66b4 1709 ritardoStop.reset();
nerit 0:1e09cd7d66b4 1710 ritardoStop.stop();
nerit 0:1e09cd7d66b4 1711 avvicinamentoOn.stop();
nerit 0:1e09cd7d66b4 1712 avvicinamentoOff.stop();
nerit 0:1e09cd7d66b4 1713 avvicinamentoOn.reset();
nerit 0:1e09cd7d66b4 1714 avvicinamentoOff.reset();
nerit 0:1e09cd7d66b4 1715 ritardoComandoStop.reset();
nerit 0:1e09cd7d66b4 1716 ritardoComandoStop.stop();
nerit 0:1e09cd7d66b4 1717 timeOutZeroSD.stop();
nerit 0:1e09cd7d66b4 1718 timeOutZeroSD.reset();
nerit 0:1e09cd7d66b4 1719 }
nerit 0:1e09cd7d66b4 1720 if (TBzeroCyclePulse==1){
nerit 0:1e09cd7d66b4 1721 TBticker.detach();
nerit 0:1e09cd7d66b4 1722 TBzeroed=1;
nerit 0:1e09cd7d66b4 1723 }
nerit 0:1e09cd7d66b4 1724 if ((SDzeroed==1)&&(TBzeroed==1)){
nerit 0:1e09cd7d66b4 1725 avvicinamentoOn.stop();
nerit 0:1e09cd7d66b4 1726 avvicinamentoOff.stop();
nerit 0:1e09cd7d66b4 1727 ritardoComandoStop.stop();
nerit 0:1e09cd7d66b4 1728 ritardoStop.stop();
nerit 0:1e09cd7d66b4 1729 zeroCycleEnd=1;
nerit 0:1e09cd7d66b4 1730 zeroCycle=0;
nerit 0:1e09cd7d66b4 1731 zeroTrigger=0;
nerit 0:1e09cd7d66b4 1732 runRequestBuf=1;
nerit 0:1e09cd7d66b4 1733 zeroRequestBuf=0;
nerit 0:1e09cd7d66b4 1734 cycleStopRequest=0;
nerit 0:1e09cd7d66b4 1735 SDzeroed=0;
nerit 0:1e09cd7d66b4 1736 TBzeroed=0;
nerit 0:1e09cd7d66b4 1737 timeOutZeroSD.stop();
nerit 0:1e09cd7d66b4 1738 timeOutZeroSD.reset();
nerit 0:1e09cd7d66b4 1739 }
nerit 0:1e09cd7d66b4 1740 }
nerit 0:1e09cd7d66b4 1741
nerit 0:1e09cd7d66b4 1742 //*************************************************************************************************
nerit 0:1e09cd7d66b4 1743 if (enableCycle==0){
nerit 0:1e09cd7d66b4 1744 zeroTrigger=0;
nerit 0:1e09cd7d66b4 1745 SDmotorPWM=0;
nerit 0:1e09cd7d66b4 1746 SDmotorInA=0;
nerit 0:1e09cd7d66b4 1747 SDmotorInB=0;
nerit 0:1e09cd7d66b4 1748 }
nerit 0:1e09cd7d66b4 1749 SDzeroCyclePulse=0;
nerit 0:1e09cd7d66b4 1750 TBzeroCyclePulse=0;
nerit 0:1e09cd7d66b4 1751 //*************************************************************************************************
nerit 0:1e09cd7d66b4 1752 }else{//end ciclo normale
nerit 0:1e09cd7d66b4 1753 //*************************************************************************************************
nerit 0:1e09cd7d66b4 1754 // task di prova della scheda
nerit 0:1e09cd7d66b4 1755 //*************************************************************************************************
nerit 0:1e09cd7d66b4 1756 #if defined(provaScheda)
nerit 0:1e09cd7d66b4 1757 clocca++;
nerit 0:1e09cd7d66b4 1758 //led = !led;
nerit 0:1e09cd7d66b4 1759 //txMsg.clear();
nerit 0:1e09cd7d66b4 1760 //txMsg << clocca;
nerit 0:1e09cd7d66b4 1761 //test.printf("aogs \n");
nerit 0:1e09cd7d66b4 1762 //if(can1.write(txMsg)){
nerit 0:1e09cd7d66b4 1763 #if defined(pcSerial)
nerit 0:1e09cd7d66b4 1764 pc.printf("Can write OK \n");
nerit 0:1e09cd7d66b4 1765 #endif
nerit 0:1e09cd7d66b4 1766 //}
nerit 0:1e09cd7d66b4 1767 switch (clocca){
nerit 0:1e09cd7d66b4 1768 case 1:
nerit 0:1e09cd7d66b4 1769 TBmotorStepOut=1; // define step command for up down motor driver
nerit 0:1e09cd7d66b4 1770 break;
nerit 0:1e09cd7d66b4 1771 case 2:
nerit 0:1e09cd7d66b4 1772 SDmotorPWM=1; // define step command for seeding whell motor driver
nerit 0:1e09cd7d66b4 1773 break;
nerit 0:1e09cd7d66b4 1774 case 3:
nerit 0:1e09cd7d66b4 1775 speedClock=1; // define input of
nerit 0:1e09cd7d66b4 1776 break;
nerit 0:1e09cd7d66b4 1777 case 4:
nerit 0:1e09cd7d66b4 1778 break;
nerit 0:1e09cd7d66b4 1779 case 5:
nerit 0:1e09cd7d66b4 1780 SDmotorInA=1;
nerit 0:1e09cd7d66b4 1781 SDmotorInB=0;
nerit 0:1e09cd7d66b4 1782 break;
nerit 0:1e09cd7d66b4 1783 case 6:
nerit 0:1e09cd7d66b4 1784 break;
nerit 0:1e09cd7d66b4 1785 case 7:
nerit 0:1e09cd7d66b4 1786 break;
nerit 0:1e09cd7d66b4 1787 case 8:
nerit 0:1e09cd7d66b4 1788 break;
nerit 0:1e09cd7d66b4 1789 case 9:
nerit 0:1e09cd7d66b4 1790 break;
nerit 0:1e09cd7d66b4 1791 case 10:
nerit 0:1e09cd7d66b4 1792 break;
nerit 0:1e09cd7d66b4 1793 case 11:
nerit 0:1e09cd7d66b4 1794 break;
nerit 0:1e09cd7d66b4 1795 case 12:
nerit 0:1e09cd7d66b4 1796 break;
nerit 0:1e09cd7d66b4 1797 case 13:
nerit 0:1e09cd7d66b4 1798 break;
nerit 0:1e09cd7d66b4 1799 case 14:
nerit 0:1e09cd7d66b4 1800 SDmotorPWM=1; // power mosfet 2 command out
nerit 0:1e09cd7d66b4 1801 break;
nerit 0:1e09cd7d66b4 1802 case 15:
nerit 0:1e09cd7d66b4 1803 break;
nerit 0:1e09cd7d66b4 1804 case 16:
nerit 0:1e09cd7d66b4 1805 case 17:
nerit 0:1e09cd7d66b4 1806 break;
nerit 0:1e09cd7d66b4 1807 case 18:
nerit 0:1e09cd7d66b4 1808 TBmotorStepOut=0; // define step command for up down motor driver
nerit 0:1e09cd7d66b4 1809 SDmotorPWM=0; // define step command for seeding whell motor driver
nerit 0:1e09cd7d66b4 1810 speedClock=0; // define input of
nerit 0:1e09cd7d66b4 1811 SDmotorInA=0;
nerit 0:1e09cd7d66b4 1812 SDmotorInB=0;
nerit 0:1e09cd7d66b4 1813 SDmotorPWM=0; // power mosfet 2 command out
nerit 0:1e09cd7d66b4 1814 break;
nerit 0:1e09cd7d66b4 1815 default:
nerit 0:1e09cd7d66b4 1816 clocca=0;
nerit 0:1e09cd7d66b4 1817 break;
nerit 0:1e09cd7d66b4 1818 }
nerit 0:1e09cd7d66b4 1819 wait_ms(100);
nerit 0:1e09cd7d66b4 1820 #endif // end prova scheda
nerit 0:1e09cd7d66b4 1821
nerit 0:1e09cd7d66b4 1822 #if defined(provaDC)
nerit 0:1e09cd7d66b4 1823 int rampa=1000;
nerit 0:1e09cd7d66b4 1824 int pausa=3000;
nerit 0:1e09cd7d66b4 1825 switch (clocca){
nerit 0:1e09cd7d66b4 1826 case 0:
nerit 0:1e09cd7d66b4 1827 DC_brake=0;
nerit 0:1e09cd7d66b4 1828 DC_forward=1;
nerit 0:1e09cd7d66b4 1829 duty_DC+=0.01f;
nerit 0:1e09cd7d66b4 1830 if (duty_DC>=0.2f){
nerit 0:1e09cd7d66b4 1831 duty_DC=0.2f;
nerit 0:1e09cd7d66b4 1832 clocca=1;
nerit 0:1e09cd7d66b4 1833 }
nerit 0:1e09cd7d66b4 1834 wait_ms(rampa);
nerit 0:1e09cd7d66b4 1835 break;
nerit 0:1e09cd7d66b4 1836 case 1:
nerit 0:1e09cd7d66b4 1837 wait_ms(pausa*4);
nerit 0:1e09cd7d66b4 1838 clocca=2;
nerit 0:1e09cd7d66b4 1839 break;
nerit 0:1e09cd7d66b4 1840 case 2:
nerit 0:1e09cd7d66b4 1841 DC_brake=0;
nerit 0:1e09cd7d66b4 1842 DC_forward=1;
nerit 0:1e09cd7d66b4 1843 duty_DC-=0.01f;
nerit 0:1e09cd7d66b4 1844 if (duty_DC<=0.0f){
nerit 0:1e09cd7d66b4 1845 duty_DC=0.0f;
nerit 0:1e09cd7d66b4 1846 clocca=3;
nerit 0:1e09cd7d66b4 1847 }
nerit 0:1e09cd7d66b4 1848 wait_ms(rampa);
nerit 0:1e09cd7d66b4 1849 break;
nerit 0:1e09cd7d66b4 1850 case 3:
nerit 0:1e09cd7d66b4 1851 wait_ms(pausa);
nerit 0:1e09cd7d66b4 1852 clocca=4;
nerit 0:1e09cd7d66b4 1853 break;
nerit 0:1e09cd7d66b4 1854 case 4:
nerit 0:1e09cd7d66b4 1855 DC_brake=0;
nerit 0:1e09cd7d66b4 1856 DC_forward=1;
nerit 0:1e09cd7d66b4 1857 duty_DC+=0.01f;
nerit 0:1e09cd7d66b4 1858 if (duty_DC>=1.0f){
nerit 0:1e09cd7d66b4 1859 duty_DC=1.0f;
nerit 0:1e09cd7d66b4 1860 clocca=5;
nerit 0:1e09cd7d66b4 1861 }
nerit 0:1e09cd7d66b4 1862 wait_ms(rampa);
nerit 0:1e09cd7d66b4 1863 break;
nerit 0:1e09cd7d66b4 1864 case 5:
nerit 0:1e09cd7d66b4 1865 wait_ms(pausa);
nerit 0:1e09cd7d66b4 1866 clocca=6;
nerit 0:1e09cd7d66b4 1867 break;
nerit 0:1e09cd7d66b4 1868 case 6:
nerit 0:1e09cd7d66b4 1869 DC_brake=0;
nerit 0:1e09cd7d66b4 1870 DC_forward=1;
nerit 0:1e09cd7d66b4 1871 duty_DC-=0.01f;
nerit 0:1e09cd7d66b4 1872 if (duty_DC<=0.0f){
nerit 0:1e09cd7d66b4 1873 duty_DC=0.0f;
nerit 0:1e09cd7d66b4 1874 clocca=7;
nerit 0:1e09cd7d66b4 1875 }
nerit 0:1e09cd7d66b4 1876 wait_ms(rampa);
nerit 0:1e09cd7d66b4 1877 break;
nerit 0:1e09cd7d66b4 1878 case 7:
nerit 0:1e09cd7d66b4 1879 wait_ms(pausa);
nerit 0:1e09cd7d66b4 1880 clocca=0;
nerit 0:1e09cd7d66b4 1881 break;
nerit 0:1e09cd7d66b4 1882 default:
nerit 0:1e09cd7d66b4 1883 break;
nerit 0:1e09cd7d66b4 1884 }
nerit 0:1e09cd7d66b4 1885 if (oldDuty_DC != duty_DC){
nerit 0:1e09cd7d66b4 1886 SDmotorPWM.write(duty_DC); // duty cycle = stop
nerit 0:1e09cd7d66b4 1887 oldDuty_DC=duty_DC;
nerit 0:1e09cd7d66b4 1888 DC_prepare();
nerit 0:1e09cd7d66b4 1889 }
nerit 0:1e09cd7d66b4 1890 #endif
nerit 0:1e09cd7d66b4 1891 }//end in prova
nerit 0:1e09cd7d66b4 1892 wd.Service(); // kick the dog before the timeout
nerit 0:1e09cd7d66b4 1893 } // end while
nerit 0:1e09cd7d66b4 1894 }// end main
nerit 0:1e09cd7d66b4 1895