Forigo / Mbed 2 deprecated FORIGO_Modula_V7_3_VdcStep_Modula2021

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed X_NUCLEO_IHM03A1_for

main.cpp@47:8455eedea226, 2020-12-02 (annotated)

Committer:
francescopistone
Date:
Wed Dec 02 09:07:00 2020 +0000
Revision:
47:8455eedea226
Parent:
45:162116db828a
Child:
48:d46f3c5638ee
angoli di fase modificabili anche con macchina in movimento

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