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@9:7f02256f6e8f, 2019-03-11 (annotated)

Committer:
nerit
Date:
Mon Mar 11 17:13:17 2019 +0000
Revision:
9:7f02256f6e8f
Parent:
8:310f9e4eac7b
Child:
10:9e70619e97ab
11 marzo 2019 18:10

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