Francesco Pistone / Mbed 2 deprecated FORIGO_Modula_V6_R2C_DE_HwV5_31gen2019_2019

new

Dependencies:   mbed CANMsg

main.cpp@16:6f994c4c3991, 2019-01-31 (annotated)

Committer:
francescopistone
Date:
Thu Jan 31 08:01:11 2019 +0000
Branch:
V6_R2C_DE_HwV5_versione3
Revision:
16:6f994c4c3991
Parent:
15:2af7ca4c5eb9 
3v_2019

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