Forigo / Mbed 2 deprecated FORIGO_Modula_V7_3_VdcStep_maggio2020

Messa in campo 4 file - 26/06/2020 Francia

Dependencies:   mbed X_NUCLEO_IHM03A1_for

Fork of FORIGO_Modula_V7_3_VdcStep_maggio2020 by Francesco Pistone

main.cpp@17:9629eb019892, 2019-04-05 (annotated)

Committer:
nerit
Date:
Fri Apr 05 07:52:39 2019 +0000
Revision:
17:9629eb019892
Parent:
16:786bb20a6759
Child:
18:7c978f69cc51
aggiornamento del 04/04/2019

Who changed what in which revision?

UserRevisionLine numberNew contents of line
nerit 3:a469bbd294b5 1
nerit 3:a469bbd294b5 2 //********************************************************************************************************************
nerit 3:a469bbd294b5 3 //********************************************************************************************************************
nerit 3:a469bbd294b5 4 // FIRMWARE SEMINATRICE MODULA
nerit 3:a469bbd294b5 5 // VERSIONE PER SCHEDA DI CONTROLLO CON DRIVER INTEGRATI
nerit 3:a469bbd294b5 6 // V7 - ATTENZIONE - LA VERSIONE V7 HA IL DRIVER STEPPER POWERSTEP01 DA 10A
nerit 3:a469bbd294b5 7 // IL PROCESSORE UTILIZZATO E' IL STM32L476RG A 80MHz
nerit 3:a469bbd294b5 8 // IL MOTORE DC E' GESTITO CON IL DRIVER VNH3SP30-E E CON LA LETTURA
nerit 3:a469bbd294b5 9 // DELLA CORRENTE ASSORBITA TRAMITE IL CONVERTITORE MLX91210-CAS102 CON 50A FONDOSCALA
nerit 3:a469bbd294b5 10 // CHE FORNISCE UNA TENSIONE DI USCITA PARI A 40mV/A
nerit 3:a469bbd294b5 11 // FIRST RELEASE OF BOARD DEC 2017
nerit 3:a469bbd294b5 12 // FIRST RELEASE OF FIRMWARE JAN 2018
nerit 3:a469bbd294b5 13 //
nerit 4:de1b296e9757 14 // THIS RELEASE: 10 february 2019
nerit 3:a469bbd294b5 15 //
nerit 8:310f9e4eac7b 16 // APPLICATION: MODULA CON DISTRIBUTORE RISO ED ENCODER MOTORE
nerit 3:a469bbd294b5 17 //
nerit 3:a469bbd294b5 18 // 29 05 2018 - INSERITO SECONDO ENCODER VIRTUALE PER LA GESTIONE DEL SINCRONISMO TRA TAMBURO E RUOTA DI SEMINA
nerit 3:a469bbd294b5 19 // IN PRATICA IL PRIMO ENCODER è SINCRONO CON IL SEGNALE DEI BECCHI E VIENE AZZERATO DA QUESTI, MENTRE
nerit 3:a469bbd294b5 20 // IL SECONDO E' INCREMENTATO IN SINCRONO CON IL PRIMO MA AZZERATO DALLA FASE. IL SUO VALORE E' POI DIVISO
nerit 3:a469bbd294b5 21 // PER IL RAPPORTO RUOTE E LA CORREZIONE AGISCE SULLA VELOCITA' DEL TAMBURO PER MANTENERE LA FASE DEL SECONDO
nerit 3:a469bbd294b5 22 // ENCODER
nerit 3:a469bbd294b5 23 // 05 06 2018 - INSERITO IL CONTROLLO DI GESTIONE DEL QUINCONCE SENZA ENCODER
nerit 3:a469bbd294b5 24 // 09 06 2018 - INSERITO CONTROLLO DI FASE CON ENCODER MASTER PER QUINCONCE - DATO SCAMBIATO IN CAN
nerit 3:a469bbd294b5 25 // 03 01 2019 - INSERITA GESTIONE IN RTOS PER IL DRIVER POWERSTEP01
nerit 4:de1b296e9757 26 // 10 02 2019 - INSERITO FUNZIONAMENTO STEPPER IN MODALITA' CONTROLLO DI TENSIONE E STEP DA CLOCKPIN
nerit 6:e8c18f0f399a 27 // 16 02 2019 - SOSTITUITA LIBRERIA MBED PER PROBLEMI DI COMPILAZIONE DEL FIRMWARE
nerit 3:a469bbd294b5 28 /********************
nerit 3:a469bbd294b5 29 IL FIRMWARE SI COMPONE DI 10 FILES:
nerit 3:a469bbd294b5 30 - main.cpp
nerit 3:a469bbd294b5 31 - main.hpp
nerit 3:a469bbd294b5 32 - iodefinition.hpp
nerit 3:a469bbd294b5 33 - canbus.hpp
nerit 3:a469bbd294b5 34 - parameters.hpp
nerit 4:de1b296e9757 35 - powerstep.hpp
nerit 3:a469bbd294b5 36 - timeandtick.hpp
nerit 3:a469bbd294b5 37 - variables.hpp
nerit 3:a469bbd294b5 38 - powerstep.hpp
nerit 3:a469bbd294b5 39 - watchdog.cpp
nerit 3:a469bbd294b5 40 - watchdog.h
nerit 8:310f9e4eac7b 41 ED UTILIZZA LE LIBRERIE STANDARD MBED PIU'
nerit 3:a469bbd294b5 42 UNA LIBRERIA MODIFICATA E DEDICATA PER IL CAN
nerit 3:a469bbd294b5 43 UNA LIBRERIA DEDICATA PER IL DRIVER STEPPER
nerit 3:a469bbd294b5 44 *********************
nerit 3:a469bbd294b5 45 LA MACCHINA UTILIZZA SEMPRE 2 SOLI SENSORI; UNO PER SENTIRE LE CELLE DI CARICO SEME ED UNO PER SENTIRE I BECCHI DI SEMINA.
nerit 3:a469bbd294b5 46 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 3:a469bbd294b5 47 UN SENSORE AGGIUNTIVO SULL'ELEMENTO MASTER RILEVA LA VELOCITA' DI AVANZAMENTO
nerit 3:a469bbd294b5 48 UN SENSORE AGGIUNTIVO SULLA RUOTA DI SEMINA RILEVA LA ROTAZIONE DELLA RUOTA STESSA ATTRAVERSO FORI PRESENTI SUL DISCO DI SEMINA
nerit 3:a469bbd294b5 49 *********************
nerit 3:a469bbd294b5 50 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 8:310f9e4eac7b 51 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 3:a469bbd294b5 52 PROPRIO PERCHE' L'AVANZAMANETO NEL TERRENO IMPRIME UNA VELOCITA' PROPRIA AL BECCO E QUINDI ANCHE ALLA RUOTA.
nerit 3:a469bbd294b5 53 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 3:a469bbd294b5 54 IL VALORE DI RIFERIMENTO DELL'ANALOGICA DI INGRESSO VIENE AGGIORNATO OGNI VOLTA CHE LA RUOTA DI SEMINA E' FERMA
nerit 3:a469bbd294b5 55 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 3:a469bbd294b5 56 LA FASE VIENE DETERMINATA DAL PASSAGGIO DEI BECCHI SUL SENSORE RELATIVO.
nerit 3:a469bbd294b5 57 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 3:a469bbd294b5 58 SOPRATUTTO QUANDO I BECCHI SONO MOLTO DISTANZIATI.
nerit 3:a469bbd294b5 59 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 3:a469bbd294b5 60 TARATO SULLA REALE VELOCITA' DI ROTAZIONE DELLA RUOTA DI SEMINA.
nerit 3:a469bbd294b5 61 IL PRIMO ENCODER VIRTUALE SI OCCUPA DI DETERMINARE LA POSIZIONE FISICA DELLA RUOTA DI SEMINA E SI AZZERA AL PASSAGGIO DI OGNI BECCO.
nerit 3:a469bbd294b5 62 IL SECONDO VIENE AZZERATO DALL'IMPULSO DI FASE DEL PRIMO ENCODER DETERMINATO DAI VALORI IMPOSTI SUL TERMINALE TRITECNICA
nerit 3:a469bbd294b5 63 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 3:a469bbd294b5 64 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 3:a469bbd294b5 65 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 3:a469bbd294b5 66 DI CAMBIARE L'ANGOLO DI ANTICIPO DI RILASCIO DEL SEME IN FUNZIONE DELLA VELOCITA' E RECUPERARE COSI' IL TEMPO DI VOLO DEL SEME.
nerit 3:a469bbd294b5 67 IL TAMBURO HA DUE TIPI DI FUNZIONAMENTO: CONTINUO E AD IMPULSI. E' SELEZIONABILE IN FUNZIONE DELLA VELOCITA' E DEL TIPO DI DISTRIBUTORE MONTATO.
nerit 3:a469bbd294b5 68 **********************
nerit 3:a469bbd294b5 69 TUTTI I VALORI, CELLE, BECCHI, IMPULSI VELOCITA', ANCGOLO DI AVVIO, FASE DI SEMINA, ECC.. SONO IMPOSTABILI DA PANNELLO OPERATORE
nerit 3:a469bbd294b5 70 I DATI SONO SCAMBIATI CON IL PANNELLO OPERATORE E CON GLI ALTRI MODULI ATTRAVERSO RETE CAN CON PROTOCOLLO FREESTYLE ATTRAVERSO INDIRIZZAMENTI DEDICATI
nerit 3:a469bbd294b5 71 AL MOMENTO NON E' POSSIBILE ATTRIBUIRE L'INIDIRIZZO BASE DELL'ELEMENTO DA TERMINALE OPERATORE MA SOLO IN FASE DI COMPILAZIONE DEL FIRMWARE.
nerit 3:a469bbd294b5 72 **********************
nerit 3:a469bbd294b5 73 ALTRE SEZIONI RIGUARDANO LA GENERAZIONE DEGLI ALLARMI, LA COMUNICAZIONE CAN, LA SIMULAZIONE DI LAVORO, LA GESTIONE DELLA DIAGNOSI ECC..
nerit 3:a469bbd294b5 74 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 3:a469bbd294b5 75 LO STEPPER SEGUE IL DC.
nerit 3:a469bbd294b5 76 **********************
nerit 3:a469bbd294b5 77 IN FASE DI ACCENSIONE ED OGNI QUALVOLTA SI ARRIVA A VELOCITA' ZERO, LA MACCHINA ESEGUE UN CICLO DI AZZERAMENTO
nerit 3:a469bbd294b5 78 NON ESISTE PULSANTE DI MARCIA/STOP; E' SEMPRE ATTIVA.
nerit 3:a469bbd294b5 79 **********************
nerit 3:a469bbd294b5 80 NEL PROGRAMMA E' PRESENTE UNA SEZIONE DI TEST FISICO DELLA SCHEDA ATTIVABILE SOLO IN FASE DI COMPILAZIONE
nerit 3:a469bbd294b5 81 **********************
nerit 3:a469bbd294b5 82 ALTRE FUNZIONI: PRECARICAMENTO DEL TAMBURO
nerit 3:a469bbd294b5 83 AZZERAMENTO MANUALE
nerit 3:a469bbd294b5 84 STATISTICA DI SEMINA (CONTA LE CELLE)
nerit 3:a469bbd294b5 85 */
nerit 3:a469bbd294b5 86 //********************************************************************************************************************
nerit 3:a469bbd294b5 87 //********************************************************************************************************************
nerit 3:a469bbd294b5 88 #include "main.hpp"
nerit 8:310f9e4eac7b 89 /* Helper header files. */
nerit 8:310f9e4eac7b 90 #include "DevSPI.h"
nerit 8:310f9e4eac7b 91 /* Component specific header files. */
nerit 8:310f9e4eac7b 92 #include "PowerStep01.h"
nerit 3:a469bbd294b5 93 #include "timeandtick.hpp"
nerit 3:a469bbd294b5 94 #include "canbus.hpp"
nerit 3:a469bbd294b5 95 #include "watchdog.h"
nerit 3:a469bbd294b5 96 #include "iodefinition.hpp"
nerit 3:a469bbd294b5 97 #include "parameters.hpp"
nerit 3:a469bbd294b5 98 #include "variables.hpp"
nerit 8:310f9e4eac7b 99 #include "powerstep.hpp"
nerit 3:a469bbd294b5 100 //********************************************************************************************************************
nerit 3:a469bbd294b5 101 //********************************************************************************************************************
nerit 8:310f9e4eac7b 102 #if defined(runnerTos)
nerit 8:310f9e4eac7b 103 Thread thread;
nerit 8:310f9e4eac7b 104 #endif
nerit 3:a469bbd294b5 105
nerit 3:a469bbd294b5 106 /* Variables -----------------------------------------------------------------*/
nerit 3:a469bbd294b5 107
nerit 3:a469bbd294b5 108 /* Functions -----------------------------------------------------------------*/
nerit 3:a469bbd294b5 109
nerit 3:a469bbd294b5 110 /**
nerit 3:a469bbd294b5 111 * @brief This is an example of user handler for the flag interrupt.
nerit 3:a469bbd294b5 112 * @param None
nerit 3:a469bbd294b5 113 * @retval None
nerit 3:a469bbd294b5 114 * @note If needed, implement it, and then attach and enable it:
nerit 3:a469bbd294b5 115 * + motor->attach_flag_irq(&my_flag_irq_handler);
nerit 3:a469bbd294b5 116 * + motor->enable_flag_irq();
nerit 3:a469bbd294b5 117 * To disable it:
nerit 3:a469bbd294b5 118 * + motor->DisbleFlagIRQ();
nerit 3:a469bbd294b5 119 */
nerit 3:a469bbd294b5 120 void my_flag_irq_handler(void)
nerit 3:a469bbd294b5 121 {
nerit 3:a469bbd294b5 122 /* Set ISR flag. */
nerit 3:a469bbd294b5 123 motor->isrFlag = TRUE;
nerit 3:a469bbd294b5 124 /* Get the value of the status register. */
nerit 3:a469bbd294b5 125 unsigned int statusRegister = motor->get_status();
nerit 10:9e70619e97ab 126 #if defined(pcSerial)
nerit 10:9e70619e97ab 127 pc.printf(" WARNING: \"FLAG\" interrupt triggered.\r\n");
nerit 10:9e70619e97ab 128 #endif
nerit 3:a469bbd294b5 129 /* Check SW_F flag: if not set, the SW input is opened */
nerit 3:a469bbd294b5 130 if ((statusRegister & POWERSTEP01_STATUS_SW_F ) != 0) {
nerit 10:9e70619e97ab 131 #if defined(pcSerial)
nerit 8:310f9e4eac7b 132 pc.printf(" SW closed (connected to ground).\r\n");
nerit 10:9e70619e97ab 133 #endif
nerit 3:a469bbd294b5 134 }
nerit 3:a469bbd294b5 135 /* Check SW_EN bit */
nerit 3:a469bbd294b5 136 if ((statusRegister & POWERSTEP01_STATUS_SW_EVN) == POWERSTEP01_STATUS_SW_EVN) {
nerit 10:9e70619e97ab 137 #if defined(pcSerial)
nerit 8:310f9e4eac7b 138 pc.printf(" SW turn_on event.\r\n");
nerit 10:9e70619e97ab 139 #endif
nerit 3:a469bbd294b5 140 }
nerit 3:a469bbd294b5 141 /* Check Command Error flag: if set, the command received by SPI can't be */
nerit 3:a469bbd294b5 142 /* performed. This occurs for instance when a move command is sent to the */
nerit 3:a469bbd294b5 143 /* Powerstep01 while it is already running */
nerit 3:a469bbd294b5 144 if ((statusRegister & POWERSTEP01_STATUS_CMD_ERROR) == POWERSTEP01_STATUS_CMD_ERROR) {
nerit 10:9e70619e97ab 145 #if defined(pcSerial)
nerit 8:310f9e4eac7b 146 pc.printf(" Non-performable command detected.\r\n");
nerit 10:9e70619e97ab 147 #endif
nerit 8:310f9e4eac7b 148 }
nerit 3:a469bbd294b5 149 /* Check UVLO flag: if not set, there is an undervoltage lock-out */
nerit 3:a469bbd294b5 150 if ((statusRegister & POWERSTEP01_STATUS_UVLO)==0) {
nerit 10:9e70619e97ab 151 #if defined(pcSerial)
nerit 8:310f9e4eac7b 152 pc.printf(" undervoltage lock-out.\r\n");
nerit 10:9e70619e97ab 153 #endif
nerit 8:310f9e4eac7b 154 }
nerit 3:a469bbd294b5 155 /* Check thermal STATUS flags: if set, the thermal status is not normal */
nerit 3:a469bbd294b5 156 if ((statusRegister & POWERSTEP01_STATUS_TH_STATUS)!=0) {
nerit 8:310f9e4eac7b 157 //thermal status: 1: Warning, 2: Bridge shutdown, 3: Device shutdown
nerit 10:9e70619e97ab 158 #if defined(pcSerial)
nerit 8:310f9e4eac7b 159 pc.printf(" Thermal status: %d.\r\n", (statusRegister & POWERSTEP01_STATUS_TH_STATUS)>>11);
nerit 10:9e70619e97ab 160 #endif
nerit 8:310f9e4eac7b 161 }
nerit 3:a469bbd294b5 162 /* Check OCD flag: if not set, there is an overcurrent detection */
nerit 3:a469bbd294b5 163 if ((statusRegister & POWERSTEP01_STATUS_OCD)==0) {
nerit 10:9e70619e97ab 164 #if defined(pcSerial)
nerit 8:310f9e4eac7b 165 pc.printf(" Overcurrent detection.\r\n");
nerit 10:9e70619e97ab 166 #endif
nerit 3:a469bbd294b5 167 }
nerit 3:a469bbd294b5 168 /* Reset ISR flag. */
nerit 3:a469bbd294b5 169 motor->isrFlag = FALSE;
nerit 3:a469bbd294b5 170 }
nerit 3:a469bbd294b5 171
nerit 3:a469bbd294b5 172 /**
nerit 3:a469bbd294b5 173 * @brief This is an example of error handler.
nerit 3:a469bbd294b5 174 * @param[in] error Number of the error
nerit 3:a469bbd294b5 175 * @retval None
nerit 3:a469bbd294b5 176 * @note If needed, implement it, and then attach it:
nerit 3:a469bbd294b5 177 * + motor->attach_error_handler(&my_error_handler);
nerit 3:a469bbd294b5 178 */
nerit 3:a469bbd294b5 179 void my_error_handler(uint16_t error)
nerit 3:a469bbd294b5 180 {
nerit 3:a469bbd294b5 181 /* Printing to the console. */
nerit 10:9e70619e97ab 182 #if defined(pcSerial)
nerit 10:9e70619e97ab 183 pc.printf("Error %d detected\r\n\n", error);
nerit 10:9e70619e97ab 184 #endif
nerit 8:310f9e4eac7b 185
nerit 3:a469bbd294b5 186 /* Infinite loop */
nerit 3:a469bbd294b5 187 //while (true) {
nerit 8:310f9e4eac7b 188 //}
nerit 3:a469bbd294b5 189 }
nerit 8:310f9e4eac7b 190 //*******************************************************************************
nerit 8:310f9e4eac7b 191 // FREE RUNNING RTOS THREAD FOR DRUM STEPPER POSITION READING
nerit 8:310f9e4eac7b 192 //*******************************************************************************
nerit 8:310f9e4eac7b 193 #if defined(runner)
nerit 8:310f9e4eac7b 194 void step_Reading(){
nerit 8:310f9e4eac7b 195 //while(true){
nerit 8:310f9e4eac7b 196 /* Get current position of device and print to the console */
nerit 8:310f9e4eac7b 197 TBpassPosition= (uint32_t) motor->get_position();
nerit 8:310f9e4eac7b 198 if (TBpassPosition >= TBoldPosition){
nerit 8:310f9e4eac7b 199 TBactualPosition= ((TBpassPosition-TBoldPosition)*TBreductionRatio);//*10;
nerit 8:310f9e4eac7b 200 #if defined(pcSerial)
nerit 8:310f9e4eac7b 201 #if defined(rtosData)
nerit 8:310f9e4eac7b 202 printf(" 1 Position: %d TBpass: %d Tbold: %d \r\n", TBactualPosition, TBpassPosition, TBoldPosition);
nerit 8:310f9e4eac7b 203 #endif
nerit 8:310f9e4eac7b 204 #endif
nerit 8:310f9e4eac7b 205 }else{
nerit 8:310f9e4eac7b 206 TBactualPosition=((((2097152-TBoldPosition)+TBpassPosition))*TBreductionRatio);//*10;
nerit 8:310f9e4eac7b 207 #if defined(pcSerial)
nerit 8:310f9e4eac7b 208 #if defined(rtosData)
nerit 8:310f9e4eac7b 209 printf(" 2 Position: %d TBpass: %d Tbold: %d \r\n", TBactualPosition, TBpassPosition, TBoldPosition);
nerit 8:310f9e4eac7b 210 #endif
nerit 8:310f9e4eac7b 211 #endif
nerit 8:310f9e4eac7b 212 }
nerit 8:310f9e4eac7b 213 //wait_us(50); // 50 mS di intervallo lettura
nerit 8:310f9e4eac7b 214 //}
nerit 8:310f9e4eac7b 215 }
nerit 8:310f9e4eac7b 216 #endif
nerit 3:a469bbd294b5 217 //*******************************************************************************
nerit 3:a469bbd294b5 218 //********************************************************************************************************************
nerit 3:a469bbd294b5 219 // ------------------------------------------------------------------------------------------------------------------------------------------------------------------
nerit 3:a469bbd294b5 220 // TASK SECTION
nerit 3:a469bbd294b5 221 // ------------------------------------------------------------------------------------------------------------------------------------------------------------------
nerit 3:a469bbd294b5 222 //************************************************************************
nerit 8:310f9e4eac7b 223 void aggioVelocita()
nerit 8:310f9e4eac7b 224 {
nerit 8:310f9e4eac7b 225 realGiroSD = seedPerimeter / speedOfSeedWheel;
nerit 17:9629eb019892 226 tempoBecco = realGiroSD*44.4444f; //(realGiroSD/360.0f)*16000.0f;
nerit 8:310f9e4eac7b 227 frequenzaReale = fixedStepGiroSD/realGiroSD;
nerit 8:310f9e4eac7b 228 semiPeriodoReale = (1000000.0f/frequenzaReale);
nerit 8:310f9e4eac7b 229 seedWheelRPM = (speedOfSeedWheel)*K_WheelRPM ; // calcola i giri al minuto della ruota di semina 7.37 31,75
nerit 8:310f9e4eac7b 230 #if defined(runner)
nerit 10:9e70619e97ab 231 TBrpm = seedWheelRPM*rapportoRuote; // 5.896 31,75
nerit 9:7f02256f6e8f 232 #if defined(Zucca)
nerit 9:7f02256f6e8f 233 TBperiod=5.2f*TBrpm*2.0f; //prova dopo test con contagiri
nerit 9:7f02256f6e8f 234 #else
nerit 17:9629eb019892 235 if (cellsNumber<8.0f){
nerit 17:9629eb019892 236 TBperiod=4.8f*TBrpm; //prova dopo test con contagiri
nerit 17:9629eb019892 237 }else{
nerit 17:9629eb019892 238 TBperiod=5.2f*TBrpm; //prova dopo test con contagiri
nerit 17:9629eb019892 239 }
nerit 9:7f02256f6e8f 240 #endif
nerit 8:310f9e4eac7b 241 #else
nerit 10:9e70619e97ab 242 TBfrequency = (TBrpm*K_TBfrequency); // 130Hz a 0,29Mts 1397,00 a 1,25mt/s con 15 becchi e 15 celle
nerit 8:310f9e4eac7b 243 TBperiod=1000000.0f/TBfrequency; // 715uS
nerit 8:310f9e4eac7b 244 #endif
nerit 8:310f9e4eac7b 245
nerit 8:310f9e4eac7b 246 }
nerit 3:a469bbd294b5 247 //************************************************************************
nerit 3:a469bbd294b5 248 // rise of seed speed 25 pulse sensor
nerit 10:9e70619e97ab 249 void sd25Fall(){
nerit 3:a469bbd294b5 250 timeHole=metalTimer.read_ms();
nerit 10:9e70619e97ab 251 double memo_TimeHole= (memoTimeHole + (double)timeHole)/ 2.0f;
nerit 10:9e70619e97ab 252 memoTimeHole = (double)timeHole;
nerit 3:a469bbd294b5 253 metalTimer.reset();
nerit 8:310f9e4eac7b 254 if (speedFromPick==0) {
nerit 10:9e70619e97ab 255 speedOfSeedWheel=((seedPerimeter/25.0f)/memo_TimeHole)*1000.0f; //mtS
nerit 3:a469bbd294b5 256 }
nerit 6:e8c18f0f399a 257 #if defined(pcSerial)
nerit 6:e8c18f0f399a 258 #if defined(checkLoop)
nerit 6:e8c18f0f399a 259 pc.printf("1\n");
nerit 6:e8c18f0f399a 260 #endif
nerit 6:e8c18f0f399a 261 #endif
nerit 3:a469bbd294b5 262 }
nerit 10:9e70619e97ab 263 //************************************************************************
nerit 3:a469bbd294b5 264 // rise of seed speed motor encoder
nerit 3:a469bbd294b5 265 void encoRise(){
nerit 3:a469bbd294b5 266 timeHole=metalTimer.read_us();
nerit 10:9e70619e97ab 267 double memo_TimeHole= (memoTimeHole + (double)timeHole)/ 2.0f;
nerit 10:9e70619e97ab 268 memoTimeHole = (double)timeHole;
nerit 3:a469bbd294b5 269 metalTimer.reset();
nerit 8:310f9e4eac7b 270 if (encoder==true) {
nerit 13:d1030d4e51a8 271 speedOfSeedWheel=((seedPerimeter/seedWheelDcPulse)/memo_TimeHole)*1000000.0f; //mtS
nerit 3:a469bbd294b5 272 pulseRised2=1;
nerit 13:d1030d4e51a8 273 dcEncoderCnt++;
nerit 13:d1030d4e51a8 274 double sincronizza = frazioneImpulsi * dcEncoderCnt;
nerit 13:d1030d4e51a8 275 prePosSD=500+(uint32_t)sincronizza; // preposizionamento SD
nerit 13:d1030d4e51a8 276 #if defined(speedMaster)
nerit 13:d1030d4e51a8 277 if (quinconceActive==0) {
nerit 13:d1030d4e51a8 278 posForQuinc=500+(uint32_t)sincronizza;
nerit 13:d1030d4e51a8 279 }
nerit 13:d1030d4e51a8 280 #endif
nerit 3:a469bbd294b5 281 }
nerit 6:e8c18f0f399a 282 #if defined(pcSerial)
nerit 6:e8c18f0f399a 283 #if defined(checkLoop)
nerit 6:e8c18f0f399a 284 pc.printf("2\n");
nerit 6:e8c18f0f399a 285 #endif
nerit 6:e8c18f0f399a 286 #endif
nerit 8:310f9e4eac7b 287 aggioVelocita();
nerit 3:a469bbd294b5 288 }
nerit 6:e8c18f0f399a 289 //**************************************************
nerit 3:a469bbd294b5 290 // rise of seed presence sensor
nerit 3:a469bbd294b5 291 void seedSensorTask(){
nerit 3:a469bbd294b5 292 seedSee=1;
nerit 6:e8c18f0f399a 293 #if defined(pcSerial)
nerit 6:e8c18f0f399a 294 #if defined(checkLoop)
nerit 6:e8c18f0f399a 295 pc.printf("3\n");
nerit 6:e8c18f0f399a 296 #endif
nerit 6:e8c18f0f399a 297 #endif
nerit 3:a469bbd294b5 298 }
nerit 3:a469bbd294b5 299 //**************************************************
nerit 3:a469bbd294b5 300 // generate speed clock when speed is simulated from Tritecnica display
nerit 8:310f9e4eac7b 301 void speedSimulationClock(){
nerit 3:a469bbd294b5 302 lastPulseRead=speedTimer.read_us();
nerit 3:a469bbd294b5 303 oldLastPulseRead=lastPulseRead;
nerit 3:a469bbd294b5 304 speedTimer.reset();
nerit 3:a469bbd294b5 305 pulseRised=1;
nerit 3:a469bbd294b5 306 speedFilter.reset();
nerit 6:e8c18f0f399a 307 #if defined(pcSerial)
nerit 6:e8c18f0f399a 308 #if defined(checkLoop)
nerit 6:e8c18f0f399a 309 pc.printf("4\n");
nerit 6:e8c18f0f399a 310 #endif
nerit 6:e8c18f0f399a 311 #endif
nerit 3:a469bbd294b5 312 }
nerit 3:a469bbd294b5 313 //*******************************************************
nerit 3:a469bbd294b5 314 // interrupt task for tractor speed reading
nerit 3:a469bbd294b5 315 //*******************************************************
nerit 3:a469bbd294b5 316 void tractorReadSpeed(){
nerit 8:310f9e4eac7b 317 if ((oldTractorSpeedRead==0)) {
nerit 3:a469bbd294b5 318 lastPulseRead=speedTimer.read_us();
nerit 3:a469bbd294b5 319 oldLastPulseRead=lastPulseRead;
nerit 3:a469bbd294b5 320 speedTimer.reset();
nerit 3:a469bbd294b5 321 pulseRised=1;
nerit 3:a469bbd294b5 322 oldTractorSpeedRead=1;
nerit 3:a469bbd294b5 323 }
nerit 8:310f9e4eac7b 324 speedClock=1;
nerit 3:a469bbd294b5 325 speedFilter.reset();
nerit 10:9e70619e97ab 326 cntSpeedError=0;
nerit 6:e8c18f0f399a 327 #if defined(pcSerial)
nerit 6:e8c18f0f399a 328 #if defined(checkLoop)
nerit 6:e8c18f0f399a 329 pc.printf("5\n");
nerit 6:e8c18f0f399a 330 #endif
nerit 6:e8c18f0f399a 331 #endif
nerit 3:a469bbd294b5 332 }
nerit 3:a469bbd294b5 333 //*******************************************************
nerit 8:310f9e4eac7b 334 void speedMediaCalc()
nerit 8:310f9e4eac7b 335 {
nerit 3:a469bbd294b5 336 double lastPd=(double) lastPulseRead/1000.0f;
nerit 3:a469bbd294b5 337 pulseSpeedInterval = (mediaSpeed[0]+lastPd)/2.0f;
nerit 8:310f9e4eac7b 338 if (enableSimula==1) {
nerit 3:a469bbd294b5 339 double TMT = (double)(speedSimula) * 100.0f /3600.0f;
nerit 3:a469bbd294b5 340 pulseSpeedInterval = pulseDistance / TMT;
nerit 8:310f9e4eac7b 341 }
nerit 3:a469bbd294b5 342 mediaSpeed[0]=lastPd;
nerit 3:a469bbd294b5 343 OLDpulseSpeedInterval=pulseSpeedInterval;
nerit 10:9e70619e97ab 344 #if defined(pcSerial)
nerit 10:9e70619e97ab 345 #if defined(checkLoop)
nerit 10:9e70619e97ab 346 pc.printf("6\n");
nerit 10:9e70619e97ab 347 #endif
nerit 10:9e70619e97ab 348 #endif
nerit 8:310f9e4eac7b 349 }
bcostm 0:5701b41769fd 350
nerit 3:a469bbd294b5 351 //*******************************************************
nerit 3:a469bbd294b5 352 // clocked task for manage virtual encoder of seed wheel i/o
nerit 3:a469bbd294b5 353 //*******************************************************
nerit 3:a469bbd294b5 354 //*******************************************************
nerit 8:310f9e4eac7b 355 void step_SDPulseOut()
nerit 8:310f9e4eac7b 356 {
nerit 3:a469bbd294b5 357 SDactualPosition++;
nerit 3:a469bbd294b5 358 prePosSD++;
nerit 10:9e70619e97ab 359 #if defined(speedMaster)
nerit 10:9e70619e97ab 360 posForQuinc++;
nerit 10:9e70619e97ab 361 #endif
nerit 10:9e70619e97ab 362 #if defined(pcSerial)
nerit 10:9e70619e97ab 363 #if defined(checkLoop)
nerit 10:9e70619e97ab 364 pc.printf("7\n");
nerit 10:9e70619e97ab 365 #endif
nerit 10:9e70619e97ab 366 #endif
nerit 3:a469bbd294b5 367 }
nerit 3:a469bbd294b5 368 //*******************************************************
nerit 8:310f9e4eac7b 369 void step_TBPulseOut()
nerit 8:310f9e4eac7b 370 {
nerit 3:a469bbd294b5 371 TBmotorStepOut=!TBmotorStepOut;
nerit 8:310f9e4eac7b 372 if (TBmotorStepOut==0) {
nerit 8:310f9e4eac7b 373 if (TBmotorDirecti==TBforward) {
nerit 3:a469bbd294b5 374 TBactualPosition++;
nerit 3:a469bbd294b5 375 }
nerit 3:a469bbd294b5 376 }
nerit 10:9e70619e97ab 377 #if defined(pcSerial)
nerit 10:9e70619e97ab 378 #if defined(stepTamb)
nerit 10:9e70619e97ab 379 pc.printf("step\n");
nerit 10:9e70619e97ab 380 #endif
nerit 10:9e70619e97ab 381 #endif
nerit 6:e8c18f0f399a 382 /*
nerit 6:e8c18f0f399a 383 #if defined(pcSerial)
nerit 6:e8c18f0f399a 384 #if defined(checkLoop)
nerit 6:e8c18f0f399a 385 pc.printf("8\n");
nerit 6:e8c18f0f399a 386 #endif
nerit 6:e8c18f0f399a 387 #endif
nerit 6:e8c18f0f399a 388 */
nerit 3:a469bbd294b5 389 }
nerit 3:a469bbd294b5 390 //*******************************************************
nerit 8:310f9e4eac7b 391 void invertiLo()
nerit 8:310f9e4eac7b 392 {
nerit 8:310f9e4eac7b 393 if (TBmotorDirecti==TBreverse) {
nerit 3:a469bbd294b5 394 TBmotorDirecti=TBforward;
nerit 8:310f9e4eac7b 395 #if !defined(runner)
nerit 9:7f02256f6e8f 396 #if defined(Zucca)
nerit 9:7f02256f6e8f 397 motor->step_clock_mode_enable(StepperMotor::BWD);
nerit 9:7f02256f6e8f 398 #else
nerit 9:7f02256f6e8f 399 motor->step_clock_mode_enable(StepperMotor::FWD);
nerit 9:7f02256f6e8f 400 #endif
nerit 8:310f9e4eac7b 401 #endif
nerit 8:310f9e4eac7b 402 } else {
nerit 3:a469bbd294b5 403 TBmotorDirecti=TBreverse;
nerit 8:310f9e4eac7b 404 #if !defined(runner)
nerit 9:7f02256f6e8f 405 #if defined(Zucca)
nerit 9:7f02256f6e8f 406 motor->step_clock_mode_enable(StepperMotor::FWD);
nerit 9:7f02256f6e8f 407 #else
nerit 9:7f02256f6e8f 408 motor->step_clock_mode_enable(StepperMotor::BWD);
nerit 9:7f02256f6e8f 409 #endif
nerit 8:310f9e4eac7b 410 #endif
nerit 3:a469bbd294b5 411 }
nerit 3:a469bbd294b5 412 #if defined(pcSerial)
nerit 3:a469bbd294b5 413 #if defined(inversione)
nerit 3:a469bbd294b5 414 pc.printf("cambio M %d\n",cambiaStep);
nerit 3:a469bbd294b5 415 pc.printf("posizione %d \n",TBactualPosition);
nerit 3:a469bbd294b5 416 #endif
nerit 3:a469bbd294b5 417 #endif
nerit 6:e8c18f0f399a 418 #if defined(pcSerial)
nerit 6:e8c18f0f399a 419 #if defined(checkLoop)
nerit 6:e8c18f0f399a 420 pc.printf("9\n");
nerit 6:e8c18f0f399a 421 #endif
nerit 6:e8c18f0f399a 422 #endif
nerit 3:a469bbd294b5 423 }
nerit 3:a469bbd294b5 424 //*******************************************************
nerit 3:a469bbd294b5 425 // aggiornamento parametri di lavoro fissi e da Tritecnica
nerit 8:310f9e4eac7b 426 void aggiornaParametri()
nerit 8:310f9e4eac7b 427 {
nerit 3:a469bbd294b5 428 speedPerimeter = Pi * speedWheelDiameter ; // perimeter of speed wheel
nerit 3:a469bbd294b5 429 pulseDistance = (speedPerimeter / speedWheelPulse)*1000.0f; // linear space between speed wheel pulse
nerit 3:a469bbd294b5 430 seedPerimeter = Pi * (seedWheelDiameter-(deepOfSeed*2.0f)); // perimeter of seed wheel
nerit 3:a469bbd294b5 431 intraPickDistance = seedPerimeter/pickNumber;
nerit 3:a469bbd294b5 432 K_WheelRPM = 60.0f/seedPerimeter; // calcola il K per i giri al minuto della ruota di semina
nerit 17:9629eb019892 433 //K_WhellFrequency = (seedWheelMotorSteps*SDreductionRatio)/60.0f; // calcola il K per la frequenza di comando del motore di semina
nerit 3:a469bbd294b5 434 rapportoRuote = pickNumber/cellsNumber; // calcola il rapporto tra il numero di becchi ed il numero di celle
nerit 13:d1030d4e51a8 435 SDsectorStep = fixedStepGiroSD / pickNumber;
nerit 3:a469bbd294b5 436 TBsectorStep = (TBmotorSteps*TBreductionRatio)/cellsNumber;
nerit 13:d1030d4e51a8 437 seedWheelDcPulse=SDreductionRatio*dcPulseTurn;
nerit 13:d1030d4e51a8 438 frazioneImpulsi= (SDsectorStep/(seedWheelDcPulse/pickNumber));
nerit 8:310f9e4eac7b 439 #if defined(runner)
nerit 9:7f02256f6e8f 440 #if defined(Zucca)
nerit 9:7f02256f6e8f 441 KcorT = (SDsectorStep/TBsectorStep)*2.0f;
nerit 9:7f02256f6e8f 442 #else
nerit 9:7f02256f6e8f 443 KcorT = (SDsectorStep/TBsectorStep)/2.0f;
nerit 9:7f02256f6e8f 444 #endif
nerit 8:310f9e4eac7b 445 #else
nerit 8:310f9e4eac7b 446 KcorT = (SDsectorStep/TBsectorStep);///2.0f;
nerit 8:310f9e4eac7b 447 #endif
nerit 3:a469bbd294b5 448 angoloFase=angoloPh;
nerit 3:a469bbd294b5 449 avvioGradi=angoloAv;
nerit 3:a469bbd294b5 450 stepGrado=fixedStepGiroSD/360.0f;
nerit 3:a469bbd294b5 451 TBdeltaStep=(fixedStepGiroSD/pickNumber)+(stepGrado*avvioGradi);
nerit 8:310f9e4eac7b 452 TBfaseStep = (stepGrado*angoloFase);
nerit 3:a469bbd294b5 453 TBgiroStep = TBmotorSteps*TBreductionRatio;
nerit 3:a469bbd294b5 454 K_TBfrequency = TBgiroStep/60.0f; // 1600 * 1.65625f /60 = 44 44,00
nerit 3:a469bbd294b5 455 if (speedFromPick==1) {
nerit 3:a469bbd294b5 456 intraPickDistance = seedPerimeter/pickNumber;
nerit 8:310f9e4eac7b 457 } else {
nerit 3:a469bbd294b5 458 intraPickDistance = seedPerimeter/25.0f; // 25 è il numero di fori presenti nel disco di semina
nerit 3:a469bbd294b5 459 }
nerit 6:e8c18f0f399a 460 #if defined(pcSerial)
nerit 6:e8c18f0f399a 461 #if defined(checkLoop)
nerit 6:e8c18f0f399a 462 pc.printf("10\n");
nerit 6:e8c18f0f399a 463 #endif
nerit 6:e8c18f0f399a 464 #endif
nerit 3:a469bbd294b5 465 }
nerit 3:a469bbd294b5 466 //*******************************************************
nerit 8:310f9e4eac7b 467 void cambiaTB(double perio)
nerit 8:310f9e4eac7b 468 {
nerit 8:310f9e4eac7b 469 #if defined(runner)
nerit 8:310f9e4eac7b 470 // update TB frequency
nerit 8:310f9e4eac7b 471 double TBper=0.0f;
nerit 8:310f9e4eac7b 472 if (aspettaStart==0){
nerit 8:310f9e4eac7b 473 TBper=perio;
nerit 8:310f9e4eac7b 474 if (oldPeriodoTB!=TBper){
nerit 6:e8c18f0f399a 475 #if defined(pcSerial)
nerit 8:310f9e4eac7b 476 #if defined(TBperSo)
nerit 8:310f9e4eac7b 477 pc.printf("TBper: %f MtS: %f\n",TBper,tractorSpeed_MtS_timed);
nerit 6:e8c18f0f399a 478 #endif
nerit 6:e8c18f0f399a 479 #endif
nerit 9:7f02256f6e8f 480 #if defined(Zucca)
nerit 9:7f02256f6e8f 481 motor->run(StepperMotor::BWD,TBper);
nerit 9:7f02256f6e8f 482 #else
nerit 9:7f02256f6e8f 483 motor->run(StepperMotor::FWD,TBper);
nerit 9:7f02256f6e8f 484 #endif
nerit 8:310f9e4eac7b 485 oldPeriodoTB=TBper;
nerit 8:310f9e4eac7b 486 }
nerit 8:310f9e4eac7b 487 }
nerit 8:310f9e4eac7b 488 #else
nerit 8:310f9e4eac7b 489 // update TB frequency
nerit 8:310f9e4eac7b 490 double limite=500.0f;
nerit 8:310f9e4eac7b 491 double TBper=0.0f;
nerit 8:310f9e4eac7b 492 double scala =2.0f;
nerit 8:310f9e4eac7b 493 if (aspettaStart==0) {
nerit 8:310f9e4eac7b 494 if (perio<limite) {
nerit 8:310f9e4eac7b 495 perio=limite;
nerit 8:310f9e4eac7b 496 }
nerit 8:310f9e4eac7b 497 TBper=perio/scala;
nerit 8:310f9e4eac7b 498 if (oldPeriodoTB!=TBper) {
nerit 8:310f9e4eac7b 499 if (TBper >= (limite/2.0f)) {
nerit 8:310f9e4eac7b 500 #if defined(pcSerial)
nerit 8:310f9e4eac7b 501 #if defined(checkLoop)
nerit 8:310f9e4eac7b 502 pc.printf("11a\n");
nerit 8:310f9e4eac7b 503 pc.printf("11a TBper: %f \n",TBper);
nerit 8:310f9e4eac7b 504 #endif
nerit 6:e8c18f0f399a 505 #endif
nerit 8:310f9e4eac7b 506 if (TBper != NULL) {
nerit 9:7f02256f6e8f 507 #if defined(Zucca)
nerit 9:7f02256f6e8f 508 motor->step_clock_mode_enable(StepperMotor::BWD);
nerit 9:7f02256f6e8f 509 #else
nerit 9:7f02256f6e8f 510 motor->step_clock_mode_enable(StepperMotor::FWD);
nerit 9:7f02256f6e8f 511 #endif
nerit 8:310f9e4eac7b 512 TBticker.attach_us(&step_TBPulseOut,TBper); // clock time are milliseconds and attach seed motor stepper controls
nerit 8:310f9e4eac7b 513 }
nerit 8:310f9e4eac7b 514 } else {
nerit 8:310f9e4eac7b 515 #if defined(pcSerial)
nerit 8:310f9e4eac7b 516 #if defined(checkLoop)
nerit 8:310f9e4eac7b 517 pc.printf("11b\n");
nerit 8:310f9e4eac7b 518 #endif
nerit 6:e8c18f0f399a 519 #endif
nerit 8:310f9e4eac7b 520 TBticker.detach();
nerit 8:310f9e4eac7b 521 #if defined(pcSerial)
nerit 8:310f9e4eac7b 522 #if defined(loStop)
nerit 8:310f9e4eac7b 523 pc.printf("A1\n");
nerit 8:310f9e4eac7b 524 #endif
nerit 8:310f9e4eac7b 525 #endif
nerit 8:310f9e4eac7b 526 motor->soft_hiz();
nerit 8:310f9e4eac7b 527 }
nerit 8:310f9e4eac7b 528 oldPeriodoTB=TBper;
nerit 3:a469bbd294b5 529 }
nerit 3:a469bbd294b5 530 }
nerit 8:310f9e4eac7b 531 #endif
nerit 3:a469bbd294b5 532 }
nerit 3:a469bbd294b5 533 //*******************************************************
nerit 8:310f9e4eac7b 534 void seedCorrect()
nerit 8:310f9e4eac7b 535 {
nerit 3:a469bbd294b5 536 /*
nerit 3:a469bbd294b5 537 posError determina la posizione relativa di TB rispetto ad SD
nerit 8:310f9e4eac7b 538 la reale posizione di SD viene modificata in funzione della velocità per
nerit 3:a469bbd294b5 539 traslare la posizione relativa di TB. All'aumentare della velocità la posizione
nerit 3:a469bbd294b5 540 di SD viene incrementata così che TB acceleri per raggiungerla in modo da rilasciare il seme prima
nerit 3:a469bbd294b5 541 La taratura del sistema avviene determinando prima il valore di angoloFase alla minima velocità,
nerit 3:a469bbd294b5 542 poi, alla massima velocità, dovrebbe spostarsi la posizione relativa con una variabile proporzionale alla velocità, ma c'è un però.
nerit 3:a469bbd294b5 543 Il problema è che il momento di avvio determina una correzione dell'angolo di partenza del tamburo
nerit 3:a469bbd294b5 544 angolo che viene rideterminato ogni volta che il sensore becchi legge un transito.
nerit 3:a469bbd294b5 545 Di fatto c'è una concorrenza tra l'angolo di avvio determinato e la correzione di posizione relativa
nerit 3:a469bbd294b5 546 del tamburo. E' molto probabile che convenga modificare solo la posizione relativa e non anche l'angolo di avvio
nerit 3:a469bbd294b5 547 Ancora di più se viene eliminata la parte gestita da ciclata.
nerit 3:a469bbd294b5 548 In questo modo dovrebbe esserci solo un andamento in accelerazione di TB che viene poi eventualmente decelerato
nerit 3:a469bbd294b5 549 dal passaggio sul sensore di TB. Funzione corretta perchè il sincronismo tra i sensori genera l'inibizione della correzione
nerit 3:a469bbd294b5 550 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 3:a469bbd294b5 551 Appena il sensore di TB viene impegnato allora viene abilitato il controllo di fase del tamburo.
nerit 3:a469bbd294b5 552 Questo si traduce nel fatto che il controllo di posizione viene gestito solo all'interno di uno slot di semina in modo che
nerit 3:a469bbd294b5 553 il tamburo non risenta della condizione di reset della posizione di SD mentre lui è ancora nella fase precedente. Si fermerebbe.
nerit 8:310f9e4eac7b 554
nerit 3:a469bbd294b5 555 // La considerazione finale è che mantenendo l'angolo di avvio fisso e regolato sulla bassa velocità, intervenendo solo sulla correzione
nerit 3:a469bbd294b5 556 // di posizione in questa routine, dovrebbe essere possibile seminare correttamente a tutte le velocità regolando solo 2 parametri.
nerit 8:310f9e4eac7b 557 */
nerit 3:a469bbd294b5 558 /*
nerit 3:a469bbd294b5 559 SDsectorStep = (double)fixedStepGiroSD / (double)pickNumber;
nerit 3:a469bbd294b5 560 TBsectorStep = (TBmotorSteps*TBreductionRatio)/cellsNumber;
nerit 3:a469bbd294b5 561 KcorT = (SDsectorStep/TBsectorStep);
nerit 3:a469bbd294b5 562 angoloFase=angoloPh;
nerit 3:a469bbd294b5 563 stepGrado=fixedStepGiroSD/360.0f;
nerit 3:a469bbd294b5 564 avvioGradi = costante da terminale tritecnica
nerit 3:a469bbd294b5 565 TBdeltaStep=(fixedStepGiroSD/pickNumber)-(stepGrado*avvioGradi);
nerit 8:310f9e4eac7b 566 TBfaseStep = (stepGrado*angoloFase);
nerit 3:a469bbd294b5 567 */
nerit 8:310f9e4eac7b 568 if ((tractorSpeed_MtS_timed>0.01f)) {
nerit 8:310f9e4eac7b 569 if (inhibit==0) {
nerit 3:a469bbd294b5 570 double posError =0.0f;
nerit 3:a469bbd294b5 571 double posSD=((double)SDactualPosition)/KcorT;
nerit 8:310f9e4eac7b 572 posError = posSD - (double)TBactualPosition;
nerit 3:a469bbd294b5 573 // interviene sulla velocità di TB per raggiungere la corretta posizione relativa
nerit 8:310f9e4eac7b 574 if((lowSpeed==0)&&(aspettaStart==0)) {
nerit 4:de1b296e9757 575 double lowLim=-50.0f;
nerit 4:de1b296e9757 576 double higLim= 50.0f;
nerit 4:de1b296e9757 577 double divide= 100.0f;
nerit 8:310f9e4eac7b 578 if (pickNumber <= 5) {
nerit 4:de1b296e9757 579 lowLim=-500.0f;
nerit 4:de1b296e9757 580 higLim= 500.0f;
nerit 4:de1b296e9757 581 divide= 25.0f;
nerit 8:310f9e4eac7b 582 } else {
nerit 8:310f9e4eac7b 583 lowLim=-10.0f;
nerit 8:310f9e4eac7b 584 higLim= 130.0f;
nerit 4:de1b296e9757 585 divide= 100.0f;
nerit 4:de1b296e9757 586 }
nerit 8:310f9e4eac7b 587 if (posError>higLim) {
nerit 17:9629eb019892 588 posError=higLim;
nerit 17:9629eb019892 589 //posError=0.0f;
nerit 17:9629eb019892 590 //motor->soft_hiz();
nerit 8:310f9e4eac7b 591 }
nerit 8:310f9e4eac7b 592 if (posError<lowLim) {
nerit 8:310f9e4eac7b 593 posError=lowLim;
nerit 8:310f9e4eac7b 594 }
nerit 8:310f9e4eac7b 595 if ((posError >=1.0f)||(posError<=-1.0f)) {
nerit 8:310f9e4eac7b 596 #if defined(runner)
nerit 8:310f9e4eac7b 597 ePpos = periodo *(1.0f+ ((posError/divide)));
nerit 8:310f9e4eac7b 598 #else
nerit 8:310f9e4eac7b 599 ePpos = periodo /(1.0f+ ((posError/divide)));
nerit 8:310f9e4eac7b 600 #endif
nerit 3:a469bbd294b5 601 #if defined(pcSerial)
nerit 6:e8c18f0f399a 602 #if defined(checkLoop)
nerit 6:e8c18f0f399a 603 pc.printf("12a ePpos:%f\n",ePpos);
nerit 3:a469bbd294b5 604 #endif
nerit 3:a469bbd294b5 605 #endif
nerit 8:310f9e4eac7b 606 if (ePpos>0.0f) {
nerit 6:e8c18f0f399a 607 cambiaTB(ePpos);
nerit 8:310f9e4eac7b 608 } else {
nerit 6:e8c18f0f399a 609 cambiaTB(periodo);///2.0f);
nerit 6:e8c18f0f399a 610 }
nerit 6:e8c18f0f399a 611 }
nerit 6:e8c18f0f399a 612 #if defined(pcSerial)
nerit 6:e8c18f0f399a 613 #if defined(TBperS)
nerit 8:310f9e4eac7b 614 pc.printf("TBpos: %f SDpos: %f SDact: %f Err: %f Correggi: %f periodo: %f \n",(double)TBactualPosition,posSD,(double)SDactualPosition,posError,ePpos,periodo);
nerit 6:e8c18f0f399a 615 #endif
nerit 6:e8c18f0f399a 616 #endif
nerit 3:a469bbd294b5 617 }
nerit 3:a469bbd294b5 618 }
nerit 3:a469bbd294b5 619 }
nerit 10:9e70619e97ab 620 #if defined(pcSerial)
nerit 10:9e70619e97ab 621 #if defined(checkLoopa)
nerit 10:9e70619e97ab 622 pc.printf("12\n");
nerit 10:9e70619e97ab 623 #endif
nerit 10:9e70619e97ab 624 #endif
nerit 3:a469bbd294b5 625 }
nerit 3:a469bbd294b5 626 //*******************************************************
nerit 8:310f9e4eac7b 627 void videoUpdate()
nerit 8:310f9e4eac7b 628 {
nerit 8:310f9e4eac7b 629 for(int aa=0; aa<4; aa++) {
nerit 8:310f9e4eac7b 630 speedForDisplay[aa]=speedForDisplay[aa+1];
nerit 8:310f9e4eac7b 631 }
nerit 3:a469bbd294b5 632 speedForDisplay[4]=tractorSpeed_MtS_timed;
nerit 3:a469bbd294b5 633 totalSpeed=0.0f;
nerit 8:310f9e4eac7b 634 for (int aa=0; aa<5; aa++) {
nerit 8:310f9e4eac7b 635 totalSpeed += speedForDisplay[aa];
nerit 8:310f9e4eac7b 636 }
nerit 3:a469bbd294b5 637 totalSpeed = totalSpeed / 5.0f;
nerit 10:9e70619e97ab 638 #if defined(pcSerial)
nerit 10:9e70619e97ab 639 #if defined(SDreset)
nerit 10:9e70619e97ab 640 pc.printf("Fase: %d",fase);
nerit 10:9e70619e97ab 641 pc.printf(" PrePosSD: %d",prePosSD);
nerit 10:9e70619e97ab 642 pc.printf(" PosSD: %d",SDactualPosition);
nerit 10:9e70619e97ab 643 pc.printf(" speed: %f",tractorSpeed_MtS_timed);
nerit 10:9e70619e97ab 644 pc.printf(" Trigger: %d \n", trigRepos);
nerit 10:9e70619e97ab 645 #endif
nerit 10:9e70619e97ab 646 #endif
nerit 10:9e70619e97ab 647 #if defined(pcSerial)
nerit 10:9e70619e97ab 648 #if defined(checkLoop)
nerit 10:9e70619e97ab 649 pc.printf("13\n");
nerit 10:9e70619e97ab 650 #endif
nerit 10:9e70619e97ab 651 #endif
nerit 3:a469bbd294b5 652 }
nerit 3:a469bbd294b5 653 //*******************************************************
nerit 8:310f9e4eac7b 654 void ciclaTB()
nerit 8:310f9e4eac7b 655 {
nerit 8:310f9e4eac7b 656 if ((startCicloTB==1)&&(cicloTbinCorso==0)) {
nerit 6:e8c18f0f399a 657 #if defined(pcSerial)
nerit 6:e8c18f0f399a 658 #if defined(checkLoop)
nerit 6:e8c18f0f399a 659 pc.printf("14a TBperiod: %f\n",TBperiod);
nerit 6:e8c18f0f399a 660 #endif
nerit 6:e8c18f0f399a 661 #endif
nerit 8:310f9e4eac7b 662 #if defined(runner)
nerit 9:7f02256f6e8f 663 #if defined(Zucca)
nerit 9:7f02256f6e8f 664 motor->run(StepperMotor::BWD,TBperiod);
nerit 9:7f02256f6e8f 665 #else
nerit 9:7f02256f6e8f 666 motor->run(StepperMotor::FWD,TBperiod);
nerit 9:7f02256f6e8f 667 #endif
nerit 8:310f9e4eac7b 668 #else
nerit 8:310f9e4eac7b 669 if (TBperiod >= (250.0f*2.0f)) {
nerit 8:310f9e4eac7b 670 if (TBperiod != NULL) {
nerit 9:7f02256f6e8f 671 #if defined(Zucca)
nerit 9:7f02256f6e8f 672 motor->step_clock_mode_enable(StepperMotor::BWD);
nerit 9:7f02256f6e8f 673 #else
nerit 9:7f02256f6e8f 674 motor->step_clock_mode_enable(StepperMotor::FWD);
nerit 9:7f02256f6e8f 675 #endif
nerit 8:310f9e4eac7b 676 TBticker.attach_us(&step_TBPulseOut,TBperiod/2.0f); // clock time are milliseconds and attach seed motor stepper controls
nerit 8:310f9e4eac7b 677 }
nerit 6:e8c18f0f399a 678 }
nerit 8:310f9e4eac7b 679 #endif
nerit 8:310f9e4eac7b 680 cicloTbinCorso = 1;
nerit 8:310f9e4eac7b 681 startCicloTB=0;
nerit 3:a469bbd294b5 682 }
nerit 8:310f9e4eac7b 683 if ((loadDaCan==1)&&(loadDaCanInCorso==0)) {
nerit 6:e8c18f0f399a 684 #if defined(pcSerial)
nerit 6:e8c18f0f399a 685 #if defined(checkLoop)
nerit 6:e8c18f0f399a 686 pc.printf("14b\n");
nerit 6:e8c18f0f399a 687 #endif
nerit 6:e8c18f0f399a 688 #endif
nerit 8:310f9e4eac7b 689 #if defined(runner)
nerit 9:7f02256f6e8f 690 #if defined(Zucca)
nerit 9:7f02256f6e8f 691 motor->run(StepperMotor::BWD,50.0f);
nerit 9:7f02256f6e8f 692 #else
nerit 9:7f02256f6e8f 693 motor->run(StepperMotor::FWD,50.0f);
nerit 9:7f02256f6e8f 694 #endif
nerit 8:310f9e4eac7b 695 #else
nerit 9:7f02256f6e8f 696 #if defined(Zucca)
nerit 9:7f02256f6e8f 697 motor->step_clock_mode_enable(StepperMotor::BWD);
nerit 9:7f02256f6e8f 698 #else
nerit 9:7f02256f6e8f 699 motor->step_clock_mode_enable(StepperMotor::FWD);
nerit 9:7f02256f6e8f 700 #endif
nerit 8:310f9e4eac7b 701 TBticker.attach_us(&step_TBPulseOut,1000.0f); // clock time are milliseconds and attach seed motor stepper controls
nerit 8:310f9e4eac7b 702 #endif
nerit 3:a469bbd294b5 703 loadDaCanInCorso=1;
nerit 3:a469bbd294b5 704 stopCicloTB=0;
nerit 3:a469bbd294b5 705 }
nerit 8:310f9e4eac7b 706 if ((stopCicloTB==1)&&(TBactualPosition>5)) {
nerit 6:e8c18f0f399a 707 #if defined(pcSerial)
nerit 6:e8c18f0f399a 708 #if defined(checkLoop)
nerit 6:e8c18f0f399a 709 pc.printf("14c\n");
nerit 6:e8c18f0f399a 710 #endif
nerit 6:e8c18f0f399a 711 #endif
nerit 8:310f9e4eac7b 712 #if !defined(runner)
nerit 8:310f9e4eac7b 713 TBticker.detach();
nerit 8:310f9e4eac7b 714 #endif
nerit 6:e8c18f0f399a 715 #if defined(pcSerial)
nerit 6:e8c18f0f399a 716 #if defined(loStop)
nerit 6:e8c18f0f399a 717 pc.printf("A2\n");
nerit 6:e8c18f0f399a 718 #endif
nerit 6:e8c18f0f399a 719 #endif
nerit 3:a469bbd294b5 720 motor->soft_hiz();
nerit 3:a469bbd294b5 721 cicloTbinCorso = 0;
nerit 3:a469bbd294b5 722 stopCicloTB=0;
nerit 3:a469bbd294b5 723 loadDaCanInCorso=0;
nerit 3:a469bbd294b5 724 loadDaCan=0;
nerit 3:a469bbd294b5 725 }
nerit 3:a469bbd294b5 726 }
nerit 3:a469bbd294b5 727 // ------------------------------------------------------------------------------------------------------------------------------------------------------------------
nerit 8:310f9e4eac7b 728 void stepSetting()
nerit 8:310f9e4eac7b 729 {
nerit 3:a469bbd294b5 730 // Stepper driver init and set
nerit 3:a469bbd294b5 731 TBmotorRst=0; // reset stepper driver
nerit 3:a469bbd294b5 732 TBmotorDirecti=TBforward; // reset stepper direction
nerit 6:e8c18f0f399a 733 #if defined(pcSerial)
nerit 6:e8c18f0f399a 734 #if defined(checkLoop)
nerit 6:e8c18f0f399a 735 pc.printf("15\n");
nerit 6:e8c18f0f399a 736 #endif
nerit 6:e8c18f0f399a 737 #endif
nerit 8:310f9e4eac7b 738 TBmotorRst=1;
nerit 3:a469bbd294b5 739 }
nerit 3:a469bbd294b5 740 //****************************************
nerit 12:b0fc1d313813 741 void controllaCorrente(){
nerit 12:b0fc1d313813 742 correggiCorrente=1;
nerit 12:b0fc1d313813 743 }
nerit 12:b0fc1d313813 744 //****************************************
nerit 3:a469bbd294b5 745 void dcSetting(){
nerit 8:310f9e4eac7b 746 if ((speedFromPick==0)&&(encoder==false)) {
nerit 8:310f9e4eac7b 747 DcEncoder.rise(&sd25Fall);
nerit 8:310f9e4eac7b 748 }
nerit 8:310f9e4eac7b 749 if (encoder==true) {
nerit 8:310f9e4eac7b 750 DcEncoder.rise(&encoRise);
nerit 8:310f9e4eac7b 751 }
nerit 6:e8c18f0f399a 752 #if defined(pcSerial)
nerit 6:e8c18f0f399a 753 #if defined(checkLoop)
nerit 6:e8c18f0f399a 754 pc.printf("16\n");
nerit 6:e8c18f0f399a 755 #endif
nerit 6:e8c18f0f399a 756 #endif
nerit 3:a469bbd294b5 757 }
nerit 3:a469bbd294b5 758 //*******************************************************
nerit 8:310f9e4eac7b 759 void allarmi()
nerit 8:310f9e4eac7b 760 {
nerit 8:310f9e4eac7b 761 uint8_t alarmLowRegister1=0x00;
nerit 8:310f9e4eac7b 762 alarmLowRegister=0x00;
nerit 8:310f9e4eac7b 763 alarmHighRegister=0x80;
nerit 3:a469bbd294b5 764
nerit 8:310f9e4eac7b 765 //alarmLowRegister=alarmLowRegister+(all_semiFiniti*0x01); // manca il sensore
nerit 8:310f9e4eac7b 766 alarmLowRegister=alarmLowRegister+(all_pickSignal*0x02); // fatto
nerit 8:310f9e4eac7b 767 alarmLowRegister=alarmLowRegister+(all_cellSignal*0x04); // fatto
nerit 8:310f9e4eac7b 768 alarmLowRegister=alarmLowRegister+(all_lowBattery*0x08); // fatto
nerit 8:310f9e4eac7b 769 alarmLowRegister=alarmLowRegister+(all_overCurrDC*0x10); // fatto
nerit 8:310f9e4eac7b 770 alarmLowRegister=alarmLowRegister+(all_stopSistem*0x20); // verificarne la necessità
nerit 8:310f9e4eac7b 771 //alarmLowRegister=alarmLowRegister+(all_upElements*0x40); // manca il sensore
nerit 8:310f9e4eac7b 772 if (seedSensorEnable==true) {
nerit 8:310f9e4eac7b 773 alarmLowRegister=alarmLowRegister+(all_noSeedOnCe*0x80); // manca il sensore
nerit 8:310f9e4eac7b 774 }
nerit 3:a469bbd294b5 775
nerit 8:310f9e4eac7b 776 //alarmLowRegister1=alarmLowRegister1+(all_cfgnErrors*0x01); // da scrivere
nerit 8:310f9e4eac7b 777 alarmLowRegister1=alarmLowRegister1+(all_noDcRotati*0x02); // fatto
nerit 8:310f9e4eac7b 778 alarmLowRegister1=alarmLowRegister1+(all_noStepRota*0x04); // fatto
nerit 8:310f9e4eac7b 779 alarmLowRegister1=alarmLowRegister1+(all_speedError*0x08); // fatto
nerit 8:310f9e4eac7b 780 alarmLowRegister1=alarmLowRegister1+(all_noSpeedSen*0x10); // fatto
nerit 8:310f9e4eac7b 781 alarmLowRegister1=alarmLowRegister1+(all_no_Zeroing*0x20); // fatto
nerit 8:310f9e4eac7b 782 alarmLowRegister1=alarmLowRegister1+(all_genericals*0x40);
nerit 8:310f9e4eac7b 783 if (alarmLowRegister1 > 0) {
nerit 8:310f9e4eac7b 784 alarmHighRegister = 0x81;
nerit 8:310f9e4eac7b 785 alarmLowRegister = alarmLowRegister1;
nerit 8:310f9e4eac7b 786 }
nerit 3:a469bbd294b5 787
nerit 10:9e70619e97ab 788 #if defined(pcSerial)
nerit 10:9e70619e97ab 789 #if defined(VediAllarmi)
nerit 10:9e70619e97ab 790 if (all_pickSignal==1) {
nerit 10:9e70619e97ab 791 pc.printf("AllarmeBecchi\n");
nerit 10:9e70619e97ab 792 }
nerit 10:9e70619e97ab 793 if (all_cellSignal==1) {
nerit 10:9e70619e97ab 794 pc.printf("AllarmeCelle\n");
nerit 10:9e70619e97ab 795 }
nerit 10:9e70619e97ab 796 if (all_lowBattery==1) {
nerit 10:9e70619e97ab 797 pc.printf("AllarmeBassaCorrente\n");
nerit 10:9e70619e97ab 798 }
nerit 10:9e70619e97ab 799 if (all_overCurrDC==1) {
nerit 10:9e70619e97ab 800 pc.printf("AllarmeAltaCorrente\n");
nerit 10:9e70619e97ab 801 }
nerit 10:9e70619e97ab 802 if (all_stopSistem==1) {
nerit 10:9e70619e97ab 803 pc.printf("AllarmeStop\n");
nerit 10:9e70619e97ab 804 }
nerit 10:9e70619e97ab 805 if (all_noDcRotati==1) {
nerit 10:9e70619e97ab 806 pc.printf("AllarmeDCnoRotation\n");
nerit 10:9e70619e97ab 807 }
nerit 10:9e70619e97ab 808 if (all_noStepRota==1) {
nerit 10:9e70619e97ab 809 pc.printf("AllarmeNoStepRotation\n");
nerit 10:9e70619e97ab 810 }
nerit 10:9e70619e97ab 811 if (all_speedError==1) {
nerit 10:9e70619e97ab 812 pc.printf("AllarmeSpeedError\n");
nerit 10:9e70619e97ab 813 }
nerit 10:9e70619e97ab 814 if (all_noSpeedSen==1) {
nerit 10:9e70619e97ab 815 pc.printf("AllarmeNoSpeedSensor\n");
nerit 10:9e70619e97ab 816 }
nerit 10:9e70619e97ab 817 if (all_no_Zeroing==1) {
nerit 10:9e70619e97ab 818 pc.printf("AllarmeNoZero\n");
nerit 10:9e70619e97ab 819 }
nerit 10:9e70619e97ab 820 if (all_genericals==1) {
nerit 10:9e70619e97ab 821 pc.printf("AllarmeGenerico\n");
nerit 10:9e70619e97ab 822 }
nerit 10:9e70619e97ab 823 pc.printf("Code: 0x%x%x\n",alarmHighRegister,alarmLowRegister);
nerit 10:9e70619e97ab 824 #endif
nerit 10:9e70619e97ab 825 #endif
nerit 8:310f9e4eac7b 826 all_semiFiniti=0;
nerit 8:310f9e4eac7b 827 all_pickSignal=0;
nerit 8:310f9e4eac7b 828 all_cellSignal=0;
nerit 8:310f9e4eac7b 829 all_lowBattery=0;
nerit 8:310f9e4eac7b 830 all_overCurrDC=0;
nerit 8:310f9e4eac7b 831 all_stopSistem=0;
nerit 8:310f9e4eac7b 832 all_upElements=0;
nerit 8:310f9e4eac7b 833 all_noSeedOnCe=0;
nerit 8:310f9e4eac7b 834 all_cfgnErrors=0;
nerit 8:310f9e4eac7b 835 all_noDcRotati=0;
nerit 8:310f9e4eac7b 836 all_noStepRota=0;
nerit 8:310f9e4eac7b 837 all_speedError=0;
nerit 8:310f9e4eac7b 838 all_noSpeedSen=0;
nerit 8:310f9e4eac7b 839 all_no_Zeroing=0;
nerit 8:310f9e4eac7b 840 all_genericals=0;
nerit 10:9e70619e97ab 841 #if defined(pcSerial)
nerit 10:9e70619e97ab 842 #if defined(checkLoop)
nerit 10:9e70619e97ab 843 pc.printf("17\n");
nerit 10:9e70619e97ab 844 #endif
nerit 10:9e70619e97ab 845 #endif
nerit 3:a469bbd294b5 846 }
nerit 3:a469bbd294b5 847 //*******************************************************
nerit 3:a469bbd294b5 848 #if defined(speedMaster)
nerit 3:a469bbd294b5 849 void upDateSincro(){
nerit 8:310f9e4eac7b 850 char val1[8]= {0,0,0,0,0,0,0,0};
nerit 8:310f9e4eac7b 851 val1[3]=(posForQuinc /0x01000000)&0x000000FF;
nerit 8:310f9e4eac7b 852 val1[2]=(posForQuinc /0x00010000)&0x000000FF;
nerit 8:310f9e4eac7b 853 val1[1]=(posForQuinc /0x00000100)&0x000000FF;
nerit 3:a469bbd294b5 854 val1[0]=posForQuinc & 0x000000FF;
nerit 3:a469bbd294b5 855 //double pass = tractorSpeed_MtS_timed*100.0f;
nerit 3:a469bbd294b5 856 double pass = speedOfSeedWheel*100.0f;
nerit 3:a469bbd294b5 857 val1[4]=(uint8_t)(pass)&0x000000FF;
nerit 8:310f9e4eac7b 858 val1[5]=(prePosSD /0x00010000)&0x000000FF;
nerit 8:310f9e4eac7b 859 val1[6]=(prePosSD /0x00000100)&0x000000FF;
nerit 3:a469bbd294b5 860 val1[7]=prePosSD & 0x000000FF;
nerit 3:a469bbd294b5 861 #if defined(canbusActive)
nerit 3:a469bbd294b5 862 #if defined(speedMaster)
nerit 8:310f9e4eac7b 863 if(can1.write(CANMessage(TX_SI, *&val1,8))) {
nerit 3:a469bbd294b5 864 checkState=0;
nerit 3:a469bbd294b5 865 }
nerit 3:a469bbd294b5 866 #endif
nerit 3:a469bbd294b5 867 #endif
nerit 6:e8c18f0f399a 868 #if defined(pcSerial)
nerit 6:e8c18f0f399a 869 #if defined(checkLoop)
nerit 6:e8c18f0f399a 870 pc.printf("18\n");
nerit 6:e8c18f0f399a 871 #endif
nerit 6:e8c18f0f399a 872 #endif
nerit 3:a469bbd294b5 873 }
nerit 3:a469bbd294b5 874 #endif
nerit 3:a469bbd294b5 875 //*******************************************************
nerit 8:310f9e4eac7b 876 void upDateSpeed()
nerit 8:310f9e4eac7b 877 {
nerit 3:a469bbd294b5 878 /*
nerit 3:a469bbd294b5 879 aggiorna dati OPUSA3
nerit 3:a469bbd294b5 880 val1[0] contiene il dato di velocità
nerit 3:a469bbd294b5 881 val1[1] contiene il byte basso della tabella allarmi
nerit 3:a469bbd294b5 882 uint8_t all_semiFiniti = 0; // semi finiti (generato dal relativo sensore)
nerit 3:a469bbd294b5 883 uint8_t all_pickSignal = 0; // errore segnale becchi (generato dal tempo tra un becco ed il successivo)
nerit 3:a469bbd294b5 884 uint8_t all_cellSignal = 0; // errore segnale celle (generato dal sensore tamburo )
nerit 3:a469bbd294b5 885 uint8_t all_lowBattery = 0; // allarme batteria (valore interno di tritecnica)
nerit 3:a469bbd294b5 886 uint8_t all_overCurrDC = 0; // sovracorrente motore DC (generato dal sensore di corrente)
nerit 3:a469bbd294b5 887 uint8_t all_stopSistem = 0; // sistema in stop (a tempo con ruota ferma)
nerit 3:a469bbd294b5 888 uint8_t all_upElements = 0; // elementi sollevati (generato dal relativo sensore)
nerit 3:a469bbd294b5 889 uint8_t all_noSeedOnCe = 0; // fallanza di semina (manca il seme nella cella)
nerit 3:a469bbd294b5 890 uint8_t all_cfgnErrors = 0; // errore di configurazione (incongruenza dei parametri impostati)
nerit 3:a469bbd294b5 891 uint8_t all_noDcRotati = 0; // ruota di semina bloccata (arriva la velocità ma non i becchi)
nerit 3:a469bbd294b5 892 uint8_t all_noStepRota = 0; // tamburo di semina bloccato (comando il tamburo ma non ricevo il sensore)
nerit 3:a469bbd294b5 893 uint8_t all_speedError = 0; // errore sensore velocità (tempo impulsi non costante)
nerit 3:a469bbd294b5 894 uint8_t all_noSpeedSen = 0; // mancanza segnale di velocità (sento i becchi ma non la velocita)
nerit 3:a469bbd294b5 895 uint8_t all_no_Zeroing = 0; // mancato azzeramento sistema (generato dal timeout del comando motore DC)
nerit 3:a469bbd294b5 896 uint8_t all_genericals = 0; // allarme generico
nerit 3:a469bbd294b5 897 val1[2] contiene il byte alto della tabella di allarmi
nerit 3:a469bbd294b5 898 val1[3] contiene i segnali per la diagnostica
nerit 3:a469bbd294b5 899 bit 0= sensore ruota fonica
nerit 3:a469bbd294b5 900 bit 1= sensore celle
nerit 3:a469bbd294b5 901 bit 2= sensore posizione
nerit 3:a469bbd294b5 902 bit 3= sensore becchi
nerit 3:a469bbd294b5 903 bit 4= motore DC
nerit 3:a469bbd294b5 904 bit 5= controller
nerit 3:a469bbd294b5 905 bit 6= motore stepper
nerit 3:a469bbd294b5 906 */
nerit 8:310f9e4eac7b 907 char val1[8]= {0,0,0,0,0,0,0,0};
nerit 3:a469bbd294b5 908
nerit 8:310f9e4eac7b 909 val1[3]=0;
nerit 8:310f9e4eac7b 910 val1[3]=val1[3]+(tractorSpeedRead*0x01);
nerit 8:310f9e4eac7b 911 val1[3]=val1[3]+(TBzeroPinInput*0x02);
nerit 8:310f9e4eac7b 912 val1[3]=val1[3]+(DcEncoder*0x04);
nerit 8:310f9e4eac7b 913 val1[3]=val1[3]+(seedWheelZeroPinInput*0x08);
nerit 10:9e70619e97ab 914 #if defined(simulaPerCan)
nerit 10:9e70619e97ab 915 if (buttonUser==0) {
nerit 10:9e70619e97ab 916 val1[1]=0x02;
nerit 10:9e70619e97ab 917 } else {
nerit 10:9e70619e97ab 918 val1[1]=0x00;
nerit 10:9e70619e97ab 919 }
nerit 10:9e70619e97ab 920 val1[3]=valore;
nerit 10:9e70619e97ab 921 valore+=1;
nerit 10:9e70619e97ab 922 if(valore>50) {
nerit 10:9e70619e97ab 923 valore=0;
nerit 10:9e70619e97ab 924 }
nerit 10:9e70619e97ab 925 tractorSpeed_MtS_timed=valore;
nerit 10:9e70619e97ab 926 oldLocalTractorSpeed=valore;
nerit 10:9e70619e97ab 927 #endif
nerit 3:a469bbd294b5 928 allarmi();
nerit 3:a469bbd294b5 929 valore = totalSpeed*36.0f; // tractorSpeed_MtS_timed*36.0f;
nerit 3:a469bbd294b5 930 val1[0]=valore;
nerit 3:a469bbd294b5 931 val1[1]=alarmHighRegister; // registro alto allarmi. Parte sempre da 0x80
nerit 3:a469bbd294b5 932 val1[2]=alarmLowRegister; // registro basso allarmi
nerit 3:a469bbd294b5 933 //val1[3]=val1[3];// registro di stato degli ingressi
nerit 3:a469bbd294b5 934 val1[4]=resetComandi;
nerit 3:a469bbd294b5 935 val1[5]=cellsCounterLow;
nerit 3:a469bbd294b5 936 val1[6]=cellsCounterHig;
nerit 10:9e70619e97ab 937 #if defined(canbusActive)
nerit 10:9e70619e97ab 938 if(can1.write(CANMessage(TX_ID, *&val1,8))) {
nerit 10:9e70619e97ab 939 checkState=0;
nerit 10:9e70619e97ab 940 }
nerit 10:9e70619e97ab 941 #endif
nerit 10:9e70619e97ab 942 #if defined(pcSerial)
nerit 10:9e70619e97ab 943 #if defined(checkLoop)
nerit 10:9e70619e97ab 944 pc.printf("19\n");
nerit 10:9e70619e97ab 945 #endif
nerit 10:9e70619e97ab 946 #endif
nerit 3:a469bbd294b5 947 }
nerit 3:a469bbd294b5 948 //*******************************************************
nerit 8:310f9e4eac7b 949 void leggiCAN()
nerit 8:310f9e4eac7b 950 {
nerit 8:310f9e4eac7b 951 #if defined(canbusActive)
nerit 8:310f9e4eac7b 952 if(can1.read(rxMsg)) {
nerit 8:310f9e4eac7b 953 if (firstStart==1) {
nerit 8:310f9e4eac7b 954 #if defined(speedMaster)
nerit 8:310f9e4eac7b 955 sincUpdate.attach(&upDateSincro,0.012f);//0.009f
nerit 8:310f9e4eac7b 956 canUpdate.attach(&upDateSpeed,0.21f);
nerit 8:310f9e4eac7b 957 #else
nerit 8:310f9e4eac7b 958 canUpdate.attach(&upDateSpeed,0.407f);
nerit 8:310f9e4eac7b 959 #endif
nerit 8:310f9e4eac7b 960 firstStart=0;
nerit 8:310f9e4eac7b 961 }
nerit 8:310f9e4eac7b 962 if (rxMsg.id==RX_ID) {
nerit 8:310f9e4eac7b 963 #if defined(pcSerial)
nerit 8:310f9e4eac7b 964 #if defined(canDataReceiveda)
nerit 8:310f9e4eac7b 965 pc.printf("Messaggio ricevuto\n");
nerit 3:a469bbd294b5 966 #endif
nerit 8:310f9e4eac7b 967 #endif
nerit 8:310f9e4eac7b 968 }
nerit 8:310f9e4eac7b 969 if (rxMsg.id==RX_Broadcast) {
nerit 8:310f9e4eac7b 970 #if defined(pcSerial)
nerit 8:310f9e4eac7b 971 #if defined(canDataReceivedb)
nerit 3:a469bbd294b5 972 pc.printf("BroadCast ricevuto\n");
nerit 3:a469bbd294b5 973 #endif
nerit 8:310f9e4eac7b 974 #endif
nerit 8:310f9e4eac7b 975 enableSimula= rxMsg.data[0];
nerit 8:310f9e4eac7b 976 speedSimula = rxMsg.data[1];
nerit 8:310f9e4eac7b 977 avviaSimula = rxMsg.data[2];
nerit 8:310f9e4eac7b 978 selezionato = rxMsg.data[3];
nerit 8:310f9e4eac7b 979 comandiDaCan = rxMsg.data[4];
nerit 8:310f9e4eac7b 980 #if defined(pcSerial)
nerit 8:310f9e4eac7b 981 #if defined(canDataReceived)
nerit 3:a469bbd294b5 982 pc.printf("Speed simula %d \n",speedSimula);
nerit 3:a469bbd294b5 983 #endif
nerit 8:310f9e4eac7b 984 #endif
nerit 8:310f9e4eac7b 985 switch (comandiDaCan) {
nerit 8:310f9e4eac7b 986 case 1:
nerit 8:310f9e4eac7b 987 case 3:
nerit 8:310f9e4eac7b 988 azzeraDaCan=1;
nerit 8:310f9e4eac7b 989 resetComandi=0x01;
nerit 8:310f9e4eac7b 990 comandiDaCan=0;
nerit 8:310f9e4eac7b 991 break;
nerit 8:310f9e4eac7b 992 case 2:
nerit 8:310f9e4eac7b 993 loadDaCan=1;
nerit 8:310f9e4eac7b 994 resetComandi=0x02;
nerit 8:310f9e4eac7b 995 comandiDaCan=0;
nerit 8:310f9e4eac7b 996 break;
nerit 8:310f9e4eac7b 997 default:
nerit 8:310f9e4eac7b 998 comandiDaCan=0;
nerit 8:310f9e4eac7b 999 resetComandi=0xFF;
nerit 8:310f9e4eac7b 1000 break;
nerit 8:310f9e4eac7b 1001 }
nerit 8:310f9e4eac7b 1002 #if defined(pcSerial)
nerit 8:310f9e4eac7b 1003 #if defined(canDataReceivedR)
nerit 8:310f9e4eac7b 1004 pc.printf("Comandi: %x\n",resetComandi);
nerit 8:310f9e4eac7b 1005 #endif
nerit 8:310f9e4eac7b 1006 #endif
nerit 8:310f9e4eac7b 1007 if (speedSimula>45) {
nerit 8:310f9e4eac7b 1008 speedSimula=45;
nerit 8:310f9e4eac7b 1009 }
nerit 8:310f9e4eac7b 1010 // modulo 1
nerit 8:310f9e4eac7b 1011 if (RX_ID==0x100) {
nerit 8:310f9e4eac7b 1012 if ((selezionato&0x01)==0x01) {
nerit 8:310f9e4eac7b 1013 simOk=1;
nerit 8:310f9e4eac7b 1014 } else {
nerit 8:310f9e4eac7b 1015 simOk=0;
nerit 3:a469bbd294b5 1016 }
nerit 8:310f9e4eac7b 1017 }
nerit 8:310f9e4eac7b 1018 // modulo 2
nerit 8:310f9e4eac7b 1019 if (RX_ID==0x102) {
nerit 8:310f9e4eac7b 1020 if ((selezionato&0x02)==0x02) {
nerit 8:310f9e4eac7b 1021 simOk=1;
nerit 8:310f9e4eac7b 1022 } else {
nerit 8:310f9e4eac7b 1023 simOk=0;
nerit 3:a469bbd294b5 1024 }
nerit 8:310f9e4eac7b 1025 }
nerit 8:310f9e4eac7b 1026 // modulo 3
nerit 8:310f9e4eac7b 1027 if (RX_ID==0x104) {
nerit 8:310f9e4eac7b 1028 if ((selezionato&0x04)==0x04) {
nerit 8:310f9e4eac7b 1029 simOk=1;
nerit 8:310f9e4eac7b 1030 } else {
nerit 8:310f9e4eac7b 1031 simOk=0;
nerit 3:a469bbd294b5 1032 }
nerit 8:310f9e4eac7b 1033 }
nerit 8:310f9e4eac7b 1034 // modulo 4
nerit 8:310f9e4eac7b 1035 if (RX_ID==0x106) {
nerit 8:310f9e4eac7b 1036 if ((selezionato&0x08)==0x08) {
nerit 8:310f9e4eac7b 1037 simOk=1;
nerit 8:310f9e4eac7b 1038 } else {
nerit 8:310f9e4eac7b 1039 simOk=0;
nerit 3:a469bbd294b5 1040 }
nerit 8:310f9e4eac7b 1041 }
nerit 8:310f9e4eac7b 1042 // modulo 5
nerit 8:310f9e4eac7b 1043 if (RX_ID==0x108) {
nerit 8:310f9e4eac7b 1044 if ((selezionato&0x10)==0x10) {
nerit 8:310f9e4eac7b 1045 simOk=1;
nerit 8:310f9e4eac7b 1046 } else {
nerit 8:310f9e4eac7b 1047 simOk=0;
nerit 3:a469bbd294b5 1048 }
nerit 3:a469bbd294b5 1049 }
nerit 8:310f9e4eac7b 1050
nerit 8:310f9e4eac7b 1051 }
nerit 14:e2b5efa06c41 1052 //if (tractorSpeed_MtS_timed <= 0.01f){
nerit 8:310f9e4eac7b 1053 if (rxMsg.id==RX_Settings) {
nerit 14:e2b5efa06c41 1054 //if (tractorSpeed_MtS_timed==0.0f) {
nerit 3:a469bbd294b5 1055 pickNumber = rxMsg.data[0]; // numero di becchi installati sulla ruota di semina
nerit 3:a469bbd294b5 1056 cellsNumber = rxMsg.data[1]; // numero di celle del tamburo
nerit 17:9629eb019892 1057 deepOfSeed=((double)rxMsg.data[2]/1000.0f); // deep of seeding
nerit 5:2a3a64b52f54 1058 seedWheelDiameter = ((rxMsg.data[4]*0x100)+rxMsg.data[3])/10000.0f; // seed wheel diameter setting
nerit 5:2a3a64b52f54 1059 speedWheelDiameter = ((rxMsg.data[6]*0x100)+rxMsg.data[5])/10000.0f; // variable for tractor speed calculation (need to be set from UI) ( Unit= meters )
nerit 3:a469bbd294b5 1060 speedWheelPulse = rxMsg.data[7]; // variable which define the number of pulse each turn of tractor speed wheel (need to be set from UI)
nerit 3:a469bbd294b5 1061 aggiornaParametri();
nerit 14:e2b5efa06c41 1062 //}
nerit 3:a469bbd294b5 1063 }
nerit 14:e2b5efa06c41 1064 if (tractorSpeed_MtS_timed <= 0.01f){
nerit 8:310f9e4eac7b 1065 if (rxMsg.id==RX_AngoloPh) {
nerit 8:310f9e4eac7b 1066 if (tractorSpeed_MtS_timed==0.0f) {
nerit 3:a469bbd294b5 1067 #if defined(M1)
nerit 3:a469bbd294b5 1068 angoloPh = (double) rxMsg.data[0] ;
nerit 3:a469bbd294b5 1069 aggiornaParametri();
nerit 3:a469bbd294b5 1070 #endif
nerit 3:a469bbd294b5 1071 #if defined(M2)
nerit 3:a469bbd294b5 1072 angoloPh = (double) rxMsg.data[1] ;
nerit 3:a469bbd294b5 1073 aggiornaParametri();
nerit 3:a469bbd294b5 1074 #endif
nerit 3:a469bbd294b5 1075 #if defined(M3)
nerit 3:a469bbd294b5 1076 angoloPh = (double) rxMsg.data[2] ;
nerit 3:a469bbd294b5 1077 aggiornaParametri();
nerit 3:a469bbd294b5 1078 #endif
nerit 3:a469bbd294b5 1079 #if defined(M4)
nerit 3:a469bbd294b5 1080 angoloPh = (double) rxMsg.data[3] ;
nerit 3:a469bbd294b5 1081 aggiornaParametri();
nerit 3:a469bbd294b5 1082 #endif
nerit 3:a469bbd294b5 1083 #if defined(M5)
nerit 3:a469bbd294b5 1084 angoloPh = (double) rxMsg.data[4] ;
nerit 3:a469bbd294b5 1085 aggiornaParametri();
nerit 3:a469bbd294b5 1086 #endif
nerit 3:a469bbd294b5 1087 #if defined(M6)
nerit 3:a469bbd294b5 1088 angoloPh = (double) rxMsg.data[5] ;
nerit 3:a469bbd294b5 1089 aggiornaParametri();
nerit 3:a469bbd294b5 1090 #endif
nerit 3:a469bbd294b5 1091 }
nerit 3:a469bbd294b5 1092 }
nerit 8:310f9e4eac7b 1093 if (rxMsg.id==RX_AngoloAv) {
nerit 8:310f9e4eac7b 1094 if (tractorSpeed_MtS_timed==0.0f) {
nerit 3:a469bbd294b5 1095 #if defined(M1)
nerit 3:a469bbd294b5 1096 angoloAv = (double) rxMsg.data[0] ;
nerit 3:a469bbd294b5 1097 aggiornaParametri();
nerit 3:a469bbd294b5 1098 #endif
nerit 3:a469bbd294b5 1099 #if defined(M2)
nerit 3:a469bbd294b5 1100 angoloAv = (double) rxMsg.data[1] ;
nerit 3:a469bbd294b5 1101 aggiornaParametri();
nerit 3:a469bbd294b5 1102 #endif
nerit 3:a469bbd294b5 1103 #if defined(M3)
nerit 3:a469bbd294b5 1104 angoloAv = (double) rxMsg.data[2] ;
nerit 3:a469bbd294b5 1105 aggiornaParametri();
nerit 3:a469bbd294b5 1106 #endif
nerit 3:a469bbd294b5 1107 #if defined(M4)
nerit 3:a469bbd294b5 1108 angoloAv = (double) rxMsg.data[3] ;
nerit 3:a469bbd294b5 1109 aggiornaParametri();
nerit 3:a469bbd294b5 1110 #endif
nerit 3:a469bbd294b5 1111 #if defined(M5)
nerit 3:a469bbd294b5 1112 angoloAv = (double) rxMsg.data[4] ;
nerit 3:a469bbd294b5 1113 aggiornaParametri();
nerit 3:a469bbd294b5 1114 #endif
nerit 3:a469bbd294b5 1115 #if defined(M6)
nerit 3:a469bbd294b5 1116 angoloAv = (double) rxMsg.data[5] ;
nerit 3:a469bbd294b5 1117 aggiornaParametri();
nerit 3:a469bbd294b5 1118 #endif
nerit 3:a469bbd294b5 1119 }
nerit 3:a469bbd294b5 1120 }
nerit 8:310f9e4eac7b 1121 if (rxMsg.id==RX_Quinconce) {
nerit 8:310f9e4eac7b 1122 if (tractorSpeed_MtS_timed==0.0f) {
nerit 8:310f9e4eac7b 1123 quinconceActive = (uint8_t) rxMsg.data[0];
nerit 8:310f9e4eac7b 1124 quincPIDminus = (uint8_t) rxMsg.data[1];
nerit 8:310f9e4eac7b 1125 quincPIDplus = (uint8_t) rxMsg.data[2];
nerit 8:310f9e4eac7b 1126 quincLIMminus = (uint8_t) rxMsg.data[3];
nerit 8:310f9e4eac7b 1127 quincLIMplus = (uint8_t) rxMsg.data[4];
nerit 3:a469bbd294b5 1128 quincSector = (uint8_t) rxMsg.data[5];
nerit 3:a469bbd294b5 1129 aggiornaParametri();
nerit 3:a469bbd294b5 1130 }
nerit 3:a469bbd294b5 1131 }
nerit 8:310f9e4eac7b 1132 }
nerit 8:310f9e4eac7b 1133 if (tractorSpeed_MtS_timed > 0.0f){
nerit 8:310f9e4eac7b 1134 if (rxMsg.id==RX_QuincSinc) {
nerit 8:310f9e4eac7b 1135 masterSinc = (uint32_t) rxMsg.data[3] * 0x01000000;
nerit 8:310f9e4eac7b 1136 masterSinc = masterSinc + ((uint32_t) rxMsg.data[2] * 0x00010000);
nerit 8:310f9e4eac7b 1137 masterSinc = masterSinc + ((uint32_t) rxMsg.data[1] * 0x00000100);
nerit 3:a469bbd294b5 1138 masterSinc = masterSinc + ((uint32_t) rxMsg.data[0]);
nerit 3:a469bbd294b5 1139 speedFromMaster = (double)rxMsg.data[4]/100.0f;
nerit 8:310f9e4eac7b 1140 mast2_Sinc = ((uint32_t) rxMsg.data[5] * 0x00010000);
nerit 8:310f9e4eac7b 1141 mast2_Sinc = mast2_Sinc + ((uint32_t) rxMsg.data[6] * 0x00000100);
nerit 3:a469bbd294b5 1142 mast2_Sinc = mast2_Sinc + ((uint32_t) rxMsg.data[7]);
nerit 3:a469bbd294b5 1143 canDataCheck=1;
nerit 3:a469bbd294b5 1144 }
nerit 8:310f9e4eac7b 1145 }
nerit 8:310f9e4eac7b 1146 if (tractorSpeed_MtS_timed <= 0.01f){
nerit 8:310f9e4eac7b 1147 if (rxMsg.id==RX_Configure) {
nerit 3:a469bbd294b5 1148 uint8_t flags = rxMsg.data[0];
nerit 3:a469bbd294b5 1149 uint16_t steps = (uint32_t) rxMsg.data[2]*0x00000100;
nerit 3:a469bbd294b5 1150 steps = steps + ((uint32_t)rxMsg.data[1]);
nerit 3:a469bbd294b5 1151 TBmotorSteps =steps;
nerit 8:310f9e4eac7b 1152 //stepSetting();
nerit 3:a469bbd294b5 1153 cellsCountSet = rxMsg.data[3];
nerit 8:310f9e4eac7b 1154 if ((flags&0x01)==0x01) {
nerit 8:310f9e4eac7b 1155 if (encoder==false) {
nerit 3:a469bbd294b5 1156 encoder=true;
nerit 3:a469bbd294b5 1157 DcEncoder.rise(NULL);
nerit 3:a469bbd294b5 1158 dcSetting();
nerit 3:a469bbd294b5 1159 }
nerit 8:310f9e4eac7b 1160 } else {
nerit 8:310f9e4eac7b 1161 if (encoder==true) {
nerit 3:a469bbd294b5 1162 encoder=false;
nerit 3:a469bbd294b5 1163 DcEncoder.rise(NULL);
nerit 3:a469bbd294b5 1164 dcSetting();
nerit 3:a469bbd294b5 1165 }
nerit 3:a469bbd294b5 1166 }
nerit 8:310f9e4eac7b 1167 if ((flags&0x02)==0x02) {
nerit 3:a469bbd294b5 1168 tankLevelEnable=true;
nerit 8:310f9e4eac7b 1169 } else {
nerit 3:a469bbd294b5 1170 tankLevelEnable=false;
nerit 3:a469bbd294b5 1171 }
nerit 8:310f9e4eac7b 1172 if ((flags&0x04)==0x04) {
nerit 3:a469bbd294b5 1173 seedSensorEnable=true;
nerit 8:310f9e4eac7b 1174 } else {
nerit 3:a469bbd294b5 1175 seedSensorEnable=false;
nerit 3:a469bbd294b5 1176 }
nerit 8:310f9e4eac7b 1177 if ((flags&0x08)==0x08) {
nerit 3:a469bbd294b5 1178 stendiNylonEnable=true;
nerit 8:310f9e4eac7b 1179 } else {
nerit 3:a469bbd294b5 1180 stendiNylonEnable=false;
nerit 3:a469bbd294b5 1181 }
nerit 8:310f9e4eac7b 1182 if ((flags&0x10)==0x10) {
nerit 3:a469bbd294b5 1183 canDataCheckEnable=true;
nerit 8:310f9e4eac7b 1184 } else {
nerit 3:a469bbd294b5 1185 canDataCheckEnable=false;
nerit 3:a469bbd294b5 1186 }
nerit 8:310f9e4eac7b 1187 if ((flags&0x20)==0x20) {
nerit 3:a469bbd294b5 1188 tamburoStandard=1;
nerit 8:310f9e4eac7b 1189 } else {
nerit 3:a469bbd294b5 1190 tamburoStandard=0;
nerit 3:a469bbd294b5 1191 }
nerit 8:310f9e4eac7b 1192 if ((flags&0x40)==0x40) {
nerit 3:a469bbd294b5 1193 currentCheckEnable=true;
nerit 8:310f9e4eac7b 1194 } else {
nerit 3:a469bbd294b5 1195 currentCheckEnable=false;
nerit 3:a469bbd294b5 1196 }
nerit 3:a469bbd294b5 1197 }
nerit 8:310f9e4eac7b 1198 }
nerit 8:310f9e4eac7b 1199 }
nerit 10:9e70619e97ab 1200 #endif
nerit 10:9e70619e97ab 1201 #if defined(overWriteCanSimulation)
nerit 10:9e70619e97ab 1202 enableSimula=1;
nerit 10:9e70619e97ab 1203 speedSimula=25;
nerit 10:9e70619e97ab 1204 avviaSimula=1;
nerit 10:9e70619e97ab 1205 simOk=1;
nerit 10:9e70619e97ab 1206 #endif
nerit 10:9e70619e97ab 1207 #if defined(pcSerial)
nerit 10:9e70619e97ab 1208 #if defined(checkLoop)
nerit 10:9e70619e97ab 1209 pc.printf("20\n");
nerit 10:9e70619e97ab 1210 #endif
nerit 10:9e70619e97ab 1211 #endif
bcostm 0:5701b41769fd 1212 }
bcostm 0:5701b41769fd 1213
nerit 3:a469bbd294b5 1214 //*******************************************************
nerit 8:310f9e4eac7b 1215 void DC_CheckCurrent()
nerit 8:310f9e4eac7b 1216 {
nerit 8:310f9e4eac7b 1217
nerit 3:a469bbd294b5 1218 // TODO: tabella di riferimento assorbimenti alle varie velocità al fine di gestire
nerit 3:a469bbd294b5 1219 // gli allarmi e le correzioni di velocità
nerit 8:310f9e4eac7b 1220
nerit 3:a469bbd294b5 1221 //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 3:a469bbd294b5 1222 //int SD_analogIndex[10]={0,0,0,0,0,0,0,0,0,0};
nerit 3:a469bbd294b5 1223 // Analog reading
nerit 3:a469bbd294b5 1224 //number = floor(number * 100) / 100;
nerit 8:310f9e4eac7b 1225 timeout.detach();
nerit 8:310f9e4eac7b 1226 SD_CurrentAnalog = floor(SDcurrent.read()*100)/100; // valore in ingresso compreso tra 0.00 e 1.00
nerit 12:b0fc1d313813 1227 SD_CurrentScaled = floor(( (SD_CurrentStart-SD_CurrentAnalog)*3.3f) / (SD_CurrentFactor/1000.0f)*10)/10;
nerit 3:a469bbd294b5 1228 }
nerit 3:a469bbd294b5 1229 //*******************************************************
nerit 8:310f9e4eac7b 1230 void DC_prepare()
nerit 8:310f9e4eac7b 1231 {
nerit 3:a469bbd294b5 1232 // direction or brake preparation
nerit 8:310f9e4eac7b 1233 if (DC_brake==1) {
nerit 3:a469bbd294b5 1234 SDmotorInA=1;
nerit 3:a469bbd294b5 1235 SDmotorInB=1;
nerit 8:310f9e4eac7b 1236 } else {
nerit 8:310f9e4eac7b 1237 if (DC_forward==0) {
nerit 3:a469bbd294b5 1238 SDmotorInA=1;
nerit 3:a469bbd294b5 1239 SDmotorInB=0;
nerit 8:310f9e4eac7b 1240 } else {
nerit 3:a469bbd294b5 1241 SDmotorInA=0;
nerit 3:a469bbd294b5 1242 SDmotorInB=1;
nerit 3:a469bbd294b5 1243 }
nerit 3:a469bbd294b5 1244 }
nerit 3:a469bbd294b5 1245 // fault reading
nerit 8:310f9e4eac7b 1246 if (SDmotorInA==1) {
nerit 8:310f9e4eac7b 1247 SD_faultA=1;
nerit 8:310f9e4eac7b 1248 } else {
nerit 8:310f9e4eac7b 1249 SD_faultA=0;
nerit 8:310f9e4eac7b 1250 }
nerit 8:310f9e4eac7b 1251 if (SDmotorInB==1) {
nerit 8:310f9e4eac7b 1252 SD_faultB=1;
nerit 8:310f9e4eac7b 1253 } else {
nerit 8:310f9e4eac7b 1254 SD_faultB=0;
nerit 8:310f9e4eac7b 1255 }
nerit 10:9e70619e97ab 1256 #if defined(pcSerial)
nerit 10:9e70619e97ab 1257 #if defined(checkLoopa)
nerit 10:9e70619e97ab 1258 pc.printf("22\n");
nerit 10:9e70619e97ab 1259 #endif
nerit 10:9e70619e97ab 1260 #endif
nerit 3:a469bbd294b5 1261 }
nerit 3:a469bbd294b5 1262 //*******************************************************
nerit 12:b0fc1d313813 1263 void startDelay(){
nerit 12:b0fc1d313813 1264 SD_CurrentStart = floor(SDcurrent.read()*100)/100; // valore in ingresso compreso tra 0.00 e 1.00
nerit 12:b0fc1d313813 1265 int ritardo =0;
nerit 12:b0fc1d313813 1266 ritardo = (int)((float)(dcActualDuty*800.0f));
nerit 12:b0fc1d313813 1267 timeout.attach_us(&DC_CheckCurrent,ritardo);
nerit 3:a469bbd294b5 1268 }
nerit 3:a469bbd294b5 1269 //*******************************************************
nerit 10:9e70619e97ab 1270 void quincTrigon(){
nerit 3:a469bbd294b5 1271 quincClock=true;
nerit 10:9e70619e97ab 1272 #if defined(pcSerial)
nerit 10:9e70619e97ab 1273 #if defined(checkLoop)
nerit 10:9e70619e97ab 1274 pc.printf("24\n");
nerit 10:9e70619e97ab 1275 #endif
nerit 10:9e70619e97ab 1276 #endif
nerit 3:a469bbd294b5 1277 }
nerit 10:9e70619e97ab 1278 void quincTrigof(){
nerit 3:a469bbd294b5 1279 quincClock=false;
nerit 10:9e70619e97ab 1280 #if defined(pcSerial)
nerit 10:9e70619e97ab 1281 #if defined(checkLoop)
nerit 10:9e70619e97ab 1282 pc.printf("25\n");
nerit 10:9e70619e97ab 1283 #endif
nerit 10:9e70619e97ab 1284 #endif
nerit 3:a469bbd294b5 1285 }
nerit 3:a469bbd294b5 1286 //*******************************************************
nerit 8:310f9e4eac7b 1287 void quinCalc()
nerit 8:310f9e4eac7b 1288 {
nerit 3:a469bbd294b5 1289 // riceve l'impulso di sincro dal master e fa partire il timer di verifica dell'errore
nerit 10:9e70619e97ab 1290 #if !defined(mezzo)
nerit 10:9e70619e97ab 1291 if ((quincClock==true)&&(oldQuincIn==0)) {
nerit 10:9e70619e97ab 1292 oldQuincIn=1;
nerit 10:9e70619e97ab 1293 if (quincStart==0) {
nerit 10:9e70619e97ab 1294 oldQuincTimeSD = (double) quincTimeSD.read_ms();
nerit 10:9e70619e97ab 1295 quincTime.reset();
nerit 10:9e70619e97ab 1296 quincTimeSD.reset();
nerit 10:9e70619e97ab 1297 quincStart=1;
nerit 10:9e70619e97ab 1298 }
nerit 3:a469bbd294b5 1299 }
nerit 10:9e70619e97ab 1300 if(quincClock==false) {
nerit 3:a469bbd294b5 1301 oldQuincIn=0;
nerit 3:a469bbd294b5 1302 }
nerit 10:9e70619e97ab 1303 #else
nerit 10:9e70619e97ab 1304 if ((((quinconceActive==0)&&(quincClock==true))||((quinconceActive==1)&&(quincClock==false)))&&(oldQuincIn==0)) {
nerit 10:9e70619e97ab 1305 oldQuincIn=1;
nerit 10:9e70619e97ab 1306 if (quincStart==0) {
nerit 10:9e70619e97ab 1307 oldQuincTimeSD = (double) quincTimeSD.read_ms();
nerit 10:9e70619e97ab 1308 quincTime.reset();
nerit 10:9e70619e97ab 1309 quincStart=1;
nerit 10:9e70619e97ab 1310 }
nerit 3:a469bbd294b5 1311 }
nerit 10:9e70619e97ab 1312 if (quinconceActive==0) {
nerit 10:9e70619e97ab 1313 if (quincClock==false) {
nerit 10:9e70619e97ab 1314 oldQuincIn=0;
nerit 10:9e70619e97ab 1315 }
nerit 10:9e70619e97ab 1316 } else {
nerit 10:9e70619e97ab 1317 if (quincClock==true) {
nerit 10:9e70619e97ab 1318 oldQuincIn=0;
nerit 10:9e70619e97ab 1319 }
nerit 10:9e70619e97ab 1320 }
nerit 10:9e70619e97ab 1321 #endif
nerit 3:a469bbd294b5 1322 //****************************************************************************************
nerit 8:310f9e4eac7b 1323 if (quincCnt>=4) {
nerit 8:310f9e4eac7b 1324 if (countPicks==0) {
nerit 8:310f9e4eac7b 1325 if ((sincroQui==1)&&(quincStart==0)) {
nerit 3:a469bbd294b5 1326 // decelera
nerit 3:a469bbd294b5 1327 countPicks=1;
nerit 3:a469bbd294b5 1328 }
nerit 8:310f9e4eac7b 1329 if ((sincroQui==0)&&(quincStart==1)) {
nerit 3:a469bbd294b5 1330 // accelera
nerit 3:a469bbd294b5 1331 countPicks=2;
nerit 3:a469bbd294b5 1332 }
nerit 3:a469bbd294b5 1333 }
nerit 8:310f9e4eac7b 1334 if ((sincroQui==1)&&(quincStart==1)) {
nerit 8:310f9e4eac7b 1335 if (countPicks==1) { //decelera
nerit 3:a469bbd294b5 1336 scostamento = oldQuincTimeSD;
nerit 17:9629eb019892 1337 if ((scostamento < (tempoBecchiPerQuinc*0.85f))) {
nerit 3:a469bbd294b5 1338 double scostPerc = (scostamento/tempoBecchiPerQuinc);
nerit 3:a469bbd294b5 1339 percento -= ((double)quincPIDminus/100.0f)*(scostPerc);
nerit 3:a469bbd294b5 1340 #if defined(pcSerial)
nerit 3:a469bbd294b5 1341 #if defined(laq)
nerit 3:a469bbd294b5 1342 pc.printf("RALL scos2: %f tbpq: %f percento: %f \n",scostamento,tempoBecchiPerQuinc,percento);
nerit 3:a469bbd294b5 1343 #endif
nerit 3:a469bbd294b5 1344 #endif
nerit 3:a469bbd294b5 1345 }
nerit 3:a469bbd294b5 1346 //if (scostamento <15.0f){percento=0.0f;}
nerit 3:a469bbd294b5 1347 }
nerit 8:310f9e4eac7b 1348 if (countPicks==2) { //accelera
nerit 3:a469bbd294b5 1349 scostamento = (double)quincTime.read_ms();
nerit 17:9629eb019892 1350 if (scostamento < (tempoBecchiPerQuinc*0.85f)) {
nerit 3:a469bbd294b5 1351 double scostPerc = (scostamento/tempoBecchiPerQuinc);
nerit 3:a469bbd294b5 1352 percento += ((double)quincPIDplus/100.0f)*(scostPerc);
nerit 3:a469bbd294b5 1353 #if defined(pcSerial)
nerit 3:a469bbd294b5 1354 #if defined(laq)
nerit 8:310f9e4eac7b 1355 pc.printf("ACCE scos1: %f tbpq: %f percento: %f \n",scostamento,tempoBecchiPerQuinc,percento);
nerit 3:a469bbd294b5 1356 #endif
nerit 3:a469bbd294b5 1357 #endif
nerit 3:a469bbd294b5 1358 }
nerit 3:a469bbd294b5 1359 //if (scostamento <15.0f){percento=0.0f;}
nerit 3:a469bbd294b5 1360 }
nerit 3:a469bbd294b5 1361 sincroQui=0;
nerit 3:a469bbd294b5 1362 quincStart=0;
nerit 3:a469bbd294b5 1363 countPicks=0;
nerit 5:2a3a64b52f54 1364 // questo e il primo
nerit 3:a469bbd294b5 1365 #if !defined(speedMaster)
nerit 8:310f9e4eac7b 1366 if (quincCnt>=3) {
nerit 8:310f9e4eac7b 1367 if (speedFromMaster>0.0f) {
nerit 8:310f9e4eac7b 1368 if (enableSimula==0) {
nerit 8:310f9e4eac7b 1369 tractorSpeed_MtS_timed = speedFromMaster + percento;
nerit 14:e2b5efa06c41 1370 /*if (tractorSpeed_MtS_timed > (oldLocalTractorSpeed*1.15f)){
nerit 10:9e70619e97ab 1371 tractorSpeed_MtS_timed = oldLocalTractorSpeed*1.15f;
nerit 10:9e70619e97ab 1372 }
nerit 13:d1030d4e51a8 1373 if (tractorSpeed_MtS_timed < (oldLocalTractorSpeed/1.15f)){
nerit 10:9e70619e97ab 1374 tractorSpeed_MtS_timed = oldLocalTractorSpeed/1.15f;
nerit 14:e2b5efa06c41 1375 }*/
nerit 3:a469bbd294b5 1376 }
nerit 8:310f9e4eac7b 1377 }
nerit 3:a469bbd294b5 1378 }
nerit 3:a469bbd294b5 1379 #endif
nerit 3:a469bbd294b5 1380 }
nerit 8:310f9e4eac7b 1381
nerit 3:a469bbd294b5 1382 //*******************************************************************
nerit 8:310f9e4eac7b 1383 if (canDataCheckEnable==true) {
nerit 8:310f9e4eac7b 1384 if (canDataCheck==1) { // sincro da comunicazione can del valore di posizione del tamburo master
nerit 3:a469bbd294b5 1385 canDataCheck=0;
nerit 17:9629eb019892 1386 double parametro = SDsectorStep/(double)quincSector;
nerit 3:a469bbd294b5 1387 double differenza=0.0f;
nerit 3:a469bbd294b5 1388 #if defined(mezzo)
nerit 8:310f9e4eac7b 1389 if (quinconceActive==1) {
nerit 3:a469bbd294b5 1390 differenza = (double)masterSinc - (double)prePosSD;
nerit 8:310f9e4eac7b 1391 } else {
nerit 3:a469bbd294b5 1392 differenza = (double)mast2_Sinc - (double)prePosSD;
nerit 3:a469bbd294b5 1393 }
nerit 3:a469bbd294b5 1394 #else
nerit 3:a469bbd294b5 1395 differenza = (double)mast2_Sinc - (double)prePosSD;
nerit 3:a469bbd294b5 1396 #endif
nerit 17:9629eb019892 1397 if (abs(differenza)<=50.0f){differenza=0.0f;}
nerit 8:310f9e4eac7b 1398 if ((differenza > 0.0f)&&(differenza < parametro)) {
nerit 8:310f9e4eac7b 1399 double diffPerc = differenza / parametro;
nerit 3:a469bbd294b5 1400 percento += ((double)quincPIDplus/100.0f)*abs(diffPerc);
nerit 3:a469bbd294b5 1401 }
nerit 8:310f9e4eac7b 1402 if ((differenza < 0.0f)&&(abs(differenza) < parametro)) {
nerit 8:310f9e4eac7b 1403 double diffPerc = (double)differenza / parametro;
nerit 3:a469bbd294b5 1404 percento -= ((double)quincPIDminus/100.0f)*abs(diffPerc);
nerit 3:a469bbd294b5 1405 }
nerit 5:2a3a64b52f54 1406 // questo e il secondo
nerit 3:a469bbd294b5 1407 #if !defined(speedMaster)
nerit 8:310f9e4eac7b 1408 if (quincCnt>=3) {
nerit 8:310f9e4eac7b 1409 if (speedFromMaster>0.0f) {
nerit 8:310f9e4eac7b 1410 if (enableSimula==0) {
nerit 5:2a3a64b52f54 1411 tractorSpeed_MtS_timed = speedFromMaster + percento;
nerit 14:e2b5efa06c41 1412 /*if (tractorSpeed_MtS_timed > (oldLocalTractorSpeed*1.15f)){
nerit 10:9e70619e97ab 1413 tractorSpeed_MtS_timed = oldLocalTractorSpeed*1.15f;
nerit 10:9e70619e97ab 1414 }
nerit 13:d1030d4e51a8 1415 if (tractorSpeed_MtS_timed < (oldLocalTractorSpeed/1.15f)){
nerit 10:9e70619e97ab 1416 tractorSpeed_MtS_timed = oldLocalTractorSpeed/1.15f;
nerit 14:e2b5efa06c41 1417 }*/
nerit 3:a469bbd294b5 1418 }
nerit 5:2a3a64b52f54 1419 }
nerit 3:a469bbd294b5 1420 }
nerit 3:a469bbd294b5 1421 #endif
nerit 3:a469bbd294b5 1422 }
nerit 3:a469bbd294b5 1423 }
nerit 8:310f9e4eac7b 1424
nerit 3:a469bbd294b5 1425 }
nerit 8:310f9e4eac7b 1426 if ((percento) > ((double) quincLIMplus/100.0f)) {
nerit 3:a469bbd294b5 1427 percento= (double)quincLIMplus/100.0f;
nerit 3:a469bbd294b5 1428 }
nerit 8:310f9e4eac7b 1429 if ((percento) < (((double)quincLIMminus*-1.0f)/100.0f)) {
nerit 3:a469bbd294b5 1430 percento=((double)quincLIMminus*-1.0f)/100.0f;
bcostm 0:5701b41769fd 1431 }
nerit 6:e8c18f0f399a 1432 #if defined(pcSerial)
nerit 6:e8c18f0f399a 1433 #if defined(checkLoop)
nerit 6:e8c18f0f399a 1434 pc.printf("26\n");
nerit 6:e8c18f0f399a 1435 #endif
nerit 6:e8c18f0f399a 1436 #endif
bcostm 0:5701b41769fd 1437 }
nerit 8:310f9e4eac7b 1438 // ----------------------------------------
nerit 3:a469bbd294b5 1439 #if defined(seedSensor)
nerit 3:a469bbd294b5 1440 void resetDelay(){
nerit 3:a469bbd294b5 1441 delaySeedCheck.reset();
nerit 3:a469bbd294b5 1442 delaySeedCheck.stop();
nerit 8:310f9e4eac7b 1443 #if defined(pcSerial)
nerit 8:310f9e4eac7b 1444 #if defined(checkLoop)
nerit 8:310f9e4eac7b 1445 pc.printf("27\n");
nerit 8:310f9e4eac7b 1446 #endif
nerit 6:e8c18f0f399a 1447 #endif
nerit 3:a469bbd294b5 1448 }
nerit 3:a469bbd294b5 1449 #endif
nerit 8:310f9e4eac7b 1450
nerit 17:9629eb019892 1451 void aggiornati(){
nerit 17:9629eb019892 1452 #if defined(runner)
nerit 17:9629eb019892 1453 legPos.detach();
nerit 17:9629eb019892 1454 TBoldPosition= (uint32_t) motor->get_position();
nerit 17:9629eb019892 1455 legPos.attach(&step_Reading,0.002f);
nerit 17:9629eb019892 1456 #if defined(pcSerial)
nerit 17:9629eb019892 1457 #if defined(TBperS)
nerit 17:9629eb019892 1458 pc.printf("TBoldPos: %d\n",TBoldPosition);
nerit 17:9629eb019892 1459 #endif
nerit 17:9629eb019892 1460 #endif
nerit 17:9629eb019892 1461
nerit 17:9629eb019892 1462 #else
nerit 17:9629eb019892 1463 TBactualPosition=0;
nerit 17:9629eb019892 1464 #endif
nerit 17:9629eb019892 1465 }
nerit 17:9629eb019892 1466
nerit 3:a469bbd294b5 1467 // ------------------------------------------------------------------------------------------------------------------------------------------------------------------
nerit 3:a469bbd294b5 1468 // MAIN SECTION
nerit 3:a469bbd294b5 1469 // ---------------------------------------------------------------------------------------------------------------------------------------------------------------
bcostm 0:5701b41769fd 1470
nerit 3:a469bbd294b5 1471 //*******************************************************************************
nerit 8:310f9e4eac7b 1472 int main()
nerit 8:310f9e4eac7b 1473 {
nerit 6:e8c18f0f399a 1474 #if defined(pcSerial)
nerit 6:e8c18f0f399a 1475 #if defined(resetCpu)
nerit 6:e8c18f0f399a 1476 pc.printf("RESET\n");
nerit 6:e8c18f0f399a 1477 #endif
nerit 6:e8c18f0f399a 1478 #endif
nerit 8:310f9e4eac7b 1479
nerit 8:310f9e4eac7b 1480 #if defined(canbusActive)
nerit 8:310f9e4eac7b 1481 can1.attach(&leggiCAN, CAN::RxIrq);
nerit 8:310f9e4eac7b 1482 #endif
nerit 8:310f9e4eac7b 1483
nerit 8:310f9e4eac7b 1484 wait(1.0f);
nerit 8:310f9e4eac7b 1485 wait(1.0f);
nerit 8:310f9e4eac7b 1486 wait(1.0f);
nerit 8:310f9e4eac7b 1487
nerit 8:310f9e4eac7b 1488 //stepSetting();
nerit 8:310f9e4eac7b 1489
nerit 6:e8c18f0f399a 1490 TBmotor_SW=1;
nerit 8:310f9e4eac7b 1491 TBmotorDirecti=TBforward; // reset stepper direction
nerit 8:310f9e4eac7b 1492 //----- Initialization
nerit 8:310f9e4eac7b 1493 /* Initializing SPI bus. */
nerit 8:310f9e4eac7b 1494 // dev_spi(mosi,miso,sclk)
nerit 8:310f9e4eac7b 1495 // D11= PA7; D12= PA6; D13= PA5
nerit 8:310f9e4eac7b 1496 DevSPI dev_spi(D11, D12, D13);
nerit 10:9e70619e97ab 1497 dev_spi.frequency(5000000);
nerit 10:9e70619e97ab 1498
nerit 8:310f9e4eac7b 1499 /* Initializing Motor Control Component. */
nerit 8:310f9e4eac7b 1500 // powerstep01( flag, busy,stby, stck, cs, dev_spi)
nerit 8:310f9e4eac7b 1501 // motor = new PowerStep01(D2, D4, D8, D9, D10, dev_spi); // linea standard per IHM03A1
nerit 8:310f9e4eac7b 1502 motor = new PowerStep01(PA_8, PC_7, PC_4, PB_3, PB_6, dev_spi); // linea per scheda seminatrice V7
nerit 8:310f9e4eac7b 1503 if (motor->init(&init) != COMPONENT_OK) {
nerit 6:e8c18f0f399a 1504 exit(EXIT_FAILURE);
nerit 8:310f9e4eac7b 1505 }
nerit 6:e8c18f0f399a 1506
nerit 8:310f9e4eac7b 1507 /* Attaching and enabling an interrupt handler. */
nerit 8:310f9e4eac7b 1508 motor->attach_flag_irq(&my_flag_irq_handler);
nerit 8:310f9e4eac7b 1509 motor->enable_flag_irq();
nerit 8:310f9e4eac7b 1510 //motor->disable_flag_irq();
nerit 8:310f9e4eac7b 1511
nerit 8:310f9e4eac7b 1512 /* Attaching an error handler */
nerit 8:310f9e4eac7b 1513 //motor->attach_error_handler(&my_error_handler);
nerit 8:310f9e4eac7b 1514 wait(1.0f);
nerit 8:310f9e4eac7b 1515 for (int a=0; a<5; a++) {
nerit 3:a469bbd294b5 1516 mediaSpeed[a]=0;
nerit 3:a469bbd294b5 1517 }
nerit 8:310f9e4eac7b 1518
nerit 3:a469bbd294b5 1519 // DC reset ad set
nerit 3:a469bbd294b5 1520 int decima = 100;
nerit 3:a469bbd294b5 1521 wait_ms(200);
nerit 3:a469bbd294b5 1522 SD_CurrentStart=floor(SDcurrent.read()*decima)/decima;
nerit 3:a469bbd294b5 1523 wait_ms(2);
nerit 3:a469bbd294b5 1524 SD_CurrentStart+=floor(SDcurrent.read()*decima)/decima;
nerit 3:a469bbd294b5 1525 wait_ms(1);
nerit 3:a469bbd294b5 1526 SD_CurrentStart+=floor(SDcurrent.read()*decima)/decima;
nerit 3:a469bbd294b5 1527 wait_ms(3);
nerit 3:a469bbd294b5 1528 SD_CurrentStart+=floor(SDcurrent.read()*decima)/decima;
nerit 8:310f9e4eac7b 1529 SD_CurrentStart=(floor((SD_CurrentStart/4.0f)*decima)/decima)-0.01f;
nerit 3:a469bbd294b5 1530 wait_ms(100);
nerit 3:a469bbd294b5 1531 DC_prepare();
nerit 8:310f9e4eac7b 1532
nerit 8:310f9e4eac7b 1533 speedTimer.start(); // speed pulse timer
nerit 3:a469bbd294b5 1534 intraPickTimer.start();
nerit 8:310f9e4eac7b 1535 speedTimeOut.start();
nerit 3:a469bbd294b5 1536 speedFilter.start();
nerit 3:a469bbd294b5 1537 seedFilter.start();
nerit 3:a469bbd294b5 1538 TBfilter.start();
nerit 3:a469bbd294b5 1539 sincroTimer.start();
nerit 3:a469bbd294b5 1540 rotationTimeOut.start();
nerit 3:a469bbd294b5 1541 metalTimer.start();
nerit 3:a469bbd294b5 1542 quincTime.start();
nerit 3:a469bbd294b5 1543 quincTimeSD.start();
nerit 8:310f9e4eac7b 1544 #if defined(runner)
nerit 8:310f9e4eac7b 1545 legPos.attach(&step_Reading,0.002f);
nerit 8:310f9e4eac7b 1546 #endif
nerit 8:310f9e4eac7b 1547
nerit 3:a469bbd294b5 1548 //*******************************************************
nerit 3:a469bbd294b5 1549 // controls for check DC motor current
nerit 8:310f9e4eac7b 1550 pwmCheck.rise(&startDelay);
nerit 3:a469bbd294b5 1551 wait_ms(500);
nerit 8:310f9e4eac7b 1552
nerit 12:b0fc1d313813 1553 dcVeri.attach(&controllaCorrente,2.0f);
nerit 13:d1030d4e51a8 1554 delaySpeedCheck.start();
nerit 12:b0fc1d313813 1555
nerit 8:310f9e4eac7b 1556 #if defined(runnerTos)
nerit 8:310f9e4eac7b 1557 thread.start(step_Reading);
nerit 8:310f9e4eac7b 1558 #endif
nerit 3:a469bbd294b5 1559
nerit 8:310f9e4eac7b 1560 if (inProva==0) {
nerit 3:a469bbd294b5 1561 tractorSpeedRead.rise(&tractorReadSpeed);
nerit 3:a469bbd294b5 1562 #if !defined(speedMaster)
nerit 3:a469bbd294b5 1563 quinconceIn.rise(&quincTrigon);
nerit 3:a469bbd294b5 1564 quinconceIn.fall(&quincTrigof);
nerit 3:a469bbd294b5 1565 #endif
nerit 3:a469bbd294b5 1566 #if defined(speedMaster)
nerit 3:a469bbd294b5 1567 tftUpdate.attach(&videoUpdate,0.50f);
nerit 3:a469bbd294b5 1568 #endif
nerit 3:a469bbd294b5 1569 seedCorrection.attach(&seedCorrect,0.010f); // con 16 becchi a 4,5Kmh ci sono 37mS tra un becco e l'altro, quindi 8 correzioni di tb
nerit 3:a469bbd294b5 1570 dcSetting();
nerit 3:a469bbd294b5 1571 #if defined(seedSensor)
nerit 3:a469bbd294b5 1572 seedCheck.fall(&seedSensorTask);
nerit 3:a469bbd294b5 1573 #endif
nerit 8:310f9e4eac7b 1574 } else {
nerit 8:310f9e4eac7b 1575 tftUpdate.attach(&videoUpdate,0.125f);
nerit 8:310f9e4eac7b 1576 }
nerit 8:310f9e4eac7b 1577
nerit 3:a469bbd294b5 1578 aggiornaParametri();
nerit 3:a469bbd294b5 1579
nerit 3:a469bbd294b5 1580 SDmotorPWM.period_us(periodoSD); // frequency 1KHz pilotaggio motore DC
nerit 3:a469bbd294b5 1581 SDmotorPWM.write(1.0f); // duty cycle = stop
nerit 3:a469bbd294b5 1582 TBmotorDirecti=TBforward; // tb motor direction set
nerit 3:a469bbd294b5 1583
nerit 3:a469bbd294b5 1584 #if defined(provaStepper)
nerit 3:a469bbd294b5 1585 TBmotorRst=1;
nerit 3:a469bbd294b5 1586 TBmotorDirecti=TBforward;
nerit 3:a469bbd294b5 1587 // definire il pin di clock che è PB_3
nerit 9:7f02256f6e8f 1588 #if defined(Zucca)
nerit 17:9629eb019892 1589 motor->run(StepperMotor::BWD,100.0f);
nerit 10:9e70619e97ab 1590 //motor->step_clock_mode_enable(StepperMotor::FWD);
nerit 9:7f02256f6e8f 1591 #else
nerit 17:9629eb019892 1592 motor->run(StepperMotor::BWD,5.0f);
nerit 10:9e70619e97ab 1593 //motor->step_clock_mode_enable(StepperMotor::BWD);
nerit 9:7f02256f6e8f 1594 #endif
nerit 3:a469bbd294b5 1595 // attiva l'out per il controllo dello stepper in stepClockMode
nerit 10:9e70619e97ab 1596 //DigitalOut TBmotorStepOut(PB_3); // PowerStep01 Step Input
nerit 6:e8c18f0f399a 1597 #if defined(pcSerial)
nerit 6:e8c18f0f399a 1598 #if defined(checkLoop)
nerit 6:e8c18f0f399a 1599 pc.printf("11f\n");
nerit 6:e8c18f0f399a 1600 #endif
nerit 6:e8c18f0f399a 1601 #endif
nerit 10:9e70619e97ab 1602 //TBticker.attach(&step_TBPulseOut,0.0005f); // clock time are seconds and attach seed motor stepper controls
nerit 10:9e70619e97ab 1603 //TATicker.attach(&invertiLo,3.0f);
nerit 3:a469bbd294b5 1604 #else
nerit 3:a469bbd294b5 1605 // definire il pin di clock che è PB_3
nerit 8:310f9e4eac7b 1606 motor->set_home();
nerit 8:310f9e4eac7b 1607 motor->go_to(50);
nerit 8:310f9e4eac7b 1608 motor->wait_while_active();
nerit 8:310f9e4eac7b 1609 #if !defined(runner)
nerit 9:7f02256f6e8f 1610 #if defined(Zucca)
nerit 9:7f02256f6e8f 1611 motor->step_clock_mode_enable(StepperMotor::BWD);
nerit 9:7f02256f6e8f 1612 #else
nerit 9:7f02256f6e8f 1613 motor->step_clock_mode_enable(StepperMotor::FWD);
nerit 9:7f02256f6e8f 1614 #endif
nerit 8:310f9e4eac7b 1615 #endif
nerit 6:e8c18f0f399a 1616 #if defined(pcSerial)
nerit 6:e8c18f0f399a 1617 #if defined(loStop)
nerit 6:e8c18f0f399a 1618 pc.printf("A3\n");
nerit 6:e8c18f0f399a 1619 #endif
nerit 6:e8c18f0f399a 1620 #endif
nerit 6:e8c18f0f399a 1621 motor->soft_hiz();
nerit 3:a469bbd294b5 1622 // attiva l'out per il controllo dello stepper in stepClockMode
nerit 3:a469bbd294b5 1623 DigitalOut TBmotorStepOut(PB_3); // PowerStep01 Step Input
nerit 3:a469bbd294b5 1624 #endif // end prova stepper
nerit 8:310f9e4eac7b 1625
nerit 8:310f9e4eac7b 1626 wd.Configure(2.0); //watchdog set at xx seconds
nerit 8:310f9e4eac7b 1627
nerit 8:310f9e4eac7b 1628
nerit 3:a469bbd294b5 1629 //**************************************************************************************************************
nerit 3:a469bbd294b5 1630 // MAIN LOOP
nerit 3:a469bbd294b5 1631 //**************************************************************************************************************
nerit 8:310f9e4eac7b 1632 while (true) {
nerit 6:e8c18f0f399a 1633 // ripetitore segnale di velocità. Il set a 1 è nella task ad interrupt
nerit 8:310f9e4eac7b 1634 if (tractorSpeedRead==0) {
nerit 8:310f9e4eac7b 1635 speedClock=0;
nerit 8:310f9e4eac7b 1636 }
nerit 8:310f9e4eac7b 1637
nerit 3:a469bbd294b5 1638 // inversione segnali ingressi
nerit 3:a469bbd294b5 1639 #if !defined(simulaBanco)
nerit 3:a469bbd294b5 1640 seedWheelZeroPinInput = !seedWheelZeroPinInputRev;
nerit 3:a469bbd294b5 1641 #else
nerit 8:310f9e4eac7b 1642 if ((prePosSD-500) >= SDsectorStep) {
nerit 3:a469bbd294b5 1643 seedWheelZeroPinInput=1;
nerit 3:a469bbd294b5 1644 }
nerit 8:310f9e4eac7b 1645 if ((prePosSD > 500)&&(prePosSD<580)) {
nerit 3:a469bbd294b5 1646 seedWheelZeroPinInput=0;
nerit 3:a469bbd294b5 1647 }
nerit 8:310f9e4eac7b 1648 #endif
nerit 3:a469bbd294b5 1649 TBzeroPinInput = !TBzeroPinInputRev;
nerit 8:310f9e4eac7b 1650
nerit 3:a469bbd294b5 1651 // se quinconce attivo ed unita' master invia segnale di sincro
nerit 3:a469bbd294b5 1652 #if defined(speedMaster)
nerit 8:310f9e4eac7b 1653 if (seedWheelZeroPinInput==1) {
nerit 3:a469bbd294b5 1654 quinconceOut=0;
nerit 3:a469bbd294b5 1655 }
nerit 8:310f9e4eac7b 1656 if (((double)(prePosSD-500) >= (SDsectorStep/((double)quincSector)))&&(quinconceOut=1)) {
nerit 3:a469bbd294b5 1657 quinconceOut=1;
nerit 3:a469bbd294b5 1658 }
nerit 8:310f9e4eac7b 1659 if (quinconceActive==1) {
nerit 8:310f9e4eac7b 1660 if ((quinconceOut==1)&&(oldQuinconceOut==1)) {
nerit 3:a469bbd294b5 1661 posForQuinc=500;
nerit 3:a469bbd294b5 1662 oldQuinconceOut=0;
nerit 3:a469bbd294b5 1663 }
nerit 8:310f9e4eac7b 1664 if (((double)posForQuinc-500.0f)> (SDsectorStep-200)) {
nerit 3:a469bbd294b5 1665 oldQuinconceOut=1;
nerit 3:a469bbd294b5 1666 }
nerit 8:310f9e4eac7b 1667 }
nerit 3:a469bbd294b5 1668 #endif
nerit 3:a469bbd294b5 1669
nerit 3:a469bbd294b5 1670 // simulazione velocita
nerit 8:310f9e4eac7b 1671 if (enableSimula==1) {
nerit 3:a469bbd294b5 1672 double TMT = 0.0f;
nerit 8:310f9e4eac7b 1673 if (speedSimula > 0) {
nerit 3:a469bbd294b5 1674 TMT = (double)(speedSimula) * 100.0f /3600.0f;
nerit 3:a469bbd294b5 1675 pulseSpeedInterval = pulseDistance / TMT;
nerit 8:310f9e4eac7b 1676 } else {
nerit 3:a469bbd294b5 1677 pulseSpeedInterval = 10000.0f;
nerit 8:310f9e4eac7b 1678 }
nerit 8:310f9e4eac7b 1679 if (avviaSimula==1) {
nerit 8:310f9e4eac7b 1680 if(oldSimulaSpeed!=pulseSpeedInterval) {
nerit 3:a469bbd294b5 1681 spedSimclock.attach_us(&speedSimulationClock,pulseSpeedInterval);
nerit 3:a469bbd294b5 1682 oldSimulaSpeed=pulseSpeedInterval;
nerit 3:a469bbd294b5 1683 }
nerit 8:310f9e4eac7b 1684 } else {
nerit 3:a469bbd294b5 1685 oldSimulaSpeed=10000.0f;
nerit 3:a469bbd294b5 1686 spedSimclock.detach();
nerit 3:a469bbd294b5 1687 }
nerit 8:310f9e4eac7b 1688 } else {
nerit 3:a469bbd294b5 1689 spedSimclock.detach();
nerit 3:a469bbd294b5 1690 }
nerit 8:310f9e4eac7b 1691
nerit 3:a469bbd294b5 1692 //*******************************************************
nerit 3:a469bbd294b5 1693 // determina se sono in bassa velocità per il controllo di TB
nerit 8:310f9e4eac7b 1694 if (speedOfSeedWheel<=minSeedSpeed) {
nerit 8:310f9e4eac7b 1695 if (lowSpeedRequired==0) {
nerit 3:a469bbd294b5 1696 ritardaLowSpeed.reset();
nerit 3:a469bbd294b5 1697 ritardaLowSpeed.start();
nerit 3:a469bbd294b5 1698 }
nerit 3:a469bbd294b5 1699 lowSpeedRequired=1;
nerit 8:310f9e4eac7b 1700 } else {
nerit 8:310f9e4eac7b 1701 if (lowSpeedRequired==1) {
nerit 3:a469bbd294b5 1702 lowSpeedRequired=0;
nerit 3:a469bbd294b5 1703 ritardaLowSpeed.reset();
nerit 3:a469bbd294b5 1704 ritardaLowSpeed.stop();
nerit 3:a469bbd294b5 1705 }
nerit 3:a469bbd294b5 1706 }
nerit 3:a469bbd294b5 1707
nerit 8:310f9e4eac7b 1708 if (ritardaLowSpeed.read_ms()> 2000) {
nerit 3:a469bbd294b5 1709 lowSpeed=1;
nerit 8:310f9e4eac7b 1710 } else {
nerit 3:a469bbd294b5 1711 lowSpeed=0;
nerit 3:a469bbd294b5 1712 }
bcostm 2:35f13b7f3659 1713
nerit 3:a469bbd294b5 1714 //**************************************************************************************************************
nerit 3:a469bbd294b5 1715 //**************************************************************************************************************
nerit 3:a469bbd294b5 1716 // LOGICAL CONTROLS
nerit 3:a469bbd294b5 1717 //**************************************************************************************************************
nerit 3:a469bbd294b5 1718 //**************************************************************************************************************
nerit 8:310f9e4eac7b 1719
nerit 8:310f9e4eac7b 1720 if (inProva==0) {
nerit 8:310f9e4eac7b 1721 if ((startCycleSimulation==0)&&(enableSimula==0)) {
nerit 6:e8c18f0f399a 1722 runRequestBuf=1;//0;
nerit 8:310f9e4eac7b 1723 } else {
nerit 6:e8c18f0f399a 1724 runRequestBuf=1;//0;
nerit 6:e8c18f0f399a 1725 }
nerit 8:310f9e4eac7b 1726 if ((tractorSpeedRead==0)&&(speedFilter.read_ms()>3)) {
nerit 6:e8c18f0f399a 1727 oldTractorSpeedRead=0;
nerit 3:a469bbd294b5 1728 }
nerit 3:a469bbd294b5 1729 // ----------------------------------------
nerit 3:a469bbd294b5 1730 // filtra il segnale dei becchi per misurare il tempo di intervallo tra loro
nerit 3:a469bbd294b5 1731 // ----------------------------------------
nerit 8:310f9e4eac7b 1732 if ((seedWheelZeroPinInput==0)&&(oldSeedWheelZeroPinInput==1)) {
nerit 8:310f9e4eac7b 1733 if(seedFilter.read_ms()>=4) {
nerit 3:a469bbd294b5 1734 oldSeedWheelZeroPinInput=0;
nerit 3:a469bbd294b5 1735 SDzeroDebounced=0;
nerit 3:a469bbd294b5 1736 }
nerit 3:a469bbd294b5 1737 }
nerit 8:310f9e4eac7b 1738 if ((seedWheelZeroPinInput==1)&&(oldSeedWheelZeroPinInput==0)) {
nerit 3:a469bbd294b5 1739 timeIntraPick = (double)intraPickTimer.read_ms();
nerit 3:a469bbd294b5 1740 prePosSD=500; // preposizionamento SD
nerit 3:a469bbd294b5 1741 intraPickTimer.reset();
nerit 3:a469bbd294b5 1742 rotationTimeOut.reset();
nerit 8:310f9e4eac7b 1743 seedFilter.reset();
nerit 3:a469bbd294b5 1744 sincroTimer.reset();
nerit 3:a469bbd294b5 1745 oldSeedWheelZeroPinInput=1;
nerit 3:a469bbd294b5 1746 quincTime.reset();
nerit 3:a469bbd294b5 1747 quincTimeSD.reset();
nerit 3:a469bbd294b5 1748 SDzeroDebounced=1;
nerit 3:a469bbd294b5 1749 sincroQui=1;
nerit 3:a469bbd294b5 1750 SDwheelTimer.reset();
nerit 13:d1030d4e51a8 1751 dcEncoderCnt=0;
nerit 3:a469bbd294b5 1752 #if defined(speedMaster)
nerit 8:310f9e4eac7b 1753 if (quinconceActive==0) {
nerit 3:a469bbd294b5 1754 posForQuinc=500;
nerit 3:a469bbd294b5 1755 }
nerit 17:9629eb019892 1756 if (checkSDrotation==0) {
nerit 17:9629eb019892 1757 checkSDrotation=1;
nerit 17:9629eb019892 1758 SDwheelTimer.start();
nerit 17:9629eb019892 1759 }
nerit 3:a469bbd294b5 1760 #endif
nerit 8:310f9e4eac7b 1761 if (quincCnt<10) {
nerit 3:a469bbd294b5 1762 quincCnt++;
nerit 3:a469bbd294b5 1763 }
nerit 8:310f9e4eac7b 1764 if ((aspettaStart==0)&&(lowSpeed==1)) {
nerit 3:a469bbd294b5 1765 beccoPronto=1;
nerit 3:a469bbd294b5 1766 }
nerit 3:a469bbd294b5 1767 lockStart=0;
nerit 3:a469bbd294b5 1768 double fase1=0.0f;
nerit 3:a469bbd294b5 1769 forzaFase=0;
nerit 3:a469bbd294b5 1770 double limite=fixedStepGiroSD/pickNumber;
nerit 8:310f9e4eac7b 1771 if (tamburoStandard==0) {
nerit 8:310f9e4eac7b 1772 fase1=TBdeltaStep;
nerit 8:310f9e4eac7b 1773 } else {
nerit 8:310f9e4eac7b 1774 if(speedForCorrection >= speedOfSeedWheel) {
nerit 3:a469bbd294b5 1775 fase1=TBdeltaStep;
nerit 8:310f9e4eac7b 1776 } else {
nerit 3:a469bbd294b5 1777 fase1=(TBdeltaStep)-(((speedOfSeedWheel)/maxWorkSpeed)*(TBfaseStep));
nerit 3:a469bbd294b5 1778 }
nerit 8:310f9e4eac7b 1779 if (fase1 > limite) {
nerit 3:a469bbd294b5 1780 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 3:a469bbd294b5 1781 }
nerit 8:310f9e4eac7b 1782 if ((fase1 > (limite -20.0f))&&(fase1<(limite +5.0f))) {
nerit 3:a469bbd294b5 1783 fase1 = limite -20.0f; // se la fase è molto vicina al limite interpone uno spazio minimo di lavoro del sincronismo
nerit 3:a469bbd294b5 1784 forzaFase=1;
nerit 3:a469bbd294b5 1785 }
nerit 3:a469bbd294b5 1786 trigRepos=1;
nerit 3:a469bbd294b5 1787 }
nerit 3:a469bbd294b5 1788 fase = (uint32_t)fase1+500;
nerit 3:a469bbd294b5 1789 #if defined(pcSerial)
nerit 3:a469bbd294b5 1790 #if defined(inCorre)
nerit 3:a469bbd294b5 1791 pc.printf(" limite %f", limite);
nerit 3:a469bbd294b5 1792 pc.printf(" delta %f", TBdeltaStep);
nerit 3:a469bbd294b5 1793 pc.printf(" faseStep %f", TBfaseStep);
nerit 3:a469bbd294b5 1794 pc.printf(" fase %d",fase);
nerit 3:a469bbd294b5 1795 pc.printf(" forzaFase %d",forzaFase);
nerit 3:a469bbd294b5 1796 pc.printf(" trigRepos %d", trigRepos);
nerit 3:a469bbd294b5 1797 pc.printf(" ActualSD: %d",SDactualPosition);
nerit 3:a469bbd294b5 1798 pc.printf(" SpeedWheel: %f",speedOfSeedWheel);
nerit 3:a469bbd294b5 1799 pc.printf(" SPEED: %f \n",tractorSpeed_MtS_timed);
nerit 3:a469bbd294b5 1800 #endif
nerit 3:a469bbd294b5 1801 #endif
nerit 17:9629eb019892 1802 if (tractorSpeed_MtS_timed>0.0f){
nerit 17:9629eb019892 1803 if (timeIntraPick >= (memoIntraPick*2)) {
nerit 17:9629eb019892 1804 if ((aspettaStart==0)) {
nerit 17:9629eb019892 1805 if (firstStart==0) {
nerit 17:9629eb019892 1806 all_pickSignal=1;
nerit 17:9629eb019892 1807 }
nerit 3:a469bbd294b5 1808 }
nerit 3:a469bbd294b5 1809 }
nerit 3:a469bbd294b5 1810 }
nerit 3:a469bbd294b5 1811 memoIntraPick = timeIntraPick;
nerit 8:310f9e4eac7b 1812 if ((speedFromPick==1)&&(encoder==false)) {
nerit 3:a469bbd294b5 1813 speedOfSeedWheel=((seedPerimeter / pickNumber)/timeIntraPick)*1000.0f;
nerit 10:9e70619e97ab 1814 #if defined(pcSerial)
nerit 10:9e70619e97ab 1815 #if defined(Qnca)
nerit 10:9e70619e97ab 1816 pc.printf("perim: %f pickN: %f sped: %f\n", seedPerimeter, pickNumber,speedOfSeedWheel);
nerit 10:9e70619e97ab 1817 #endif
nerit 3:a469bbd294b5 1818 #endif
nerit 3:a469bbd294b5 1819 }
nerit 8:310f9e4eac7b 1820 if (encoder==false) {
nerit 3:a469bbd294b5 1821 pulseRised2=1;
nerit 3:a469bbd294b5 1822 }
nerit 3:a469bbd294b5 1823 #if defined(speedMaster)
nerit 8:310f9e4eac7b 1824 if ((tractorSpeed_MtS_timed==0.0f)) {
nerit 8:310f9e4eac7b 1825 if ((firstStart==0)&&(simOk==0)&&(enableSimula==0)) {
nerit 10:9e70619e97ab 1826 cntSpeedError++;
nerit 10:9e70619e97ab 1827 if (cntSpeedError >= 3){
nerit 10:9e70619e97ab 1828 all_noSpeedSen=1;
nerit 10:9e70619e97ab 1829 }
nerit 3:a469bbd294b5 1830 }
nerit 3:a469bbd294b5 1831 }
nerit 17:9629eb019892 1832 if (tractorSpeed_MtS_timed>0.2f){
nerit 17:9629eb019892 1833 double oldLastPr = (double)oldLastPulseRead*1.8f;
nerit 17:9629eb019892 1834 if((double)speedTimeOut.read_us()> (oldLastPr)) {
nerit 17:9629eb019892 1835 if ((firstStart==0)&&(simOk==0)&&(enableSimula==0)) {
nerit 17:9629eb019892 1836 all_speedError =1;
nerit 17:9629eb019892 1837 }
nerit 3:a469bbd294b5 1838 }
nerit 3:a469bbd294b5 1839 }
nerit 3:a469bbd294b5 1840 #endif
nerit 3:a469bbd294b5 1841 //*******************************************
nerit 3:a469bbd294b5 1842 // esegue calcolo clock per la generazione della posizione teorica
nerit 3:a469bbd294b5 1843 // la realtà in base al segnale di presenza del becco
nerit 8:310f9e4eac7b 1844 if (speedOfSeedWheel < 0.002f) {
nerit 10:9e70619e97ab 1845 #if defined(pcSerial)
nerit 10:9e70619e97ab 1846 #if defined(checkLoopb)
nerit 10:9e70619e97ab 1847 pc.printf("forza\n");
nerit 10:9e70619e97ab 1848 #endif
nerit 8:310f9e4eac7b 1849 #endif
nerit 6:e8c18f0f399a 1850 speedOfSeedWheel=0.001f;
nerit 6:e8c18f0f399a 1851 }
nerit 8:310f9e4eac7b 1852 aggioVelocita();
nerit 3:a469bbd294b5 1853 }
nerit 3:a469bbd294b5 1854 // ----------------------------------------
nerit 8:310f9e4eac7b 1855 // check SD fase
nerit 8:310f9e4eac7b 1856 if ((prePosSD >= fase)||(forzaFase==1)) { //&&(prePosSD < (fase +30))){
nerit 3:a469bbd294b5 1857 forzaFase=0;
nerit 8:310f9e4eac7b 1858 if (trigRepos==1) {
nerit 3:a469bbd294b5 1859 SDactualPosition=0;
nerit 8:310f9e4eac7b 1860 if ((countCicli<30)&&(trigCicli==0)) {
nerit 6:e8c18f0f399a 1861 countCicli++;
nerit 6:e8c18f0f399a 1862 trigCicli=1;
nerit 6:e8c18f0f399a 1863 }
nerit 8:310f9e4eac7b 1864 if(countCicli>=cicliAspettaStart) {
nerit 6:e8c18f0f399a 1865 aspettaStart=0;
nerit 6:e8c18f0f399a 1866 }
nerit 8:310f9e4eac7b 1867 if ((lowSpeed==0)&&(aspettaStart==0)&&(lockStart==0)) {
nerit 6:e8c18f0f399a 1868 syncroCheck=1;
nerit 6:e8c18f0f399a 1869 beccoPronto=0;
nerit 6:e8c18f0f399a 1870 }
nerit 8:310f9e4eac7b 1871 if (trigTB==0) {
nerit 3:a469bbd294b5 1872 inhibit=1;
nerit 3:a469bbd294b5 1873 trigSD=1;
nerit 8:310f9e4eac7b 1874 } else {
nerit 3:a469bbd294b5 1875 inhibit=0;
nerit 3:a469bbd294b5 1876 trigTB=0;
nerit 3:a469bbd294b5 1877 trigSD=0;
nerit 3:a469bbd294b5 1878 }
nerit 3:a469bbd294b5 1879 trigRepos=0;
nerit 3:a469bbd294b5 1880 }
nerit 8:310f9e4eac7b 1881 } else {
nerit 3:a469bbd294b5 1882 trigCicli=0;
nerit 3:a469bbd294b5 1883 }
nerit 8:310f9e4eac7b 1884 // ----------------------------------------
nerit 3:a469bbd294b5 1885 // filtra il segnale del tamburo per lo stop in fase del tamburo stesso
nerit 8:310f9e4eac7b 1886 if (TBzeroPinInput==0) {
nerit 8:310f9e4eac7b 1887 if (TBfilter.read_ms()>=2) {
nerit 8:310f9e4eac7b 1888 oldTBzeroPinInput=0;
nerit 8:310f9e4eac7b 1889 }
nerit 8:310f9e4eac7b 1890 }
nerit 8:310f9e4eac7b 1891 if ((TBzeroPinInput==1)&&(oldTBzeroPinInput==0)) {
nerit 3:a469bbd294b5 1892 oldTBzeroPinInput=1;
nerit 8:310f9e4eac7b 1893 if (loadDaCanInCorso==0) {
nerit 3:a469bbd294b5 1894 stopCicloTB=1;
nerit 3:a469bbd294b5 1895 startCicloTB=0;
nerit 3:a469bbd294b5 1896 }
nerit 3:a469bbd294b5 1897 TBfilter.reset();
nerit 3:a469bbd294b5 1898 TBzeroCyclePulse=1;
nerit 8:310f9e4eac7b 1899 #if defined(runner)
nerit 8:310f9e4eac7b 1900 legPos.detach();
nerit 8:310f9e4eac7b 1901 TBoldPosition= (uint32_t) motor->get_position();
nerit 8:310f9e4eac7b 1902 legPos.attach(&step_Reading,0.002f);
nerit 8:310f9e4eac7b 1903 #if defined(pcSerial)
nerit 8:310f9e4eac7b 1904 #if defined(TBperS)
nerit 8:310f9e4eac7b 1905 pc.printf("TBoldPos: %d\n",TBoldPosition);
nerit 8:310f9e4eac7b 1906 #endif
nerit 8:310f9e4eac7b 1907 #endif
nerit 8:310f9e4eac7b 1908
nerit 8:310f9e4eac7b 1909 #else
nerit 8:310f9e4eac7b 1910 TBactualPosition=0;
nerit 8:310f9e4eac7b 1911 #endif
nerit 8:310f9e4eac7b 1912 if (cntTbError>0) {
nerit 3:a469bbd294b5 1913 cntCellsCorrect++;
nerit 3:a469bbd294b5 1914 }
nerit 8:310f9e4eac7b 1915 if (cntCellsCorrect>3) {
nerit 3:a469bbd294b5 1916 cntTbError=0;
nerit 3:a469bbd294b5 1917 cntCellsCorrect=0;
nerit 3:a469bbd294b5 1918 }
nerit 3:a469bbd294b5 1919 // conteggio celle erogate
nerit 8:310f9e4eac7b 1920 if (cellsCounterLow < 0xFF) {
nerit 3:a469bbd294b5 1921 cellsCounterLow++;
nerit 8:310f9e4eac7b 1922 } else {
nerit 3:a469bbd294b5 1923 cellsCounterHig++;
nerit 3:a469bbd294b5 1924 cellsCounterLow=0;
nerit 3:a469bbd294b5 1925 }
nerit 3:a469bbd294b5 1926 // ciclo conteggio celle per carico manuale
nerit 8:310f9e4eac7b 1927 if (loadDaCanInCorso==1) {
nerit 3:a469bbd294b5 1928 cntCellsForLoad++;
nerit 14:e2b5efa06c41 1929 if (cntCellsForLoad >= 10) {
nerit 8:310f9e4eac7b 1930 stopCicloTB=1;
nerit 3:a469bbd294b5 1931 cntCellsForLoad=0;
nerit 3:a469bbd294b5 1932 }
nerit 8:310f9e4eac7b 1933 } else {
nerit 3:a469bbd294b5 1934 cntCellsForLoad=0;
nerit 3:a469bbd294b5 1935 }
nerit 3:a469bbd294b5 1936 // inibizione controllo di sincro per fuori fase
nerit 8:310f9e4eac7b 1937 if (trigSD==0) {
nerit 3:a469bbd294b5 1938 inhibit=1;
nerit 3:a469bbd294b5 1939 trigTB=1;
nerit 8:310f9e4eac7b 1940 } else {
nerit 3:a469bbd294b5 1941 inhibit=0;
nerit 3:a469bbd294b5 1942 trigTB=0;
nerit 3:a469bbd294b5 1943 trigSD=0;
nerit 3:a469bbd294b5 1944 }
nerit 3:a469bbd294b5 1945 // conta le celle indietro per sbloccare il tamburo
nerit 8:310f9e4eac7b 1946 if ((TBmotorDirecti==TBreverse)&&(erroreTamburo==1)) {
nerit 3:a469bbd294b5 1947 cntCellsForReload++;
nerit 8:310f9e4eac7b 1948 if (cntCellsForReload >= cellsCountSet) {
nerit 3:a469bbd294b5 1949 TBmotorDirecti=TBforward; // rotazione normale
nerit 8:310f9e4eac7b 1950 #if defined(runner)
nerit 9:7f02256f6e8f 1951 #if defined(Zucca)
nerit 9:7f02256f6e8f 1952 motor->run(StepperMotor::BWD);
nerit 9:7f02256f6e8f 1953 #else
nerit 9:7f02256f6e8f 1954 motor->run(StepperMotor::FWD);
nerit 9:7f02256f6e8f 1955 #endif
nerit 8:310f9e4eac7b 1956 #else
nerit 9:7f02256f6e8f 1957 #if defined(Zucca)
nerit 9:7f02256f6e8f 1958 motor->step_clock_mode_enable(StepperMotor::FWD);
nerit 9:7f02256f6e8f 1959 #else
nerit 9:7f02256f6e8f 1960 motor->step_clock_mode_enable(StepperMotor::FWD);
nerit 9:7f02256f6e8f 1961 #endif
nerit 8:310f9e4eac7b 1962 #endif
nerit 3:a469bbd294b5 1963 erroreTamburo=0;
nerit 3:a469bbd294b5 1964 cntCellsCorrect=0;
nerit 8:310f9e4eac7b 1965 }
nerit 3:a469bbd294b5 1966 }
nerit 3:a469bbd294b5 1967 #if defined(seedSensor)
nerit 3:a469bbd294b5 1968 resetDelay();
nerit 3:a469bbd294b5 1969 delaySeedCheck.start();
nerit 3:a469bbd294b5 1970 #endif
nerit 3:a469bbd294b5 1971 }
nerit 8:310f9e4eac7b 1972 if ((double)TBactualPosition > ((TBgiroStep/cellsNumber)*1.8f)&&(erroreTamburo==0)) {
nerit 8:310f9e4eac7b 1973 if (firstStart==0) {
nerit 17:9629eb019892 1974 if (tractorSpeed_MtS_timed>0.5f){
nerit 17:9629eb019892 1975 if (cntTbError>5) {
nerit 17:9629eb019892 1976 all_cellSignal=1;
nerit 17:9629eb019892 1977 #if defined(seedSensor)
nerit 17:9629eb019892 1978 resetDelay();
nerit 17:9629eb019892 1979 #endif
nerit 17:9629eb019892 1980 }
nerit 3:a469bbd294b5 1981 }
nerit 3:a469bbd294b5 1982 }
nerit 8:310f9e4eac7b 1983 if (erroreTamburo==0) {
nerit 17:9629eb019892 1984 if (cellsCountSet>1){
nerit 17:9629eb019892 1985 erroreTamburo=1;
nerit 17:9629eb019892 1986 TBmotorDirecti=TBreverse; // rotazione inversa
nerit 17:9629eb019892 1987 #if defined(runner)
nerit 17:9629eb019892 1988 #if defined(Zucca)
nerit 17:9629eb019892 1989 motor->run(StepperMotor::FWD);
nerit 17:9629eb019892 1990 #else
nerit 17:9629eb019892 1991 motor->run(StepperMotor::BWD);
nerit 17:9629eb019892 1992 #endif
nerit 9:7f02256f6e8f 1993 #else
nerit 17:9629eb019892 1994 #if defined(Zucca)
nerit 17:9629eb019892 1995 motor->step_clock_mode_enable(StepperMotor::FWD);
nerit 17:9629eb019892 1996 #else
nerit 17:9629eb019892 1997 motor->step_clock_mode_enable(StepperMotor::BWD);
nerit 17:9629eb019892 1998 #endif
nerit 9:7f02256f6e8f 1999 #endif
nerit 17:9629eb019892 2000 }
nerit 3:a469bbd294b5 2001 cntCellsForReload=0;
nerit 17:9629eb019892 2002 if (tractorSpeed_MtS_timed>0.0f){
nerit 17:9629eb019892 2003 cntTbError++;
nerit 17:9629eb019892 2004 aggiornati();
nerit 17:9629eb019892 2005 }else{
nerit 17:9629eb019892 2006 cntTbError=0;
nerit 17:9629eb019892 2007 }
nerit 3:a469bbd294b5 2008 #if defined(seedSensor)
nerit 3:a469bbd294b5 2009 resetDelay();
nerit 3:a469bbd294b5 2010 #endif
nerit 3:a469bbd294b5 2011 }
nerit 3:a469bbd294b5 2012 }
nerit 17:9629eb019892 2013 if (tractorSpeed_MtS_timed>0.0f){
nerit 17:9629eb019892 2014 if (((double)TBactualPosition > ((TBgiroStep/cellsNumber)*3.0f))||(cntTbError>5)) {
nerit 17:9629eb019892 2015 if (firstStart==0) {
nerit 17:9629eb019892 2016 all_noStepRota=1;
nerit 17:9629eb019892 2017 #if defined(seedSensor)
nerit 17:9629eb019892 2018 resetDelay();
nerit 17:9629eb019892 2019 #endif
nerit 17:9629eb019892 2020 }
nerit 17:9629eb019892 2021 cntTbError=0;
nerit 3:a469bbd294b5 2022 }
nerit 3:a469bbd294b5 2023 }
nerit 8:310f9e4eac7b 2024 // ----------------------------------------
nerit 3:a469bbd294b5 2025 // read and manage joystick
nerit 8:310f9e4eac7b 2026 if (loadDaCan==1) {
nerit 8:310f9e4eac7b 2027 if (tractorSpeed_MtS_timed==0.0f) {
nerit 8:310f9e4eac7b 2028 #if defined(pcSerial)
nerit 8:310f9e4eac7b 2029 #if defined(checkLoop)
nerit 8:310f9e4eac7b 2030 pc.printf("daCAN\n");
nerit 6:e8c18f0f399a 2031 #endif
nerit 8:310f9e4eac7b 2032 #endif
nerit 6:e8c18f0f399a 2033 ciclaTB();
nerit 8:310f9e4eac7b 2034 }
nerit 3:a469bbd294b5 2035 }
nerit 8:310f9e4eac7b 2036
nerit 8:310f9e4eac7b 2037 //***************************************************************************************************
nerit 8:310f9e4eac7b 2038 // pulseRised define the event of speed wheel pulse occurs
nerit 8:310f9e4eac7b 2039 //
nerit 3:a469bbd294b5 2040 //double maxInterval = pulseDistance/minWorkSpeed;
nerit 3:a469bbd294b5 2041 //double minIntervalPulse = pulseDistance/maxWorkSpeed;
nerit 8:310f9e4eac7b 2042 if (pulseRised==1) {
nerit 12:b0fc1d313813 2043 if (enableSpeed<10) {enableSpeed++;}
nerit 3:a469bbd294b5 2044 pulseRised=0;
nerit 3:a469bbd294b5 2045 pulseRised1=1;
nerit 3:a469bbd294b5 2046 speedMediaCalc();
nerit 3:a469bbd294b5 2047 // calcola velocità trattore
nerit 8:310f9e4eac7b 2048 if(enableSpeed>=2) {
nerit 8:310f9e4eac7b 2049 if ((pulseSpeedInterval>=0.0f)) { //minIntervalPulse)&&(pulseSpeedInterval<maxInterval)){
nerit 8:310f9e4eac7b 2050 if((quincCnt<3)||(speedFromMaster==0.0f)||(enableSimula==1)) {
nerit 13:d1030d4e51a8 2051 //TODO: limite inferiore e superiore per non modificare la velocità selezionata
nerit 13:d1030d4e51a8 2052 // serve un timer a 5 secondi dopo l'avviamento che fa partire il controllo
nerit 3:a469bbd294b5 2053 tractorSpeed_MtS_timed = ((pulseDistance / pulseSpeedInterval)); // tractor speed (unit= Mt/S) from pulse time interval
nerit 13:d1030d4e51a8 2054 if (delaySpeedCheck.read()>=5.0f){
nerit 13:d1030d4e51a8 2055 if ((tractorSpeed_MtS_timed<=(oldLocalTractorSpeed+hiLimitSpeed))&&(tractorSpeed_MtS_timed>=(oldLocalTractorSpeed-loLimitSpeed))){
nerit 13:d1030d4e51a8 2056 tractorSpeed_MtS_timed=oldLocalTractorSpeed;
nerit 13:d1030d4e51a8 2057 }
nerit 13:d1030d4e51a8 2058 }
nerit 10:9e70619e97ab 2059 oldLocalTractorSpeed = tractorSpeed_MtS_timed;
nerit 3:a469bbd294b5 2060 }
nerit 17:9629eb019892 2061 /*if (checkSDrotation==0) {
nerit 3:a469bbd294b5 2062 checkSDrotation=1;
nerit 3:a469bbd294b5 2063 SDwheelTimer.start();
nerit 17:9629eb019892 2064 }*/
nerit 3:a469bbd294b5 2065 }
nerit 3:a469bbd294b5 2066 }
nerit 3:a469bbd294b5 2067 speedTimeOut.reset();
nerit 8:310f9e4eac7b 2068 } else {
nerit 3:a469bbd294b5 2069 double oldLastPr = (double)oldLastPulseRead*1.7f;
nerit 8:310f9e4eac7b 2070 if((double)speedTimeOut.read_us()> (oldLastPr)) {
nerit 3:a469bbd294b5 2071 tractorSpeed_MtS_timed = 0.0f;
nerit 10:9e70619e97ab 2072 oldLocalTractorSpeed=0.0f;
nerit 10:9e70619e97ab 2073 #if defined(seedSensor)
nerit 10:9e70619e97ab 2074 resetDelay();
nerit 10:9e70619e97ab 2075 #endif
nerit 3:a469bbd294b5 2076 pntMedia=0;
nerit 3:a469bbd294b5 2077 speedTimeOut.reset();
nerit 3:a469bbd294b5 2078 enableSpeed=0;
nerit 3:a469bbd294b5 2079 quincCnt=0;
nerit 3:a469bbd294b5 2080 }
nerit 3:a469bbd294b5 2081 }
nerit 8:310f9e4eac7b 2082
nerit 10:9e70619e97ab 2083 #if defined(seedSensor)
nerit 10:9e70619e97ab 2084 if (seedSensorEnable==true) {
nerit 10:9e70619e97ab 2085 if (delaySeedCheck.read_ms()>100) {
nerit 10:9e70619e97ab 2086 if (seedSee==0) {
nerit 10:9e70619e97ab 2087 all_noSeedOnCe=1;
nerit 10:9e70619e97ab 2088 }
nerit 10:9e70619e97ab 2089 resetDelay();
nerit 3:a469bbd294b5 2090 }
nerit 3:a469bbd294b5 2091 }
nerit 10:9e70619e97ab 2092 #endif
nerit 3:a469bbd294b5 2093 // esegue il controllo di velocità minima
nerit 3:a469bbd294b5 2094 /*if ((double)speedTimer.read_ms()>=maxInterval){
nerit 3:a469bbd294b5 2095 tractorSpeed_MtS_timed = 0.0f;
nerit 3:a469bbd294b5 2096 enableSpeed=0;
nerit 3:a469bbd294b5 2097 }*/
nerit 3:a469bbd294b5 2098 // esegue il controllo di velocità massima
nerit 3:a469bbd294b5 2099 /*if ((double)speedTimer.read_ms()<=minIntervalPulse){
nerit 3:a469bbd294b5 2100 tractorSpeed_MtS_timed = 4.5f;
nerit 3:a469bbd294b5 2101 }*/
nerit 8:310f9e4eac7b 2102 //***************************************************************************************************************
nerit 8:310f9e4eac7b 2103 // cycle logic control section
nerit 8:310f9e4eac7b 2104 //***************************************************************************************************************
nerit 8:310f9e4eac7b 2105 if (enableSimula==1) {
nerit 8:310f9e4eac7b 2106 if(simOk==0) {
nerit 8:310f9e4eac7b 2107 tractorSpeed_MtS_timed=0.0f;
nerit 10:9e70619e97ab 2108 oldLocalTractorSpeed=0.0f;
nerit 8:310f9e4eac7b 2109 }
nerit 8:310f9e4eac7b 2110 }
nerit 8:310f9e4eac7b 2111 if ((tractorSpeed_MtS_timed>0.01f)) {
nerit 8:310f9e4eac7b 2112 #if defined(pcSerial)
nerit 8:310f9e4eac7b 2113 #if defined(Qncc)
nerit 8:310f9e4eac7b 2114 pc.printf("TsP: %f SpW: %f InPic: %f EPerc: %f Duty:%f \n",tractorSpeed_MtS_timed,speedOfSeedWheel,timeIntraPick, errorePercentuale, dcActualDuty);
nerit 8:310f9e4eac7b 2115
nerit 8:310f9e4eac7b 2116 #endif
nerit 8:310f9e4eac7b 2117 #endif
nerit 8:310f9e4eac7b 2118 cycleStopRequest=1;
nerit 8:310f9e4eac7b 2119 // calcola il tempo teorico di passaggio becchi sulla base della velocità del trattore
nerit 8:310f9e4eac7b 2120 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 8:310f9e4eac7b 2121 if (encoder==false) {
nerit 8:310f9e4eac7b 2122 if (speedFromPick==1) {
nerit 8:310f9e4eac7b 2123 tempoTraBecchi_mS = (tempoGiroSD / pickNumber)*1000.0f; // tempo tra due impulsi dai becchi in millisecondi ( circa 157mS a 4,5Km/h)
nerit 8:310f9e4eac7b 2124 } else {
nerit 8:310f9e4eac7b 2125 tempoTraBecchi_mS = (tempoGiroSD / 25.0f)*1000.0f; // tempo tra due impulsi dai becchi in millisecondi ( circa 157mS a 4,5Km/h)
nerit 8:310f9e4eac7b 2126 }
nerit 8:310f9e4eac7b 2127 } else {
nerit 17:9629eb019892 2128 tempoTraBecchi_mS = (tempoGiroSD / (SDreductionRatio*dcPulseTurn))*1000.0f; // tempo tra due impulsi dai becchi in millisecondi ( circa 157mS a 4,5Km/h)
nerit 5:2a3a64b52f54 2129 #if defined(pcSerial)
nerit 8:310f9e4eac7b 2130 #if defined(Qnce)
nerit 8:310f9e4eac7b 2131 pc.printf("tempoGiroSD: %f SDreductionRatio: %f tempoBecchi:%f\n",tempoGiroSD,SDreductionRatio,tempoTraBecchi_mS);
nerit 5:2a3a64b52f54 2132 #endif
nerit 5:2a3a64b52f54 2133 #endif
nerit 8:310f9e4eac7b 2134 #if !defined(speedMaster)
nerit 10:9e70619e97ab 2135 double tempoGiroSDfomMaster = seedPerimeter / speedFromMaster;
nerit 10:9e70619e97ab 2136 tempoBecchiPerQuinc = (tempoGiroSDfomMaster / pickNumber)*1000.0f;
nerit 8:310f9e4eac7b 2137 #endif
nerit 8:310f9e4eac7b 2138 }
nerit 8:310f9e4eac7b 2139 //*******************************************
nerit 8:310f9e4eac7b 2140 // segue calcolo duty cycle comando motore DC per allinearsi con la velocità del trattore
nerit 8:310f9e4eac7b 2141 double dutyTeorico = 0.00;
nerit 12:b0fc1d313813 2142 double deltaV=0.0f;
nerit 12:b0fc1d313813 2143 double deltaD=0.0f;
nerit 12:b0fc1d313813 2144 double setV=0.0f;
nerit 12:b0fc1d313813 2145 double teoriaC=0.0f;
nerit 14:e2b5efa06c41 2146 double speedCorrectionMachine=0.0f;
nerit 17:9629eb019892 2147 //#if defined(speedMaster)
nerit 14:e2b5efa06c41 2148 speedCorrectionMachine=(seedWheelDiameter/(seedWheelDiameter-(deepOfSeed*2.0f)))*tractorSpeed_MtS_timed;
nerit 17:9629eb019892 2149 //#else
nerit 17:9629eb019892 2150 // speedCorrectionMachine=tractorSpeed_MtS_timed;
nerit 17:9629eb019892 2151 //#endif
nerit 14:e2b5efa06c41 2152 //if ((tractorSpeed_MtS_timed>0.0)&&(tractorSpeed_MtS_timed<tabSpeed[0])) {
nerit 17:9629eb019892 2153 if ((speedCorrectionMachine>0.0f)&&(speedCorrectionMachine<tabSpeed[0])) {
nerit 8:310f9e4eac7b 2154 dutyTeorico = tabComan[0];
nerit 8:310f9e4eac7b 2155 }
nerit 17:9629eb019892 2156 for (int ii = 0; ii<tabeling; ii++) { // era ii<16
nerit 14:e2b5efa06c41 2157 if ((speedCorrectionMachine>=tabSpeed[ii])&&(speedCorrectionMachine<tabSpeed[ii+1])) {
nerit 12:b0fc1d313813 2158 // esegue l'interpolazione dei valori stimati di duty in base alla velocità
nerit 12:b0fc1d313813 2159 deltaV=tabSpeed[ii+1]-tabSpeed[ii];
nerit 12:b0fc1d313813 2160 deltaD=tabComan[ii+1]-tabComan[ii];
nerit 14:e2b5efa06c41 2161 //setV = tractorSpeed_MtS_timed-tabSpeed[ii];
nerit 14:e2b5efa06c41 2162 setV = speedCorrectionMachine-tabSpeed[ii];
nerit 12:b0fc1d313813 2163 teoriaC=(setV/deltaV)*deltaD;
nerit 12:b0fc1d313813 2164 dutyTeorico = tabComan[ii]+teoriaC; // era ii+1 al 23/03/19
nerit 14:e2b5efa06c41 2165 #if defined(pcSerial)
nerit 14:e2b5efa06c41 2166 #if defined(boost1)
nerit 14:e2b5efa06c41 2167 pc.printf("tractor: %f tabspeed1: %f tabspeed2: %f\n",tractorSpeed_MtS_timed, tabSpeed[ii],tabSpeed[ii+1]);
nerit 14:e2b5efa06c41 2168 pc.printf("deltaV: %f deltaD: %f setV: %f teoriaC %f \n",deltaV,deltaD,setV,teoriaC);
nerit 14:e2b5efa06c41 2169 #endif
nerit 14:e2b5efa06c41 2170 #endif
nerit 3:a469bbd294b5 2171 }
nerit 8:310f9e4eac7b 2172 }
nerit 14:e2b5efa06c41 2173 //if (tractorSpeed_MtS_timed > tabSpeed[16]) {
nerit 14:e2b5efa06c41 2174 // dutyTeorico=tractorSpeed_MtS_timed/maxWorkSpeed;
nerit 14:e2b5efa06c41 2175 //}
nerit 17:9629eb019892 2176 if (speedCorrectionMachine > tabSpeed[tabeling]) {
nerit 14:e2b5efa06c41 2177 dutyTeorico=speedCorrectionMachine/maxWorkSpeed;
nerit 8:310f9e4eac7b 2178 }
nerit 8:310f9e4eac7b 2179 #if !defined(speedMaster)
nerit 8:310f9e4eac7b 2180 quinCalc();
nerit 8:310f9e4eac7b 2181 #endif
nerit 8:310f9e4eac7b 2182 #if defined(pcSerial)
nerit 8:310f9e4eac7b 2183 #if defined(Qncd)
nerit 8:310f9e4eac7b 2184 pc.printf("enableSpeed: %d pulseRised2: %d quincCnt: %d\n",enableSpeed,pulseRised2,quincCnt);
nerit 3:a469bbd294b5 2185 #endif
nerit 8:310f9e4eac7b 2186 #endif
nerit 8:310f9e4eac7b 2187 if ((enableSpeed>3)&&(pulseRised2==1)&&(quincCnt>=2)) {
nerit 8:310f9e4eac7b 2188 double erroreTempo = 0.0f;
nerit 8:310f9e4eac7b 2189 if(encoder==false) {
nerit 8:310f9e4eac7b 2190 if(speedFromPick==1) {
nerit 8:310f9e4eac7b 2191 erroreTempo = (double)timeIntraPick - tempoTraBecchi_mS;
nerit 8:310f9e4eac7b 2192 } else {
nerit 8:310f9e4eac7b 2193 erroreTempo = (double)memoTimeHole - tempoTraBecchi_mS; // errore tra il tempo previsto ed il tempo reale ( >0 se sto andando più piano del previsto)
nerit 3:a469bbd294b5 2194 }
nerit 8:310f9e4eac7b 2195 } else {
nerit 8:310f9e4eac7b 2196 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 3:a469bbd294b5 2197 #if defined(pcSerial)
nerit 8:310f9e4eac7b 2198 #if defined(Qnce)
nerit 8:310f9e4eac7b 2199 pc.printf("timeHole: %d TempoBecchi: %f erroreTempo: %f\n",memoTimeHole,tempoTraBecchi_mS,erroreTempo);
nerit 3:a469bbd294b5 2200 #endif
nerit 3:a469bbd294b5 2201 #endif
nerit 3:a469bbd294b5 2202 }
nerit 8:310f9e4eac7b 2203 double errorePercentuale = erroreTempo / tempoTraBecchi_mS;
nerit 8:310f9e4eac7b 2204 double k3=0.0f;
nerit 8:310f9e4eac7b 2205 double k4=0.0f;
nerit 8:310f9e4eac7b 2206 double k5=0.0f;
nerit 8:310f9e4eac7b 2207 double k6=0.0f;
nerit 8:310f9e4eac7b 2208 #if defined(speedMaster)
nerit 8:310f9e4eac7b 2209 k3=0.010f;
nerit 8:310f9e4eac7b 2210 #else
nerit 8:310f9e4eac7b 2211 k3=0.050f;
nerit 8:310f9e4eac7b 2212 #endif
nerit 8:310f9e4eac7b 2213 k4=1.103f;
nerit 8:310f9e4eac7b 2214 k5=10.00f;
nerit 8:310f9e4eac7b 2215 k6=20.50f;
nerit 8:310f9e4eac7b 2216 double L1 = 0.045f;
nerit 8:310f9e4eac7b 2217 double L_1=-0.045f;
nerit 8:310f9e4eac7b 2218 double L2 = 0.150f;
nerit 8:310f9e4eac7b 2219 double L_2=-0.150f;
nerit 8:310f9e4eac7b 2220 double L3 = 0.301f;
nerit 8:310f9e4eac7b 2221 double L_3=-0.301f;
nerit 8:310f9e4eac7b 2222 double k1=0.0f;
nerit 8:310f9e4eac7b 2223 if ((errorePercentuale > L3)||(errorePercentuale < L_3)) {
nerit 8:310f9e4eac7b 2224 k1=errorePercentuale*k6;
nerit 8:310f9e4eac7b 2225 }
nerit 8:310f9e4eac7b 2226 if (((errorePercentuale >= L2)&&(errorePercentuale<=L3))||((errorePercentuale <= L_2)&&(errorePercentuale>=L_3))) {
nerit 8:310f9e4eac7b 2227 k1=errorePercentuale*k5;
nerit 8:310f9e4eac7b 2228 }
nerit 8:310f9e4eac7b 2229 if (((errorePercentuale < L2)&&(errorePercentuale>L1))||((errorePercentuale > L_2)&&(errorePercentuale<L_1))) {
nerit 8:310f9e4eac7b 2230 k1=errorePercentuale*k4;
nerit 8:310f9e4eac7b 2231 }
nerit 8:310f9e4eac7b 2232 if ((errorePercentuale < L1)||(errorePercentuale > L_1)) {
nerit 8:310f9e4eac7b 2233 k1=errorePercentuale*k3;
nerit 8:310f9e4eac7b 2234 }
nerit 8:310f9e4eac7b 2235 double memoCorrezione = k1;
nerit 8:310f9e4eac7b 2236 if (quincCnt >= 2) {
nerit 8:310f9e4eac7b 2237 correzione = correzione + memoCorrezione;
nerit 8:310f9e4eac7b 2238 if (correzione > (1.0f - dutyTeorico)) {
nerit 8:310f9e4eac7b 2239 correzione = (1.0f - dutyTeorico);
nerit 14:e2b5efa06c41 2240 #if defined(pcSerial)
nerit 14:e2b5efa06c41 2241 pc.printf("limite\n");
nerit 14:e2b5efa06c41 2242 #endif
nerit 3:a469bbd294b5 2243 }
nerit 8:310f9e4eac7b 2244 if ((correzione < 0.0f)&&(dutyTeorico+correzione<0.0f)) {
nerit 8:310f9e4eac7b 2245 correzione = -1.0f*dutyTeorico;
nerit 14:e2b5efa06c41 2246 #if defined(pcSerial)
nerit 14:e2b5efa06c41 2247 pc.printf("limite\n");
nerit 14:e2b5efa06c41 2248 #endif
nerit 3:a469bbd294b5 2249 }
nerit 3:a469bbd294b5 2250 }
nerit 8:310f9e4eac7b 2251 pulseRised1=0;
nerit 8:310f9e4eac7b 2252 pulseRised2=0;
nerit 8:310f9e4eac7b 2253 #if defined(pcSerial)
nerit 8:310f9e4eac7b 2254 #if defined(Qnca)
nerit 8:310f9e4eac7b 2255 pc.printf("ErTem: %f K1: %f Corr: %f MemoCorr:%f DutyTeo: %f \n",erroreTempo, k1,correzione, memoCorrezione, dutyTeorico);
nerit 8:310f9e4eac7b 2256 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 8:310f9e4eac7b 2257 #endif
nerit 8:310f9e4eac7b 2258 #endif
nerit 8:310f9e4eac7b 2259 #if defined(pcSerial)
nerit 8:310f9e4eac7b 2260 #if defined(Qncb)
nerit 8:310f9e4eac7b 2261 pc.printf("TsP: %f SpW: %f InPic: %f EPerc: %f Duty:%f \n",tractorSpeed_MtS_timed,speedOfSeedWheel,timeIntraPick, errorePercentuale, dcActualDuty);
nerit 8:310f9e4eac7b 2262 #endif
nerit 8:310f9e4eac7b 2263 #endif
nerit 12:b0fc1d313813 2264 #if defined(pcSerial)
nerit 12:b0fc1d313813 2265 #if defined(boost)
nerit 12:b0fc1d313813 2266 pc.printf("boost: %f teory: %f real: %f curr: %f cc:%d check: %d\n",boostDcOut,dutyTeorico, dcActualDuty,SD_CurrentScaled,correggiCorrente,currentCheckEnable);
nerit 12:b0fc1d313813 2267 #endif
nerit 12:b0fc1d313813 2268 #endif
nerit 8:310f9e4eac7b 2269 }
nerit 12:b0fc1d313813 2270 // introduce il controllo di corrente
nerit 14:e2b5efa06c41 2271 double variazione=0.0f;
nerit 12:b0fc1d313813 2272 if (correggiCorrente==1){
nerit 12:b0fc1d313813 2273 if (SD_CurrentScaled < 1.6f){
nerit 12:b0fc1d313813 2274 boostDcOut +=0.01f;
nerit 12:b0fc1d313813 2275 }
nerit 12:b0fc1d313813 2276 if (SD_CurrentScaled > 2.6f){
nerit 12:b0fc1d313813 2277 boostDcOut -=0.01f;
nerit 12:b0fc1d313813 2278 }
nerit 12:b0fc1d313813 2279 if (boostDcOut >= 0.2f){
nerit 12:b0fc1d313813 2280 boostDcOut=0.2f;
nerit 14:e2b5efa06c41 2281 //all_genericals=1;
nerit 12:b0fc1d313813 2282 }
nerit 12:b0fc1d313813 2283 if (boostDcOut <=-0.2f){
nerit 12:b0fc1d313813 2284 boostDcOut=-0.2f;
nerit 14:e2b5efa06c41 2285 //all_genericals=1;
nerit 12:b0fc1d313813 2286 }
nerit 14:e2b5efa06c41 2287 variazione=boostDcOut;
nerit 12:b0fc1d313813 2288 correggiCorrente=0;
nerit 8:310f9e4eac7b 2289 }
nerit 12:b0fc1d313813 2290 if (currentCheckEnable==true){
nerit 14:e2b5efa06c41 2291 correzione += variazione;
nerit 14:e2b5efa06c41 2292 variazione=0.0f;
nerit 8:310f9e4eac7b 2293 }
nerit 8:310f9e4eac7b 2294 DC_brake=0;
nerit 8:310f9e4eac7b 2295 DC_forward=1;
nerit 8:310f9e4eac7b 2296 DC_prepare();
bcostm 2:35f13b7f3659 2297
nerit 8:310f9e4eac7b 2298 // il semiperiodoreale è calcolato sulla lettura del passaggio becchi reale
nerit 8:310f9e4eac7b 2299 seedWheelPeriod = semiPeriodoReale;
nerit 8:310f9e4eac7b 2300 if (seedWheelPeriod < 180.0f) {
nerit 8:310f9e4eac7b 2301 seedWheelPeriod = 180.0f;
nerit 8:310f9e4eac7b 2302 }
nerit 8:310f9e4eac7b 2303 if ((oldSeedWheelPeriod!=seedWheelPeriod)&&(seedWheelPeriod >=180.0f )) {
nerit 8:310f9e4eac7b 2304 SDticker.attach_us(&step_SDPulseOut,seedWheelPeriod); // clock time are microseconds and attach seed motor stepper controls
nerit 8:310f9e4eac7b 2305 oldSeedWheelPeriod=seedWheelPeriod;
nerit 8:310f9e4eac7b 2306 }
nerit 3:a469bbd294b5 2307
nerit 8:310f9e4eac7b 2308 if((quincCnt>=3)) {
nerit 8:310f9e4eac7b 2309 if (correzioneAttiva==1) {
nerit 8:310f9e4eac7b 2310 dcActualDuty = dutyTeorico + correzione;
nerit 8:310f9e4eac7b 2311 } else {
nerit 3:a469bbd294b5 2312 dcActualDuty = dutyTeorico;
nerit 3:a469bbd294b5 2313 }
nerit 8:310f9e4eac7b 2314 } else {
nerit 8:310f9e4eac7b 2315 dcActualDuty = dutyTeorico;
nerit 8:310f9e4eac7b 2316 }
nerit 8:310f9e4eac7b 2317 if (dcActualDuty <=0.0f) {
nerit 8:310f9e4eac7b 2318 dcActualDuty=0.05f;
nerit 8:310f9e4eac7b 2319 }
nerit 8:310f9e4eac7b 2320 if (dcActualDuty > 0.95f) {
nerit 8:310f9e4eac7b 2321 dcActualDuty = 0.95f;
nerit 8:310f9e4eac7b 2322 }
nerit 17:9629eb019892 2323 if (dcActualDuty > (dutyTeorico *1.15f)){dcActualDuty=dutyTeorico*1.15f;}
nerit 14:e2b5efa06c41 2324 if (dcActualDuty < (dutyTeorico *0.85f)){dcActualDuty=dutyTeorico*0.85f;}
nerit 8:310f9e4eac7b 2325 if (olddcActualDuty!=dcActualDuty) {
nerit 8:310f9e4eac7b 2326 SDmotorPWM.write(1.0f-dcActualDuty);
nerit 8:310f9e4eac7b 2327 olddcActualDuty=dcActualDuty;
nerit 8:310f9e4eac7b 2328 }
nerit 8:310f9e4eac7b 2329 // allarme
nerit 17:9629eb019892 2330 if (tractorSpeed_MtS_timed>0.0f){
nerit 17:9629eb019892 2331 if (SDwheelTimer.read_ms()>4000) {
nerit 17:9629eb019892 2332 if (firstStart==0) {
nerit 17:9629eb019892 2333 all_noDcRotati=1;
nerit 17:9629eb019892 2334 }
nerit 17:9629eb019892 2335 #if defined(pcSerial)
nerit 17:9629eb019892 2336 #if defined(VediAllarmi)
nerit 17:9629eb019892 2337 pc.printf("allarme no DC rotation");
nerit 17:9629eb019892 2338 #endif
nerit 17:9629eb019892 2339 #endif
nerit 3:a469bbd294b5 2340 }
nerit 8:310f9e4eac7b 2341 }
nerit 3:a469bbd294b5 2342
nerit 8:310f9e4eac7b 2343 //***************************************************************************************************************
nerit 8:310f9e4eac7b 2344 // CONTROLLA TAMBURO
nerit 8:310f9e4eac7b 2345 //***************************************************************************************************************
nerit 8:310f9e4eac7b 2346 if(lowSpeed==0) {
nerit 8:310f9e4eac7b 2347 if (syncroCheck==1) {
nerit 8:310f9e4eac7b 2348 syncroCheck=0;
nerit 8:310f9e4eac7b 2349 lockStart=1;
nerit 8:310f9e4eac7b 2350 periodo = TBperiod;
nerit 8:310f9e4eac7b 2351 #if !defined(runner)
nerit 9:7f02256f6e8f 2352 #if defined(Zucca)
nerit 9:7f02256f6e8f 2353 motor->step_clock_mode_enable(StepperMotor::BWD);
nerit 9:7f02256f6e8f 2354 #else
nerit 9:7f02256f6e8f 2355 motor->step_clock_mode_enable(StepperMotor::FWD);
nerit 9:7f02256f6e8f 2356 #endif
nerit 8:310f9e4eac7b 2357 #endif
nerit 8:310f9e4eac7b 2358 if (aspettaStart==0) {
nerit 6:e8c18f0f399a 2359 #if defined(pcSerial)
nerit 6:e8c18f0f399a 2360 #if defined(checkLoop)
nerit 8:310f9e4eac7b 2361 pc.printf("da sincro\n");
nerit 6:e8c18f0f399a 2362 #endif
nerit 6:e8c18f0f399a 2363 #endif
nerit 8:310f9e4eac7b 2364 cambiaTB(periodo);
nerit 3:a469bbd294b5 2365 }
nerit 8:310f9e4eac7b 2366 }
nerit 8:310f9e4eac7b 2367 // controllo di stop
nerit 8:310f9e4eac7b 2368 double memoIntraP = (double)memoIntraPick*1.8f;
nerit 8:310f9e4eac7b 2369 if ((double)rotationTimeOut.read_ms()> (memoIntraP)) {
nerit 3:a469bbd294b5 2370 syncroCheck=0;
nerit 8:310f9e4eac7b 2371 aspettaStart=1;
nerit 8:310f9e4eac7b 2372 countCicli=0;
nerit 6:e8c18f0f399a 2373 #if defined(pcSerial)
nerit 6:e8c18f0f399a 2374 #if defined(checkLoop)
nerit 8:310f9e4eac7b 2375 pc.printf("AspettaSI\n");
nerit 6:e8c18f0f399a 2376 #endif
nerit 6:e8c18f0f399a 2377 #endif
nerit 8:310f9e4eac7b 2378 if (TBzeroCyclePulse==1) {
nerit 8:310f9e4eac7b 2379 #if !defined(runner)
nerit 8:310f9e4eac7b 2380 TBticker.detach();
nerit 8:310f9e4eac7b 2381 #endif
nerit 8:310f9e4eac7b 2382 motor->soft_hiz();
nerit 8:310f9e4eac7b 2383 }
nerit 8:310f9e4eac7b 2384 }
nerit 8:310f9e4eac7b 2385 } else { // fine ciclo fuori da low speed
nerit 8:310f9e4eac7b 2386 syncroCheck=0;
nerit 8:310f9e4eac7b 2387 lockStart=0;
nerit 8:310f9e4eac7b 2388 if (beccoPronto==1) {
nerit 8:310f9e4eac7b 2389 if (tamburoStandard==1) {
nerit 8:310f9e4eac7b 2390 double ritardoMassimo = 0.0f;
nerit 8:310f9e4eac7b 2391 if (encoder==false) {
nerit 8:310f9e4eac7b 2392 if(speedFromPick==1) {
nerit 8:310f9e4eac7b 2393 ritardoMassimo = (double)timeIntraPick;
nerit 8:310f9e4eac7b 2394 } else {
nerit 8:310f9e4eac7b 2395 ritardoMassimo = (double)memoTimeHole;
nerit 8:310f9e4eac7b 2396 }
nerit 8:310f9e4eac7b 2397 } else {
nerit 8:310f9e4eac7b 2398 ritardoMassimo = (double)timeIntraPick;
nerit 8:310f9e4eac7b 2399 }
nerit 8:310f9e4eac7b 2400 int tempoDiSincro = (int)((double)(ritardoMassimo - ((tempoBecco/1.8f)+((speedOfSeedWheel/maxWorkSpeed)*ritardoMassimo)))); //
nerit 8:310f9e4eac7b 2401 if (tempoDiSincro <= 1) {
nerit 8:310f9e4eac7b 2402 tempoDiSincro=1;
nerit 8:310f9e4eac7b 2403 }
nerit 8:310f9e4eac7b 2404 if ((sincroTimer.read_ms()>= tempoDiSincro)) {
nerit 8:310f9e4eac7b 2405 if (tractorSpeed_MtS_timed >= minWorkSpeed) {
nerit 8:310f9e4eac7b 2406 startCicloTB=1;
nerit 8:310f9e4eac7b 2407 }
nerit 8:310f9e4eac7b 2408 beccoPronto=0;
nerit 8:310f9e4eac7b 2409 }
nerit 8:310f9e4eac7b 2410 } else {
nerit 8:310f9e4eac7b 2411 // tamburo per zucca
nerit 8:310f9e4eac7b 2412 if (speedOfSeedWheel >= minWorkSpeed) {
nerit 8:310f9e4eac7b 2413 startCicloTB=1;
nerit 8:310f9e4eac7b 2414 }
nerit 8:310f9e4eac7b 2415 beccoPronto=0;
nerit 8:310f9e4eac7b 2416 }
nerit 8:310f9e4eac7b 2417 }
nerit 8:310f9e4eac7b 2418 ciclaTB();
nerit 8:310f9e4eac7b 2419 }
nerit 8:310f9e4eac7b 2420 //*************************************************************
nerit 8:310f9e4eac7b 2421 } else { // fine ciclo con velocita maggiore di 0
nerit 13:d1030d4e51a8 2422 delaySpeedCheck.reset();
nerit 8:310f9e4eac7b 2423 if (cycleStopRequest==1) {
nerit 8:310f9e4eac7b 2424 SDwheelTimer.stop();
nerit 8:310f9e4eac7b 2425 SDwheelTimer.reset();
nerit 8:310f9e4eac7b 2426 #if defined(seedSensor)
nerit 8:310f9e4eac7b 2427 resetDelay();
nerit 8:310f9e4eac7b 2428 #endif
nerit 8:310f9e4eac7b 2429 checkSDrotation=0;
nerit 8:310f9e4eac7b 2430 oldFaseLavoro=0;
nerit 8:310f9e4eac7b 2431 aspettaStart=1;
nerit 8:310f9e4eac7b 2432 countCicli=0;
nerit 8:310f9e4eac7b 2433 oldSeedWheelPeriod=0.0f;
nerit 8:310f9e4eac7b 2434 oldPeriodoTB=0.0f;
nerit 8:310f9e4eac7b 2435 correzione=0.0f;
nerit 8:310f9e4eac7b 2436 OLDpulseSpeedInterval=1000.01f;
nerit 8:310f9e4eac7b 2437 cicloTbinCorso=0;
nerit 8:310f9e4eac7b 2438 cntTbError=0;
nerit 8:310f9e4eac7b 2439 olddcActualDuty=0.0f;
nerit 8:310f9e4eac7b 2440 #if defined(pcSerial)
nerit 8:310f9e4eac7b 2441 #if defined(checkLoopb)
nerit 8:310f9e4eac7b 2442 pc.printf("forza\n");
nerit 8:310f9e4eac7b 2443 #endif
nerit 8:310f9e4eac7b 2444 #endif
nerit 8:310f9e4eac7b 2445 speedOfSeedWheel=0.0f;
nerit 8:310f9e4eac7b 2446 cycleStopRequest=0;
nerit 8:310f9e4eac7b 2447 DC_brake=1;
nerit 8:310f9e4eac7b 2448 DC_prepare();
nerit 8:310f9e4eac7b 2449 metalTimer.reset();
nerit 8:310f9e4eac7b 2450 #if defined(pcSerial)
nerit 8:310f9e4eac7b 2451 #if defined(checkLoop)
nerit 8:310f9e4eac7b 2452 pc.printf("17h\n");
nerit 8:310f9e4eac7b 2453 #endif
nerit 8:310f9e4eac7b 2454 #endif
nerit 8:310f9e4eac7b 2455 #if !defined(runner)
nerit 6:e8c18f0f399a 2456 TBticker.detach();
nerit 8:310f9e4eac7b 2457 #endif
nerit 8:310f9e4eac7b 2458 #if defined(pcSerial)
nerit 8:310f9e4eac7b 2459 #if defined(loStop)
nerit 8:310f9e4eac7b 2460 pc.printf("A5\n");
nerit 6:e8c18f0f399a 2461 #endif
nerit 8:310f9e4eac7b 2462 #endif
nerit 8:310f9e4eac7b 2463 motor->soft_hiz();
nerit 8:310f9e4eac7b 2464 pntMedia=0;
nerit 8:310f9e4eac7b 2465 #if defined(pcSerial)
nerit 8:310f9e4eac7b 2466 #if defined(stopSignal)
nerit 8:310f9e4eac7b 2467 pc.printf("stop\n");
nerit 6:e8c18f0f399a 2468 #endif
nerit 8:310f9e4eac7b 2469 #endif
nerit 3:a469bbd294b5 2470 }
nerit 8:310f9e4eac7b 2471 }
nerit 8:310f9e4eac7b 2472
nerit 8:310f9e4eac7b 2473 //*************************************************************************************************
nerit 3:a469bbd294b5 2474 TBzeroCyclePulse=0;
nerit 8:310f9e4eac7b 2475 //*************************************************************************************************
nerit 6:e8c18f0f399a 2476 } //end inProva==0
nerit 3:a469bbd294b5 2477 wd.Service(); // kick the dog before the timeout
nerit 3:a469bbd294b5 2478 } // end while
nerit 3:a469bbd294b5 2479 } // end main