Forigo / Mbed 2 deprecated FORIGO_Modula_V7_3_VdcStep_DICEMBRE2020

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@49:2cfa7a02ac1e, 2021-02-05 (annotated)

Committer:
nerit
Date:
Fri Feb 05 11:24:07 2021 +0000
Revision:
49:2cfa7a02ac1e
Parent:
47:8455eedea226
Child:
50:144a92eeddb9
modifica parametri per funzionamento con 15 becchi

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 49:2cfa7a02ac1e 635 }else if(pickNumber >= 13){
nerit 49:2cfa7a02ac1e 636 lowLim=-150.0f;
nerit 49:2cfa7a02ac1e 637 higLim= 150.0f;
nerit 49:2cfa7a02ac1e 638 divide= 70.0f;
nerit 24:2b5e749e26b2 639 }else{
nerit 25:4461bc76aaab 640 lowLim=-500.0f; //pneumatico Alessandria
nerit 25:4461bc76aaab 641 higLim= 130.0f; //pneumatico Alessandria
nerit 25:4461bc76aaab 642 divide= 20.0f; //pneumatico Alessandria
nerit 24:2b5e749e26b2 643 }
nerit 4:de1b296e9757 644 }
nerit 25:4461bc76aaab 645
nerit 8:310f9e4eac7b 646 if (posError>higLim) {
nerit 17:9629eb019892 647 posError=higLim;
nerit 17:9629eb019892 648 //posError=0.0f;
nerit 17:9629eb019892 649 //motor->soft_hiz();
nerit 28:0534c86365ec 650 //aspettareSincro=1;
nerit 28:0534c86365ec 651 //stopCicloTB=1;
nerit 8:310f9e4eac7b 652 }
nerit 8:310f9e4eac7b 653 if (posError<lowLim) {
nerit 8:310f9e4eac7b 654 posError=lowLim;
nerit 28:0534c86365ec 655 //aspettareSincro=1;
nerit 28:0534c86365ec 656 //stopCicloTB=1;
nerit 8:310f9e4eac7b 657 }
nerit 28:0534c86365ec 658 if (((posError >=1.0f)||(posError<=-1.0f))) {
nerit 8:310f9e4eac7b 659 #if defined(runner)
nerit 28:0534c86365ec 660 double variante = posError/divide;
nerit 41:a0d9a71f8cb5 661 //if (variante < -0.399f){variante=-0.3999;}
nerit 41:a0d9a71f8cb5 662 if (variante < -0.5f){variante=-0.5;}
nerit 30:32e770e91998 663 /*if ((posError<=15.0f)&&(posError>=-15.0f)){
nerit 30:32e770e91998 664 ePpos = periodo;// *(1.0f+ variante);
nerit 30:32e770e91998 665 }else{*/
nerit 30:32e770e91998 666 ePpos = periodo *(1.0f+ variante);
nerit 30:32e770e91998 667 //}
nerit 30:32e770e91998 668 #if defined(pcSerial)
nerit 30:32e770e91998 669 #if defined(TBperS)
nerit 30:32e770e91998 670 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 671 #endif
nerit 30:32e770e91998 672 #endif
nerit 8:310f9e4eac7b 673 #else
nerit 39:6814c75dfa5c 674 double variante = posError/100.0f;
nerit 41:a0d9a71f8cb5 675 if (variante < -0.399f){variante=-0.3999;}
nerit 39:6814c75dfa5c 676 ePpos = periodo /(1.0f+ variante);
nerit 39:6814c75dfa5c 677 #if defined(pcSerial)
nerit 39:6814c75dfa5c 678 #if defined(calcoli)
nerit 39:6814c75dfa5c 679 pc.printf("variante: %f poserror: %f periodo: %f eppos: %f \n",variante,posError,periodo,ePpos);
nerit 39:6814c75dfa5c 680 #endif
nerit 39:6814c75dfa5c 681 #endif
nerit 8:310f9e4eac7b 682 #endif
nerit 39:6814c75dfa5c 683 if ((ePpos>0.0f)&&(ePpos!=NULL)) {
nerit 39:6814c75dfa5c 684 cambiaTB(ePpos,1);
nerit 8:310f9e4eac7b 685 } else {
nerit 39:6814c75dfa5c 686 cambiaTB(periodo,2);///2.0f);
nerit 6:e8c18f0f399a 687 }
nerit 6:e8c18f0f399a 688 }
nerit 3:a469bbd294b5 689 }
nerit 3:a469bbd294b5 690 }
nerit 3:a469bbd294b5 691 }
nerit 10:9e70619e97ab 692 #if defined(pcSerial)
nerit 10:9e70619e97ab 693 #if defined(checkLoopa)
nerit 10:9e70619e97ab 694 pc.printf("12\n");
nerit 10:9e70619e97ab 695 #endif
nerit 10:9e70619e97ab 696 #endif
nerit 3:a469bbd294b5 697 }
nerit 3:a469bbd294b5 698 //*******************************************************
nerit 8:310f9e4eac7b 699 void videoUpdate()
nerit 8:310f9e4eac7b 700 {
nerit 8:310f9e4eac7b 701 for(int aa=0; aa<4; aa++) {
nerit 8:310f9e4eac7b 702 speedForDisplay[aa]=speedForDisplay[aa+1];
nerit 8:310f9e4eac7b 703 }
nerit 3:a469bbd294b5 704 speedForDisplay[4]=tractorSpeed_MtS_timed;
nerit 3:a469bbd294b5 705 totalSpeed=0.0f;
nerit 8:310f9e4eac7b 706 for (int aa=0; aa<5; aa++) {
nerit 8:310f9e4eac7b 707 totalSpeed += speedForDisplay[aa];
nerit 8:310f9e4eac7b 708 }
nerit 3:a469bbd294b5 709 totalSpeed = totalSpeed / 5.0f;
nerit 10:9e70619e97ab 710 #if defined(pcSerial)
nerit 10:9e70619e97ab 711 #if defined(SDreset)
nerit 10:9e70619e97ab 712 pc.printf("Fase: %d",fase);
nerit 10:9e70619e97ab 713 pc.printf(" PrePosSD: %d",prePosSD);
nerit 10:9e70619e97ab 714 pc.printf(" PosSD: %d",SDactualPosition);
nerit 10:9e70619e97ab 715 pc.printf(" speed: %f",tractorSpeed_MtS_timed);
nerit 10:9e70619e97ab 716 pc.printf(" Trigger: %d \n", trigRepos);
nerit 10:9e70619e97ab 717 #endif
nerit 10:9e70619e97ab 718 #endif
nerit 10:9e70619e97ab 719 #if defined(pcSerial)
nerit 10:9e70619e97ab 720 #if defined(checkLoop)
nerit 10:9e70619e97ab 721 pc.printf("13\n");
nerit 10:9e70619e97ab 722 #endif
nerit 10:9e70619e97ab 723 #endif
nerit 3:a469bbd294b5 724 }
nerit 3:a469bbd294b5 725 //*******************************************************
nerit 8:310f9e4eac7b 726 void ciclaTB()
nerit 8:310f9e4eac7b 727 {
nerit 8:310f9e4eac7b 728 if ((startCicloTB==1)&&(cicloTbinCorso==0)) {
nerit 6:e8c18f0f399a 729 #if defined(pcSerial)
nerit 6:e8c18f0f399a 730 #if defined(checkLoop)
nerit 6:e8c18f0f399a 731 pc.printf("14a TBperiod: %f\n",TBperiod);
nerit 6:e8c18f0f399a 732 #endif
nerit 6:e8c18f0f399a 733 #endif
nerit 8:310f9e4eac7b 734 #if defined(runner)
nerit 9:7f02256f6e8f 735 #if defined(Zucca)
nerit 9:7f02256f6e8f 736 motor->run(StepperMotor::BWD,TBperiod);
nerit 9:7f02256f6e8f 737 #else
nerit 9:7f02256f6e8f 738 motor->run(StepperMotor::FWD,TBperiod);
nerit 9:7f02256f6e8f 739 #endif
nerit 8:310f9e4eac7b 740 #else
nerit 8:310f9e4eac7b 741 if (TBperiod >= (250.0f*2.0f)) {
nerit 8:310f9e4eac7b 742 if (TBperiod != NULL) {
nerit 35:3165f4c1c7bf 743 #if !defined(oldstepperDriver)
nerit 35:3165f4c1c7bf 744 #if defined(runner)
nerit 35:3165f4c1c7bf 745 #if defined(Zucca)
nerit 35:3165f4c1c7bf 746 motor->step_clock_mode_enable(StepperMotor::BWD);
nerit 35:3165f4c1c7bf 747 #else
nerit 35:3165f4c1c7bf 748 motor->step_clock_mode_enable(StepperMotor::FWD);
nerit 35:3165f4c1c7bf 749 #endif
nerit 35:3165f4c1c7bf 750 #endif
nerit 9:7f02256f6e8f 751 #endif
nerit 39:6814c75dfa5c 752 //TBmotorRst=0;
nerit 39:6814c75dfa5c 753 //TBticker.attach_us(&step_TBPulseOut,TBperiod/2.0f); // clock time are milliseconds and attach seed motor stepper controls
nerit 39:6814c75dfa5c 754 attivaTb(TBperiod/2.0f);
nerit 8:310f9e4eac7b 755 }
nerit 6:e8c18f0f399a 756 }
nerit 8:310f9e4eac7b 757 #endif
nerit 8:310f9e4eac7b 758 cicloTbinCorso = 1;
nerit 8:310f9e4eac7b 759 startCicloTB=0;
nerit 3:a469bbd294b5 760 }
nerit 8:310f9e4eac7b 761 if ((loadDaCan==1)&&(loadDaCanInCorso==0)) {
nerit 6:e8c18f0f399a 762 #if defined(pcSerial)
nerit 6:e8c18f0f399a 763 #if defined(checkLoop)
nerit 6:e8c18f0f399a 764 pc.printf("14b\n");
nerit 6:e8c18f0f399a 765 #endif
nerit 6:e8c18f0f399a 766 #endif
nerit 8:310f9e4eac7b 767 #if defined(runner)
nerit 9:7f02256f6e8f 768 #if defined(Zucca)
nerit 9:7f02256f6e8f 769 motor->run(StepperMotor::BWD,50.0f);
nerit 9:7f02256f6e8f 770 #else
nerit 9:7f02256f6e8f 771 motor->run(StepperMotor::FWD,50.0f);
nerit 9:7f02256f6e8f 772 #endif
nerit 8:310f9e4eac7b 773 #else
nerit 35:3165f4c1c7bf 774 #if !defined(oldStepperDriver)
nerit 35:3165f4c1c7bf 775 #if defined(Zucca)
nerit 35:3165f4c1c7bf 776 motor->step_clock_mode_enable(StepperMotor::BWD);
nerit 35:3165f4c1c7bf 777 #else
nerit 35:3165f4c1c7bf 778 motor->step_clock_mode_enable(StepperMotor::FWD);
nerit 35:3165f4c1c7bf 779 #endif
nerit 9:7f02256f6e8f 780 #endif
nerit 39:6814c75dfa5c 781 //TBmotorRst=0;
nerit 39:6814c75dfa5c 782 //TBticker.attach_us(&step_TBPulseOut,1000.0f); // clock time are milliseconds and attach seed motor stepper controls
nerit 40:42eae86457dd 783 attivaTb(700.0f);
nerit 8:310f9e4eac7b 784 #endif
nerit 3:a469bbd294b5 785 loadDaCanInCorso=1;
nerit 3:a469bbd294b5 786 stopCicloTB=0;
nerit 3:a469bbd294b5 787 }
nerit 28:0534c86365ec 788 if ((stopCicloTB==1)&&(TBactualPosition>5)&&(TBactualPosition<50)) {
nerit 6:e8c18f0f399a 789 #if defined(pcSerial)
nerit 6:e8c18f0f399a 790 #if defined(checkLoop)
nerit 6:e8c18f0f399a 791 pc.printf("14c\n");
nerit 6:e8c18f0f399a 792 #endif
nerit 6:e8c18f0f399a 793 #endif
nerit 8:310f9e4eac7b 794 #if !defined(runner)
nerit 39:6814c75dfa5c 795 //TBticker.detach();
nerit 39:6814c75dfa5c 796 //TBmotorRst=1;
nerit 39:6814c75dfa5c 797 stopTb();
nerit 8:310f9e4eac7b 798 #endif
nerit 6:e8c18f0f399a 799 #if defined(pcSerial)
nerit 6:e8c18f0f399a 800 #if defined(loStop)
nerit 6:e8c18f0f399a 801 pc.printf("A2\n");
nerit 6:e8c18f0f399a 802 #endif
nerit 6:e8c18f0f399a 803 #endif
nerit 35:3165f4c1c7bf 804 #if !defined(oldStepperDriver)
nerit 35:3165f4c1c7bf 805 motor->soft_hiz();
nerit 35:3165f4c1c7bf 806 #endif
nerit 3:a469bbd294b5 807 cicloTbinCorso = 0;
nerit 3:a469bbd294b5 808 stopCicloTB=0;
nerit 3:a469bbd294b5 809 loadDaCanInCorso=0;
nerit 3:a469bbd294b5 810 loadDaCan=0;
nerit 3:a469bbd294b5 811 }
nerit 3:a469bbd294b5 812 }
nerit 3:a469bbd294b5 813 // ------------------------------------------------------------------------------------------------------------------------------------------------------------------
francescopistone 42:59ceb4b6bc69 814 #if defined(oldStepperDriver)
nerit 8:310f9e4eac7b 815 void stepSetting()
nerit 8:310f9e4eac7b 816 {
nerit 3:a469bbd294b5 817 // Stepper driver init and set
nerit 37:0a225902cf48 818 TBmotorRst=1; // reset stepper driver
nerit 37:0a225902cf48 819 // M1 M2 M3 RESOLUTION
nerit 37:0a225902cf48 820 // 0 0 0 1/2
nerit 37:0a225902cf48 821 // 1 0 0 1/8
nerit 37:0a225902cf48 822 // 0 1 0 1/16
nerit 37:0a225902cf48 823 // 1 1 0 1/32
nerit 37:0a225902cf48 824 // 0 0 1 1/64
nerit 37:0a225902cf48 825 // 1 0 1 1/128
nerit 37:0a225902cf48 826 // 0 1 1 1/10
nerit 37:0a225902cf48 827 // 1 1 1 1/20
nerit 37:0a225902cf48 828 if (TBmotorSteps==400.0f){
nerit 37:0a225902cf48 829 TBmotor_M1=1;//0;
nerit 37:0a225902cf48 830 TBmotor_M2=1;//0;
nerit 37:0a225902cf48 831 TBmotor_M3=1;//0;
nerit 37:0a225902cf48 832 #if defined(pcSerial)
nerit 37:0a225902cf48 833 pc.printf("Steps 400\n");
nerit 37:0a225902cf48 834 #endif
nerit 37:0a225902cf48 835 }else if (TBmotorSteps==1600.0f){
nerit 37:0a225902cf48 836 TBmotor_M1=0;//0;
nerit 37:0a225902cf48 837 TBmotor_M2=1;//1;
nerit 37:0a225902cf48 838 TBmotor_M3=1;//1;
nerit 37:0a225902cf48 839 #if defined(pcSerial)
nerit 37:0a225902cf48 840 pc.printf("Steps 1600\n");
nerit 37:0a225902cf48 841 #endif
nerit 37:0a225902cf48 842 }else if (TBmotorSteps==3200.0f){
nerit 37:0a225902cf48 843 TBmotor_M1=1;//0;
nerit 37:0a225902cf48 844 TBmotor_M2=0;//1;
nerit 37:0a225902cf48 845 TBmotor_M3=1;//0;
nerit 37:0a225902cf48 846 #if defined(pcSerial)
nerit 37:0a225902cf48 847 pc.printf("Steps 3200\n");
nerit 37:0a225902cf48 848 #endif
nerit 37:0a225902cf48 849 }else if (TBmotorSteps==6400.0f){
nerit 37:0a225902cf48 850 TBmotor_M1=0;//1;
nerit 37:0a225902cf48 851 TBmotor_M2=0;//1;
nerit 37:0a225902cf48 852 TBmotor_M3=1;//0;
nerit 37:0a225902cf48 853 #if defined(pcSerial)
nerit 37:0a225902cf48 854 pc.printf("Steps 6400\n");
nerit 37:0a225902cf48 855 #endif
nerit 37:0a225902cf48 856 }else if (TBmotorSteps==12800.0f){
nerit 37:0a225902cf48 857 TBmotor_M1=1;//0;
nerit 37:0a225902cf48 858 TBmotor_M2=1;//0;
nerit 37:0a225902cf48 859 TBmotor_M3=0;//1;
nerit 37:0a225902cf48 860 #if defined(pcSerial)
nerit 37:0a225902cf48 861 pc.printf("Steps 12800\n");
nerit 37:0a225902cf48 862 #endif
nerit 37:0a225902cf48 863 }else if (TBmotorSteps==25600.0f){
nerit 37:0a225902cf48 864 TBmotor_M1=0;//1;
nerit 37:0a225902cf48 865 TBmotor_M2=1;//0;
nerit 37:0a225902cf48 866 TBmotor_M3=0;//1;
nerit 37:0a225902cf48 867 #if defined(pcSerial)
nerit 37:0a225902cf48 868 pc.printf("Steps 25600\n");
nerit 37:0a225902cf48 869 #endif
nerit 37:0a225902cf48 870 }else if (TBmotorSteps==2000.0f){
nerit 37:0a225902cf48 871 TBmotor_M1=1;//0;
nerit 37:0a225902cf48 872 TBmotor_M2=0;//1;
nerit 37:0a225902cf48 873 TBmotor_M3=0;//1;
nerit 37:0a225902cf48 874 #if defined(pcSerial)
nerit 37:0a225902cf48 875 pc.printf("Steps 2000\n");
nerit 37:0a225902cf48 876 #endif
nerit 37:0a225902cf48 877 }else if (TBmotorSteps==4000.0f){
nerit 37:0a225902cf48 878 TBmotor_M1=0;//1;
nerit 37:0a225902cf48 879 TBmotor_M2=0;//1;
nerit 37:0a225902cf48 880 TBmotor_M3=0;//1;
nerit 37:0a225902cf48 881 #if defined(pcSerial)
nerit 37:0a225902cf48 882 pc.printf("Steps 4000\n");
nerit 37:0a225902cf48 883 #endif
nerit 37:0a225902cf48 884 }else{
nerit 37:0a225902cf48 885 // set dei 1600 passi
nerit 37:0a225902cf48 886 TBmotor_M1=0;
nerit 37:0a225902cf48 887 TBmotor_M2=1;
nerit 37:0a225902cf48 888 TBmotor_M3=1;
nerit 37:0a225902cf48 889 #if defined(pcSerial)
nerit 37:0a225902cf48 890 pc.printf("Step standard\n");
nerit 37:0a225902cf48 891 #endif
nerit 37:0a225902cf48 892 }
nerit 3:a469bbd294b5 893 TBmotorRst=0; // reset stepper driver
nerit 3:a469bbd294b5 894 TBmotorDirecti=TBforward; // reset stepper direction
nerit 6:e8c18f0f399a 895 #if defined(pcSerial)
nerit 6:e8c18f0f399a 896 #if defined(checkLoop)
nerit 6:e8c18f0f399a 897 pc.printf("15\n");
nerit 6:e8c18f0f399a 898 #endif
nerit 6:e8c18f0f399a 899 #endif
nerit 3:a469bbd294b5 900 }
francescopistone 42:59ceb4b6bc69 901 #endif
nerit 3:a469bbd294b5 902 //****************************************
nerit 12:b0fc1d313813 903 void controllaCorrente(){
nerit 12:b0fc1d313813 904 correggiCorrente=1;
nerit 12:b0fc1d313813 905 }
nerit 12:b0fc1d313813 906 //****************************************
nerit 3:a469bbd294b5 907 void dcSetting(){
nerit 8:310f9e4eac7b 908 if ((speedFromPick==0)&&(encoder==false)) {
nerit 8:310f9e4eac7b 909 DcEncoder.rise(&sd25Fall);
nerit 8:310f9e4eac7b 910 }
nerit 8:310f9e4eac7b 911 if (encoder==true) {
nerit 8:310f9e4eac7b 912 DcEncoder.rise(&encoRise);
nerit 8:310f9e4eac7b 913 }
nerit 6:e8c18f0f399a 914 #if defined(pcSerial)
nerit 6:e8c18f0f399a 915 #if defined(checkLoop)
nerit 6:e8c18f0f399a 916 pc.printf("16\n");
nerit 6:e8c18f0f399a 917 #endif
nerit 6:e8c18f0f399a 918 #endif
nerit 3:a469bbd294b5 919 }
nerit 3:a469bbd294b5 920 //*******************************************************
nerit 8:310f9e4eac7b 921 void allarmi()
nerit 8:310f9e4eac7b 922 {
nerit 8:310f9e4eac7b 923 uint8_t alarmLowRegister1=0x00;
nerit 8:310f9e4eac7b 924 alarmLowRegister=0x00;
nerit 8:310f9e4eac7b 925 alarmHighRegister=0x80;
nerit 3:a469bbd294b5 926
nerit 8:310f9e4eac7b 927 //alarmLowRegister=alarmLowRegister+(all_semiFiniti*0x01); // manca il sensore
nerit 8:310f9e4eac7b 928 alarmLowRegister=alarmLowRegister+(all_pickSignal*0x02); // fatto
nerit 8:310f9e4eac7b 929 alarmLowRegister=alarmLowRegister+(all_cellSignal*0x04); // fatto
nerit 8:310f9e4eac7b 930 alarmLowRegister=alarmLowRegister+(all_lowBattery*0x08); // fatto
nerit 8:310f9e4eac7b 931 alarmLowRegister=alarmLowRegister+(all_overCurrDC*0x10); // fatto
nerit 8:310f9e4eac7b 932 alarmLowRegister=alarmLowRegister+(all_stopSistem*0x20); // verificarne la necessità
nerit 8:310f9e4eac7b 933 //alarmLowRegister=alarmLowRegister+(all_upElements*0x40); // manca il sensore
nerit 8:310f9e4eac7b 934 if (seedSensorEnable==true) {
nerit 8:310f9e4eac7b 935 alarmLowRegister=alarmLowRegister+(all_noSeedOnCe*0x80); // manca il sensore
nerit 8:310f9e4eac7b 936 }
nerit 3:a469bbd294b5 937
nerit 8:310f9e4eac7b 938 //alarmLowRegister1=alarmLowRegister1+(all_cfgnErrors*0x01); // da scrivere
nerit 8:310f9e4eac7b 939 alarmLowRegister1=alarmLowRegister1+(all_noDcRotati*0x02); // fatto
nerit 8:310f9e4eac7b 940 alarmLowRegister1=alarmLowRegister1+(all_noStepRota*0x04); // fatto
nerit 8:310f9e4eac7b 941 alarmLowRegister1=alarmLowRegister1+(all_speedError*0x08); // fatto
nerit 8:310f9e4eac7b 942 alarmLowRegister1=alarmLowRegister1+(all_noSpeedSen*0x10); // fatto
nerit 8:310f9e4eac7b 943 alarmLowRegister1=alarmLowRegister1+(all_no_Zeroing*0x20); // fatto
nerit 8:310f9e4eac7b 944 alarmLowRegister1=alarmLowRegister1+(all_genericals*0x40);
nerit 8:310f9e4eac7b 945 if (alarmLowRegister1 > 0) {
nerit 8:310f9e4eac7b 946 alarmHighRegister = 0x81;
nerit 8:310f9e4eac7b 947 alarmLowRegister = alarmLowRegister1;
nerit 8:310f9e4eac7b 948 }
nerit 3:a469bbd294b5 949
nerit 10:9e70619e97ab 950 #if defined(pcSerial)
nerit 10:9e70619e97ab 951 #if defined(VediAllarmi)
nerit 10:9e70619e97ab 952 if (all_pickSignal==1) {
nerit 10:9e70619e97ab 953 pc.printf("AllarmeBecchi\n");
nerit 10:9e70619e97ab 954 }
nerit 10:9e70619e97ab 955 if (all_cellSignal==1) {
nerit 10:9e70619e97ab 956 pc.printf("AllarmeCelle\n");
nerit 10:9e70619e97ab 957 }
nerit 10:9e70619e97ab 958 if (all_lowBattery==1) {
nerit 10:9e70619e97ab 959 pc.printf("AllarmeBassaCorrente\n");
nerit 10:9e70619e97ab 960 }
nerit 10:9e70619e97ab 961 if (all_overCurrDC==1) {
nerit 10:9e70619e97ab 962 pc.printf("AllarmeAltaCorrente\n");
nerit 10:9e70619e97ab 963 }
nerit 10:9e70619e97ab 964 if (all_stopSistem==1) {
nerit 10:9e70619e97ab 965 pc.printf("AllarmeStop\n");
nerit 10:9e70619e97ab 966 }
nerit 10:9e70619e97ab 967 if (all_noDcRotati==1) {
nerit 10:9e70619e97ab 968 pc.printf("AllarmeDCnoRotation\n");
nerit 10:9e70619e97ab 969 }
nerit 10:9e70619e97ab 970 if (all_noStepRota==1) {
nerit 10:9e70619e97ab 971 pc.printf("AllarmeNoStepRotation\n");
nerit 10:9e70619e97ab 972 }
nerit 10:9e70619e97ab 973 if (all_speedError==1) {
nerit 10:9e70619e97ab 974 pc.printf("AllarmeSpeedError\n");
nerit 10:9e70619e97ab 975 }
nerit 10:9e70619e97ab 976 if (all_noSpeedSen==1) {
nerit 10:9e70619e97ab 977 pc.printf("AllarmeNoSpeedSensor\n");
nerit 10:9e70619e97ab 978 }
nerit 10:9e70619e97ab 979 if (all_no_Zeroing==1) {
nerit 10:9e70619e97ab 980 pc.printf("AllarmeNoZero\n");
nerit 10:9e70619e97ab 981 }
nerit 10:9e70619e97ab 982 if (all_genericals==1) {
nerit 10:9e70619e97ab 983 pc.printf("AllarmeGenerico\n");
nerit 10:9e70619e97ab 984 }
nerit 10:9e70619e97ab 985 pc.printf("Code: 0x%x%x\n",alarmHighRegister,alarmLowRegister);
nerit 10:9e70619e97ab 986 #endif
nerit 10:9e70619e97ab 987 #endif
nerit 8:310f9e4eac7b 988 all_semiFiniti=0;
nerit 8:310f9e4eac7b 989 all_pickSignal=0;
nerit 8:310f9e4eac7b 990 all_cellSignal=0;
nerit 8:310f9e4eac7b 991 all_lowBattery=0;
nerit 8:310f9e4eac7b 992 all_overCurrDC=0;
nerit 8:310f9e4eac7b 993 all_stopSistem=0;
nerit 8:310f9e4eac7b 994 all_upElements=0;
nerit 8:310f9e4eac7b 995 all_noSeedOnCe=0;
nerit 8:310f9e4eac7b 996 all_cfgnErrors=0;
nerit 8:310f9e4eac7b 997 all_noDcRotati=0;
nerit 8:310f9e4eac7b 998 all_noStepRota=0;
nerit 8:310f9e4eac7b 999 all_speedError=0;
nerit 8:310f9e4eac7b 1000 all_noSpeedSen=0;
nerit 8:310f9e4eac7b 1001 all_no_Zeroing=0;
nerit 8:310f9e4eac7b 1002 all_genericals=0;
nerit 10:9e70619e97ab 1003 #if defined(pcSerial)
nerit 10:9e70619e97ab 1004 #if defined(checkLoop)
nerit 10:9e70619e97ab 1005 pc.printf("17\n");
nerit 10:9e70619e97ab 1006 #endif
nerit 10:9e70619e97ab 1007 #endif
nerit 3:a469bbd294b5 1008 }
nerit 3:a469bbd294b5 1009 //*******************************************************
nerit 3:a469bbd294b5 1010 #if defined(speedMaster)
nerit 3:a469bbd294b5 1011 void upDateSincro(){
nerit 8:310f9e4eac7b 1012 char val1[8]= {0,0,0,0,0,0,0,0};
nerit 8:310f9e4eac7b 1013 val1[3]=(posForQuinc /0x01000000)&0x000000FF;
nerit 8:310f9e4eac7b 1014 val1[2]=(posForQuinc /0x00010000)&0x000000FF;
nerit 8:310f9e4eac7b 1015 val1[1]=(posForQuinc /0x00000100)&0x000000FF;
nerit 3:a469bbd294b5 1016 val1[0]=posForQuinc & 0x000000FF;
nerit 3:a469bbd294b5 1017 //double pass = tractorSpeed_MtS_timed*100.0f;
nerit 3:a469bbd294b5 1018 double pass = speedOfSeedWheel*100.0f;
nerit 3:a469bbd294b5 1019 val1[4]=(uint8_t)(pass)&0x000000FF;
nerit 8:310f9e4eac7b 1020 val1[5]=(prePosSD /0x00010000)&0x000000FF;
nerit 8:310f9e4eac7b 1021 val1[6]=(prePosSD /0x00000100)&0x000000FF;
nerit 3:a469bbd294b5 1022 val1[7]=prePosSD & 0x000000FF;
nerit 3:a469bbd294b5 1023 #if defined(canbusActive)
nerit 3:a469bbd294b5 1024 #if defined(speedMaster)
nerit 8:310f9e4eac7b 1025 if(can1.write(CANMessage(TX_SI, *&val1,8))) {
nerit 3:a469bbd294b5 1026 checkState=0;
nerit 3:a469bbd294b5 1027 }
nerit 3:a469bbd294b5 1028 #endif
nerit 3:a469bbd294b5 1029 #endif
nerit 6:e8c18f0f399a 1030 #if defined(pcSerial)
nerit 6:e8c18f0f399a 1031 #if defined(checkLoop)
nerit 6:e8c18f0f399a 1032 pc.printf("18\n");
nerit 6:e8c18f0f399a 1033 #endif
nerit 6:e8c18f0f399a 1034 #endif
nerit 3:a469bbd294b5 1035 }
nerit 3:a469bbd294b5 1036 #endif
nerit 3:a469bbd294b5 1037 //*******************************************************
nerit 8:310f9e4eac7b 1038 void upDateSpeed()
nerit 8:310f9e4eac7b 1039 {
nerit 3:a469bbd294b5 1040 /*
nerit 3:a469bbd294b5 1041 aggiorna dati OPUSA3
nerit 3:a469bbd294b5 1042 val1[0] contiene il dato di velocità
nerit 3:a469bbd294b5 1043 val1[1] contiene il byte basso della tabella allarmi
nerit 3:a469bbd294b5 1044 uint8_t all_semiFiniti = 0; // semi finiti (generato dal relativo sensore)
nerit 3:a469bbd294b5 1045 uint8_t all_pickSignal = 0; // errore segnale becchi (generato dal tempo tra un becco ed il successivo)
nerit 3:a469bbd294b5 1046 uint8_t all_cellSignal = 0; // errore segnale celle (generato dal sensore tamburo )
nerit 3:a469bbd294b5 1047 uint8_t all_lowBattery = 0; // allarme batteria (valore interno di tritecnica)
nerit 3:a469bbd294b5 1048 uint8_t all_overCurrDC = 0; // sovracorrente motore DC (generato dal sensore di corrente)
nerit 3:a469bbd294b5 1049 uint8_t all_stopSistem = 0; // sistema in stop (a tempo con ruota ferma)
nerit 3:a469bbd294b5 1050 uint8_t all_upElements = 0; // elementi sollevati (generato dal relativo sensore)
nerit 3:a469bbd294b5 1051 uint8_t all_noSeedOnCe = 0; // fallanza di semina (manca il seme nella cella)
nerit 3:a469bbd294b5 1052 uint8_t all_cfgnErrors = 0; // errore di configurazione (incongruenza dei parametri impostati)
nerit 3:a469bbd294b5 1053 uint8_t all_noDcRotati = 0; // ruota di semina bloccata (arriva la velocità ma non i becchi)
nerit 3:a469bbd294b5 1054 uint8_t all_noStepRota = 0; // tamburo di semina bloccato (comando il tamburo ma non ricevo il sensore)
nerit 3:a469bbd294b5 1055 uint8_t all_speedError = 0; // errore sensore velocità (tempo impulsi non costante)
nerit 3:a469bbd294b5 1056 uint8_t all_noSpeedSen = 0; // mancanza segnale di velocità (sento i becchi ma non la velocita)
nerit 3:a469bbd294b5 1057 uint8_t all_no_Zeroing = 0; // mancato azzeramento sistema (generato dal timeout del comando motore DC)
nerit 3:a469bbd294b5 1058 uint8_t all_genericals = 0; // allarme generico
nerit 3:a469bbd294b5 1059 val1[2] contiene il byte alto della tabella di allarmi
nerit 3:a469bbd294b5 1060 val1[3] contiene i segnali per la diagnostica
nerit 3:a469bbd294b5 1061 bit 0= sensore ruota fonica
nerit 3:a469bbd294b5 1062 bit 1= sensore celle
nerit 3:a469bbd294b5 1063 bit 2= sensore posizione
nerit 3:a469bbd294b5 1064 bit 3= sensore becchi
nerit 3:a469bbd294b5 1065 bit 4= motore DC
nerit 3:a469bbd294b5 1066 bit 5= controller
nerit 3:a469bbd294b5 1067 bit 6= motore stepper
nerit 3:a469bbd294b5 1068 */
nerit 8:310f9e4eac7b 1069 char val1[8]= {0,0,0,0,0,0,0,0};
nerit 3:a469bbd294b5 1070
nerit 8:310f9e4eac7b 1071 val1[3]=0;
nerit 8:310f9e4eac7b 1072 val1[3]=val1[3]+(tractorSpeedRead*0x01);
nerit 8:310f9e4eac7b 1073 val1[3]=val1[3]+(TBzeroPinInput*0x02);
nerit 8:310f9e4eac7b 1074 val1[3]=val1[3]+(DcEncoder*0x04);
nerit 8:310f9e4eac7b 1075 val1[3]=val1[3]+(seedWheelZeroPinInput*0x08);
nerit 10:9e70619e97ab 1076 #if defined(simulaPerCan)
nerit 10:9e70619e97ab 1077 if (buttonUser==0) {
nerit 10:9e70619e97ab 1078 val1[1]=0x02;
nerit 10:9e70619e97ab 1079 } else {
nerit 10:9e70619e97ab 1080 val1[1]=0x00;
nerit 10:9e70619e97ab 1081 }
nerit 10:9e70619e97ab 1082 val1[3]=valore;
nerit 10:9e70619e97ab 1083 valore+=1;
nerit 10:9e70619e97ab 1084 if(valore>50) {
nerit 10:9e70619e97ab 1085 valore=0;
nerit 10:9e70619e97ab 1086 }
nerit 10:9e70619e97ab 1087 tractorSpeed_MtS_timed=valore;
nerit 10:9e70619e97ab 1088 oldLocalTractorSpeed=valore;
nerit 10:9e70619e97ab 1089 #endif
nerit 3:a469bbd294b5 1090 allarmi();
nerit 3:a469bbd294b5 1091 valore = totalSpeed*36.0f; // tractorSpeed_MtS_timed*36.0f;
nerit 3:a469bbd294b5 1092 val1[0]=valore;
nerit 3:a469bbd294b5 1093 val1[1]=alarmHighRegister; // registro alto allarmi. Parte sempre da 0x80
nerit 3:a469bbd294b5 1094 val1[2]=alarmLowRegister; // registro basso allarmi
nerit 3:a469bbd294b5 1095 //val1[3]=val1[3];// registro di stato degli ingressi
nerit 3:a469bbd294b5 1096 val1[4]=resetComandi;
nerit 3:a469bbd294b5 1097 val1[5]=cellsCounterLow;
nerit 3:a469bbd294b5 1098 val1[6]=cellsCounterHig;
nerit 10:9e70619e97ab 1099 #if defined(canbusActive)
nerit 10:9e70619e97ab 1100 if(can1.write(CANMessage(TX_ID, *&val1,8))) {
nerit 10:9e70619e97ab 1101 checkState=0;
nerit 10:9e70619e97ab 1102 }
nerit 10:9e70619e97ab 1103 #endif
nerit 10:9e70619e97ab 1104 #if defined(pcSerial)
nerit 10:9e70619e97ab 1105 #if defined(checkLoop)
nerit 10:9e70619e97ab 1106 pc.printf("19\n");
nerit 10:9e70619e97ab 1107 #endif
nerit 10:9e70619e97ab 1108 #endif
nerit 3:a469bbd294b5 1109 }
nerit 3:a469bbd294b5 1110 //*******************************************************
nerit 8:310f9e4eac7b 1111 void leggiCAN()
nerit 8:310f9e4eac7b 1112 {
nerit 8:310f9e4eac7b 1113 #if defined(canbusActive)
nerit 8:310f9e4eac7b 1114 if(can1.read(rxMsg)) {
nerit 8:310f9e4eac7b 1115 if (firstStart==1) {
nerit 8:310f9e4eac7b 1116 #if defined(speedMaster)
nerit 8:310f9e4eac7b 1117 sincUpdate.attach(&upDateSincro,0.012f);//0.009f
nerit 8:310f9e4eac7b 1118 canUpdate.attach(&upDateSpeed,0.21f);
nerit 8:310f9e4eac7b 1119 #else
nerit 8:310f9e4eac7b 1120 canUpdate.attach(&upDateSpeed,0.407f);
nerit 8:310f9e4eac7b 1121 #endif
nerit 8:310f9e4eac7b 1122 firstStart=0;
nerit 8:310f9e4eac7b 1123 }
nerit 8:310f9e4eac7b 1124 if (rxMsg.id==RX_ID) {
nerit 8:310f9e4eac7b 1125 #if defined(pcSerial)
nerit 8:310f9e4eac7b 1126 #if defined(canDataReceiveda)
nerit 8:310f9e4eac7b 1127 pc.printf("Messaggio ricevuto\n");
nerit 3:a469bbd294b5 1128 #endif
nerit 8:310f9e4eac7b 1129 #endif
nerit 8:310f9e4eac7b 1130 }
nerit 8:310f9e4eac7b 1131 if (rxMsg.id==RX_Broadcast) {
nerit 8:310f9e4eac7b 1132 #if defined(pcSerial)
nerit 8:310f9e4eac7b 1133 #if defined(canDataReceivedb)
nerit 3:a469bbd294b5 1134 pc.printf("BroadCast ricevuto\n");
nerit 3:a469bbd294b5 1135 #endif
nerit 8:310f9e4eac7b 1136 #endif
nerit 8:310f9e4eac7b 1137 enableSimula= rxMsg.data[0];
nerit 8:310f9e4eac7b 1138 speedSimula = rxMsg.data[1];
nerit 8:310f9e4eac7b 1139 avviaSimula = rxMsg.data[2];
nerit 8:310f9e4eac7b 1140 selezionato = rxMsg.data[3];
nerit 8:310f9e4eac7b 1141 comandiDaCan = rxMsg.data[4];
nerit 20:4a400a4cc419 1142 speedStepp = rxMsg.data[5];
nerit 20:4a400a4cc419 1143 if (speedStepp==0){
nerit 20:4a400a4cc419 1144 simStepper=0;
nerit 20:4a400a4cc419 1145 }else{
nerit 20:4a400a4cc419 1146 simStepper=1;
nerit 20:4a400a4cc419 1147 }
nerit 8:310f9e4eac7b 1148 #if defined(pcSerial)
nerit 8:310f9e4eac7b 1149 #if defined(canDataReceived)
nerit 3:a469bbd294b5 1150 pc.printf("Speed simula %d \n",speedSimula);
nerit 3:a469bbd294b5 1151 #endif
nerit 8:310f9e4eac7b 1152 #endif
nerit 8:310f9e4eac7b 1153 switch (comandiDaCan) {
nerit 8:310f9e4eac7b 1154 case 1:
nerit 8:310f9e4eac7b 1155 case 3:
nerit 8:310f9e4eac7b 1156 azzeraDaCan=1;
nerit 8:310f9e4eac7b 1157 resetComandi=0x01;
nerit 8:310f9e4eac7b 1158 comandiDaCan=0;
nerit 8:310f9e4eac7b 1159 break;
nerit 8:310f9e4eac7b 1160 case 2:
nerit 8:310f9e4eac7b 1161 loadDaCan=1;
nerit 8:310f9e4eac7b 1162 resetComandi=0x02;
nerit 8:310f9e4eac7b 1163 comandiDaCan=0;
nerit 8:310f9e4eac7b 1164 break;
nerit 8:310f9e4eac7b 1165 default:
nerit 8:310f9e4eac7b 1166 comandiDaCan=0;
nerit 8:310f9e4eac7b 1167 resetComandi=0xFF;
nerit 8:310f9e4eac7b 1168 break;
nerit 8:310f9e4eac7b 1169 }
nerit 8:310f9e4eac7b 1170 #if defined(pcSerial)
nerit 8:310f9e4eac7b 1171 #if defined(canDataReceivedR)
nerit 8:310f9e4eac7b 1172 pc.printf("Comandi: %x\n",resetComandi);
nerit 8:310f9e4eac7b 1173 #endif
nerit 8:310f9e4eac7b 1174 #endif
nerit 8:310f9e4eac7b 1175 if (speedSimula>45) {
nerit 8:310f9e4eac7b 1176 speedSimula=45;
nerit 8:310f9e4eac7b 1177 }
nerit 8:310f9e4eac7b 1178 // modulo 1
nerit 8:310f9e4eac7b 1179 if (RX_ID==0x100) {
nerit 8:310f9e4eac7b 1180 if ((selezionato&0x01)==0x01) {
nerit 8:310f9e4eac7b 1181 simOk=1;
nerit 8:310f9e4eac7b 1182 } else {
nerit 8:310f9e4eac7b 1183 simOk=0;
nerit 3:a469bbd294b5 1184 }
nerit 8:310f9e4eac7b 1185 }
nerit 8:310f9e4eac7b 1186 // modulo 2
nerit 8:310f9e4eac7b 1187 if (RX_ID==0x102) {
nerit 8:310f9e4eac7b 1188 if ((selezionato&0x02)==0x02) {
nerit 8:310f9e4eac7b 1189 simOk=1;
nerit 8:310f9e4eac7b 1190 } else {
nerit 8:310f9e4eac7b 1191 simOk=0;
nerit 3:a469bbd294b5 1192 }
nerit 8:310f9e4eac7b 1193 }
nerit 8:310f9e4eac7b 1194 // modulo 3
nerit 8:310f9e4eac7b 1195 if (RX_ID==0x104) {
nerit 8:310f9e4eac7b 1196 if ((selezionato&0x04)==0x04) {
nerit 8:310f9e4eac7b 1197 simOk=1;
nerit 8:310f9e4eac7b 1198 } else {
nerit 8:310f9e4eac7b 1199 simOk=0;
nerit 3:a469bbd294b5 1200 }
nerit 8:310f9e4eac7b 1201 }
nerit 8:310f9e4eac7b 1202 // modulo 4
nerit 8:310f9e4eac7b 1203 if (RX_ID==0x106) {
nerit 8:310f9e4eac7b 1204 if ((selezionato&0x08)==0x08) {
nerit 8:310f9e4eac7b 1205 simOk=1;
nerit 8:310f9e4eac7b 1206 } else {
nerit 8:310f9e4eac7b 1207 simOk=0;
nerit 3:a469bbd294b5 1208 }
nerit 8:310f9e4eac7b 1209 }
nerit 8:310f9e4eac7b 1210 // modulo 5
nerit 8:310f9e4eac7b 1211 if (RX_ID==0x108) {
nerit 8:310f9e4eac7b 1212 if ((selezionato&0x10)==0x10) {
nerit 8:310f9e4eac7b 1213 simOk=1;
nerit 8:310f9e4eac7b 1214 } else {
nerit 8:310f9e4eac7b 1215 simOk=0;
nerit 3:a469bbd294b5 1216 }
nerit 3:a469bbd294b5 1217 }
nerit 8:310f9e4eac7b 1218
nerit 8:310f9e4eac7b 1219 }
nerit 14:e2b5efa06c41 1220 //if (tractorSpeed_MtS_timed <= 0.01f){
nerit 8:310f9e4eac7b 1221 if (rxMsg.id==RX_Settings) {
nerit 14:e2b5efa06c41 1222 //if (tractorSpeed_MtS_timed==0.0f) {
nerit 3:a469bbd294b5 1223 pickNumber = rxMsg.data[0]; // numero di becchi installati sulla ruota di semina
nerit 3:a469bbd294b5 1224 cellsNumber = rxMsg.data[1]; // numero di celle del tamburo
nerit 17:9629eb019892 1225 deepOfSeed=((double)rxMsg.data[2]/1000.0f); // deep of seeding
nerit 5:2a3a64b52f54 1226 seedWheelDiameter = ((rxMsg.data[4]*0x100)+rxMsg.data[3])/10000.0f; // seed wheel diameter setting
nerit 5:2a3a64b52f54 1227 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 1228 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 1229 aggiornaParametri();
nerit 14:e2b5efa06c41 1230 //}
nerit 3:a469bbd294b5 1231 }
francescopistone 47:8455eedea226 1232
francescopistone 47:8455eedea226 1233 if (rxMsg.id==RX_AngoloAv) {
francescopistone 47:8455eedea226 1234 //if (tractorSpeed_MtS_timed==0.0f) {
francescopistone 47:8455eedea226 1235 #if defined(M1)
francescopistone 47:8455eedea226 1236 angoloAv = (double) rxMsg.data[0] ;
francescopistone 47:8455eedea226 1237 aggiornaParametri();
francescopistone 47:8455eedea226 1238 #endif
francescopistone 47:8455eedea226 1239 #if defined(M2)
francescopistone 47:8455eedea226 1240 angoloAv = (double) rxMsg.data[1] ;
francescopistone 47:8455eedea226 1241 aggiornaParametri();
francescopistone 47:8455eedea226 1242 #endif
francescopistone 47:8455eedea226 1243 #if defined(M3)
francescopistone 47:8455eedea226 1244 angoloAv = (double) rxMsg.data[2] ;
francescopistone 47:8455eedea226 1245 aggiornaParametri();
francescopistone 47:8455eedea226 1246 #endif
francescopistone 47:8455eedea226 1247 #if defined(M4)
francescopistone 47:8455eedea226 1248 angoloAv = (double) rxMsg.data[3] ;
francescopistone 47:8455eedea226 1249 aggiornaParametri();
francescopistone 47:8455eedea226 1250 #endif
francescopistone 47:8455eedea226 1251 #if defined(M5)
francescopistone 47:8455eedea226 1252 angoloAv = (double) rxMsg.data[4] ;
francescopistone 47:8455eedea226 1253 aggiornaParametri();
francescopistone 47:8455eedea226 1254 #endif
francescopistone 47:8455eedea226 1255 #if defined(M6)
francescopistone 47:8455eedea226 1256 angoloAv = (double) rxMsg.data[5] ;
francescopistone 47:8455eedea226 1257 aggiornaParametri();
francescopistone 47:8455eedea226 1258 #endif
francescopistone 47:8455eedea226 1259 //}
francescopistone 47:8455eedea226 1260 }
nerit 14:e2b5efa06c41 1261 if (tractorSpeed_MtS_timed <= 0.01f){
nerit 8:310f9e4eac7b 1262 if (rxMsg.id==RX_AngoloPh) {
nerit 8:310f9e4eac7b 1263 if (tractorSpeed_MtS_timed==0.0f) {
nerit 3:a469bbd294b5 1264 #if defined(M1)
nerit 3:a469bbd294b5 1265 angoloPh = (double) rxMsg.data[0] ;
nerit 3:a469bbd294b5 1266 aggiornaParametri();
nerit 3:a469bbd294b5 1267 #endif
nerit 3:a469bbd294b5 1268 #if defined(M2)
nerit 3:a469bbd294b5 1269 angoloPh = (double) rxMsg.data[1] ;
nerit 3:a469bbd294b5 1270 aggiornaParametri();
nerit 3:a469bbd294b5 1271 #endif
nerit 3:a469bbd294b5 1272 #if defined(M3)
nerit 3:a469bbd294b5 1273 angoloPh = (double) rxMsg.data[2] ;
nerit 3:a469bbd294b5 1274 aggiornaParametri();
nerit 3:a469bbd294b5 1275 #endif
nerit 3:a469bbd294b5 1276 #if defined(M4)
nerit 3:a469bbd294b5 1277 angoloPh = (double) rxMsg.data[3] ;
nerit 3:a469bbd294b5 1278 aggiornaParametri();
nerit 3:a469bbd294b5 1279 #endif
nerit 3:a469bbd294b5 1280 #if defined(M5)
nerit 3:a469bbd294b5 1281 angoloPh = (double) rxMsg.data[4] ;
nerit 3:a469bbd294b5 1282 aggiornaParametri();
nerit 3:a469bbd294b5 1283 #endif
nerit 3:a469bbd294b5 1284 #if defined(M6)
nerit 3:a469bbd294b5 1285 angoloPh = (double) rxMsg.data[5] ;
nerit 3:a469bbd294b5 1286 aggiornaParametri();
nerit 3:a469bbd294b5 1287 #endif
nerit 3:a469bbd294b5 1288 }
nerit 3:a469bbd294b5 1289 }
francescopistone 47:8455eedea226 1290 /*if (rxMsg.id==RX_AngoloAv) {
nerit 8:310f9e4eac7b 1291 if (tractorSpeed_MtS_timed==0.0f) {
nerit 3:a469bbd294b5 1292 #if defined(M1)
nerit 3:a469bbd294b5 1293 angoloAv = (double) rxMsg.data[0] ;
nerit 3:a469bbd294b5 1294 aggiornaParametri();
nerit 3:a469bbd294b5 1295 #endif
nerit 3:a469bbd294b5 1296 #if defined(M2)
nerit 3:a469bbd294b5 1297 angoloAv = (double) rxMsg.data[1] ;
nerit 3:a469bbd294b5 1298 aggiornaParametri();
nerit 3:a469bbd294b5 1299 #endif
nerit 3:a469bbd294b5 1300 #if defined(M3)
nerit 3:a469bbd294b5 1301 angoloAv = (double) rxMsg.data[2] ;
nerit 3:a469bbd294b5 1302 aggiornaParametri();
nerit 3:a469bbd294b5 1303 #endif
nerit 3:a469bbd294b5 1304 #if defined(M4)
nerit 3:a469bbd294b5 1305 angoloAv = (double) rxMsg.data[3] ;
nerit 3:a469bbd294b5 1306 aggiornaParametri();
nerit 3:a469bbd294b5 1307 #endif
nerit 3:a469bbd294b5 1308 #if defined(M5)
nerit 3:a469bbd294b5 1309 angoloAv = (double) rxMsg.data[4] ;
nerit 3:a469bbd294b5 1310 aggiornaParametri();
nerit 3:a469bbd294b5 1311 #endif
nerit 3:a469bbd294b5 1312 #if defined(M6)
nerit 3:a469bbd294b5 1313 angoloAv = (double) rxMsg.data[5] ;
nerit 3:a469bbd294b5 1314 aggiornaParametri();
nerit 3:a469bbd294b5 1315 #endif
nerit 3:a469bbd294b5 1316 }
francescopistone 47:8455eedea226 1317 }*/
nerit 8:310f9e4eac7b 1318 if (rxMsg.id==RX_Quinconce) {
nerit 8:310f9e4eac7b 1319 if (tractorSpeed_MtS_timed==0.0f) {
nerit 8:310f9e4eac7b 1320 quinconceActive = (uint8_t) rxMsg.data[0];
nerit 8:310f9e4eac7b 1321 quincPIDminus = (uint8_t) rxMsg.data[1];
nerit 8:310f9e4eac7b 1322 quincPIDplus = (uint8_t) rxMsg.data[2];
nerit 8:310f9e4eac7b 1323 quincLIMminus = (uint8_t) rxMsg.data[3];
nerit 8:310f9e4eac7b 1324 quincLIMplus = (uint8_t) rxMsg.data[4];
nerit 3:a469bbd294b5 1325 quincSector = (uint8_t) rxMsg.data[5];
nerit 3:a469bbd294b5 1326 aggiornaParametri();
nerit 3:a469bbd294b5 1327 }
nerit 3:a469bbd294b5 1328 }
nerit 8:310f9e4eac7b 1329 }
nerit 8:310f9e4eac7b 1330 if (tractorSpeed_MtS_timed > 0.0f){
nerit 8:310f9e4eac7b 1331 if (rxMsg.id==RX_QuincSinc) {
nerit 8:310f9e4eac7b 1332 masterSinc = (uint32_t) rxMsg.data[3] * 0x01000000;
nerit 8:310f9e4eac7b 1333 masterSinc = masterSinc + ((uint32_t) rxMsg.data[2] * 0x00010000);
nerit 8:310f9e4eac7b 1334 masterSinc = masterSinc + ((uint32_t) rxMsg.data[1] * 0x00000100);
nerit 3:a469bbd294b5 1335 masterSinc = masterSinc + ((uint32_t) rxMsg.data[0]);
nerit 3:a469bbd294b5 1336 speedFromMaster = (double)rxMsg.data[4]/100.0f;
nerit 8:310f9e4eac7b 1337 mast2_Sinc = ((uint32_t) rxMsg.data[5] * 0x00010000);
nerit 8:310f9e4eac7b 1338 mast2_Sinc = mast2_Sinc + ((uint32_t) rxMsg.data[6] * 0x00000100);
nerit 3:a469bbd294b5 1339 mast2_Sinc = mast2_Sinc + ((uint32_t) rxMsg.data[7]);
nerit 3:a469bbd294b5 1340 canDataCheck=1;
nerit 3:a469bbd294b5 1341 }
nerit 8:310f9e4eac7b 1342 }
nerit 33:81b406a911b6 1343 //if (tractorSpeed_MtS_timed <= 0.01f){
nerit 8:310f9e4eac7b 1344 if (rxMsg.id==RX_Configure) {
nerit 3:a469bbd294b5 1345 uint8_t flags = rxMsg.data[0];
nerit 32:cfc3d7420fa5 1346 if ((flags&0x80)==0x80) { // comando taglianylon
nerit 33:81b406a911b6 1347 if (oldTagliaNylon==false){
nerit 33:81b406a911b6 1348 tagliaNylonAttivo=true;
nerit 33:81b406a911b6 1349 oldTagliaNylon=true;
nerit 33:81b406a911b6 1350 }
nerit 32:cfc3d7420fa5 1351 } else {
nerit 33:81b406a911b6 1352 if (oldTagliaNylon==true){
nerit 33:81b406a911b6 1353 tagliaNylonAttivo=false;
nerit 33:81b406a911b6 1354 oldTagliaNylon=false;
nerit 3:a469bbd294b5 1355 }
nerit 33:81b406a911b6 1356 if ((tagliaNylonAttivo==false)&&(tractorSpeed_MtS_timed<=0.01f)){
nerit 33:81b406a911b6 1357 uint16_t steps = (uint32_t) rxMsg.data[2]*0x00000100;
nerit 33:81b406a911b6 1358 steps = steps + ((uint32_t)rxMsg.data[1]);
nerit 33:81b406a911b6 1359 TBmotorSteps =steps;
nerit 37:0a225902cf48 1360 #if defined(oldStepperDriver)
nerit 37:0a225902cf48 1361 stepSetting();
nerit 37:0a225902cf48 1362 #endif
nerit 33:81b406a911b6 1363 cellsCountSet = rxMsg.data[3];
nerit 33:81b406a911b6 1364 if ((flags&0x01)==0x01) {
nerit 33:81b406a911b6 1365 if (encoder==false) {
nerit 33:81b406a911b6 1366 encoder=true;
nerit 33:81b406a911b6 1367 speedFromPick=0;
nerit 33:81b406a911b6 1368 DcEncoder.rise(NULL);
nerit 33:81b406a911b6 1369 dcSetting();
nerit 33:81b406a911b6 1370 }
nerit 33:81b406a911b6 1371 } else {
nerit 33:81b406a911b6 1372 if (encoder==true) {
nerit 33:81b406a911b6 1373 encoder=false;
nerit 33:81b406a911b6 1374 speedFromPick=1;
nerit 33:81b406a911b6 1375 DcEncoder.rise(NULL);
nerit 33:81b406a911b6 1376 dcSetting();
nerit 33:81b406a911b6 1377 }
nerit 33:81b406a911b6 1378 }
nerit 33:81b406a911b6 1379 if ((flags&0x02)==0x02) {
nerit 33:81b406a911b6 1380 tankLevelEnable=true;
nerit 33:81b406a911b6 1381 } else {
nerit 33:81b406a911b6 1382 tankLevelEnable=false;
nerit 33:81b406a911b6 1383 }
nerit 39:6814c75dfa5c 1384 #if !defined(oldStepperDriver)
nerit 39:6814c75dfa5c 1385 if ((flags&0x04)==0x04) {
nerit 39:6814c75dfa5c 1386 seedSensorEnable=true;
nerit 39:6814c75dfa5c 1387 } else {
nerit 39:6814c75dfa5c 1388 seedSensorEnable=false;
nerit 39:6814c75dfa5c 1389 }
nerit 39:6814c75dfa5c 1390 #else
nerit 33:81b406a911b6 1391 seedSensorEnable=false;
nerit 39:6814c75dfa5c 1392 #endif
nerit 33:81b406a911b6 1393 if ((flags&0x08)==0x08) {
nerit 33:81b406a911b6 1394 drumSelect=true; // usare per selezione del tamburo =0 meccanico =1 PNEUMATICO
nerit 33:81b406a911b6 1395 } else {
nerit 33:81b406a911b6 1396 drumSelect=false; // forzato a true per Germania
nerit 33:81b406a911b6 1397 }
nerit 33:81b406a911b6 1398 if ((flags&0x10)==0x10) {
nerit 33:81b406a911b6 1399 canDataCheckEnable=true;
nerit 33:81b406a911b6 1400 } else {
nerit 33:81b406a911b6 1401 canDataCheckEnable=false;
nerit 33:81b406a911b6 1402 }
nerit 33:81b406a911b6 1403 if ((flags&0x20)==0x20) {
nerit 33:81b406a911b6 1404 tamburoStandard=1;
nerit 33:81b406a911b6 1405 } else {
nerit 33:81b406a911b6 1406 tamburoStandard=0;
nerit 33:81b406a911b6 1407 }
nerit 33:81b406a911b6 1408 if ((flags&0x40)==0x40) {
nerit 33:81b406a911b6 1409 currentCheckEnable=true;
nerit 33:81b406a911b6 1410 } else {
nerit 33:81b406a911b6 1411 currentCheckEnable=false;
nerit 33:81b406a911b6 1412 }
nerit 33:81b406a911b6 1413 aggiornaParametri();
nerit 3:a469bbd294b5 1414 }
nerit 3:a469bbd294b5 1415 }
nerit 3:a469bbd294b5 1416 }
nerit 33:81b406a911b6 1417 //}
nerit 8:310f9e4eac7b 1418 }
nerit 10:9e70619e97ab 1419 #endif
nerit 10:9e70619e97ab 1420 #if defined(overWriteCanSimulation)
nerit 10:9e70619e97ab 1421 enableSimula=1;
nerit 10:9e70619e97ab 1422 speedSimula=25;
nerit 10:9e70619e97ab 1423 avviaSimula=1;
nerit 10:9e70619e97ab 1424 simOk=1;
nerit 10:9e70619e97ab 1425 #endif
nerit 10:9e70619e97ab 1426 #if defined(pcSerial)
nerit 10:9e70619e97ab 1427 #if defined(checkLoop)
nerit 10:9e70619e97ab 1428 pc.printf("20\n");
nerit 10:9e70619e97ab 1429 #endif
nerit 10:9e70619e97ab 1430 #endif
bcostm 0:5701b41769fd 1431 }
bcostm 0:5701b41769fd 1432
nerit 3:a469bbd294b5 1433 //*******************************************************
nerit 8:310f9e4eac7b 1434 void DC_CheckCurrent()
nerit 8:310f9e4eac7b 1435 {
nerit 8:310f9e4eac7b 1436
nerit 3:a469bbd294b5 1437 // TODO: tabella di riferimento assorbimenti alle varie velocità al fine di gestire
nerit 3:a469bbd294b5 1438 // gli allarmi e le correzioni di velocità
nerit 8:310f9e4eac7b 1439
nerit 3:a469bbd294b5 1440 //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 1441 //int SD_analogIndex[10]={0,0,0,0,0,0,0,0,0,0};
nerit 3:a469bbd294b5 1442 // Analog reading
nerit 3:a469bbd294b5 1443 //number = floor(number * 100) / 100;
nerit 8:310f9e4eac7b 1444 timeout.detach();
nerit 8:310f9e4eac7b 1445 SD_CurrentAnalog = floor(SDcurrent.read()*100)/100; // valore in ingresso compreso tra 0.00 e 1.00
nerit 12:b0fc1d313813 1446 SD_CurrentScaled = floor(( (SD_CurrentStart-SD_CurrentAnalog)*3.3f) / (SD_CurrentFactor/1000.0f)*10)/10;
nerit 3:a469bbd294b5 1447 }
nerit 3:a469bbd294b5 1448 //*******************************************************
nerit 8:310f9e4eac7b 1449 void DC_prepare()
nerit 8:310f9e4eac7b 1450 {
nerit 3:a469bbd294b5 1451 // direction or brake preparation
nerit 8:310f9e4eac7b 1452 if (DC_brake==1) {
nerit 3:a469bbd294b5 1453 SDmotorInA=1;
nerit 3:a469bbd294b5 1454 SDmotorInB=1;
nerit 8:310f9e4eac7b 1455 } else {
nerit 8:310f9e4eac7b 1456 if (DC_forward==0) {
nerit 3:a469bbd294b5 1457 SDmotorInA=1;
nerit 3:a469bbd294b5 1458 SDmotorInB=0;
nerit 8:310f9e4eac7b 1459 } else {
nerit 3:a469bbd294b5 1460 SDmotorInA=0;
nerit 3:a469bbd294b5 1461 SDmotorInB=1;
nerit 3:a469bbd294b5 1462 }
nerit 3:a469bbd294b5 1463 }
nerit 3:a469bbd294b5 1464 // fault reading
nerit 8:310f9e4eac7b 1465 if (SDmotorInA==1) {
nerit 8:310f9e4eac7b 1466 SD_faultA=1;
nerit 8:310f9e4eac7b 1467 } else {
nerit 8:310f9e4eac7b 1468 SD_faultA=0;
nerit 8:310f9e4eac7b 1469 }
nerit 8:310f9e4eac7b 1470 if (SDmotorInB==1) {
nerit 8:310f9e4eac7b 1471 SD_faultB=1;
nerit 8:310f9e4eac7b 1472 } else {
nerit 8:310f9e4eac7b 1473 SD_faultB=0;
nerit 8:310f9e4eac7b 1474 }
nerit 10:9e70619e97ab 1475 #if defined(pcSerial)
nerit 10:9e70619e97ab 1476 #if defined(checkLoopa)
nerit 10:9e70619e97ab 1477 pc.printf("22\n");
nerit 10:9e70619e97ab 1478 #endif
nerit 10:9e70619e97ab 1479 #endif
nerit 3:a469bbd294b5 1480 }
nerit 3:a469bbd294b5 1481 //*******************************************************
nerit 12:b0fc1d313813 1482 void startDelay(){
nerit 12:b0fc1d313813 1483 SD_CurrentStart = floor(SDcurrent.read()*100)/100; // valore in ingresso compreso tra 0.00 e 1.00
nerit 12:b0fc1d313813 1484 int ritardo =0;
nerit 12:b0fc1d313813 1485 ritardo = (int)((float)(dcActualDuty*800.0f));
nerit 12:b0fc1d313813 1486 timeout.attach_us(&DC_CheckCurrent,ritardo);
nerit 3:a469bbd294b5 1487 }
nerit 3:a469bbd294b5 1488 //*******************************************************
nerit 10:9e70619e97ab 1489 void quincTrigon(){
nerit 3:a469bbd294b5 1490 quincClock=true;
nerit 10:9e70619e97ab 1491 #if defined(pcSerial)
nerit 10:9e70619e97ab 1492 #if defined(checkLoop)
nerit 10:9e70619e97ab 1493 pc.printf("24\n");
nerit 10:9e70619e97ab 1494 #endif
nerit 10:9e70619e97ab 1495 #endif
nerit 3:a469bbd294b5 1496 }
nerit 10:9e70619e97ab 1497 void quincTrigof(){
nerit 3:a469bbd294b5 1498 quincClock=false;
nerit 10:9e70619e97ab 1499 #if defined(pcSerial)
nerit 10:9e70619e97ab 1500 #if defined(checkLoop)
nerit 10:9e70619e97ab 1501 pc.printf("25\n");
nerit 10:9e70619e97ab 1502 #endif
nerit 10:9e70619e97ab 1503 #endif
nerit 3:a469bbd294b5 1504 }
nerit 3:a469bbd294b5 1505 //*******************************************************
nerit 8:310f9e4eac7b 1506 void quinCalc()
nerit 8:310f9e4eac7b 1507 {
nerit 3:a469bbd294b5 1508 // riceve l'impulso di sincro dal master e fa partire il timer di verifica dell'errore
nerit 10:9e70619e97ab 1509 #if !defined(mezzo)
nerit 10:9e70619e97ab 1510 if ((quincClock==true)&&(oldQuincIn==0)) {
nerit 10:9e70619e97ab 1511 oldQuincIn=1;
nerit 10:9e70619e97ab 1512 if (quincStart==0) {
nerit 10:9e70619e97ab 1513 oldQuincTimeSD = (double) quincTimeSD.read_ms();
nerit 10:9e70619e97ab 1514 quincTime.reset();
nerit 10:9e70619e97ab 1515 quincTimeSD.reset();
nerit 10:9e70619e97ab 1516 quincStart=1;
nerit 10:9e70619e97ab 1517 }
nerit 3:a469bbd294b5 1518 }
nerit 10:9e70619e97ab 1519 if(quincClock==false) {
nerit 3:a469bbd294b5 1520 oldQuincIn=0;
nerit 3:a469bbd294b5 1521 }
nerit 10:9e70619e97ab 1522 #else
nerit 10:9e70619e97ab 1523 if ((((quinconceActive==0)&&(quincClock==true))||((quinconceActive==1)&&(quincClock==false)))&&(oldQuincIn==0)) {
nerit 10:9e70619e97ab 1524 oldQuincIn=1;
nerit 10:9e70619e97ab 1525 if (quincStart==0) {
nerit 10:9e70619e97ab 1526 oldQuincTimeSD = (double) quincTimeSD.read_ms();
nerit 10:9e70619e97ab 1527 quincTime.reset();
nerit 10:9e70619e97ab 1528 quincStart=1;
nerit 10:9e70619e97ab 1529 }
nerit 3:a469bbd294b5 1530 }
nerit 10:9e70619e97ab 1531 if (quinconceActive==0) {
nerit 10:9e70619e97ab 1532 if (quincClock==false) {
nerit 10:9e70619e97ab 1533 oldQuincIn=0;
nerit 10:9e70619e97ab 1534 }
nerit 10:9e70619e97ab 1535 } else {
nerit 10:9e70619e97ab 1536 if (quincClock==true) {
nerit 10:9e70619e97ab 1537 oldQuincIn=0;
nerit 10:9e70619e97ab 1538 }
nerit 10:9e70619e97ab 1539 }
nerit 10:9e70619e97ab 1540 #endif
nerit 3:a469bbd294b5 1541 //****************************************************************************************
nerit 8:310f9e4eac7b 1542 if (quincCnt>=4) {
nerit 8:310f9e4eac7b 1543 if (countPicks==0) {
nerit 8:310f9e4eac7b 1544 if ((sincroQui==1)&&(quincStart==0)) {
nerit 3:a469bbd294b5 1545 // decelera
nerit 3:a469bbd294b5 1546 countPicks=1;
nerit 3:a469bbd294b5 1547 }
nerit 8:310f9e4eac7b 1548 if ((sincroQui==0)&&(quincStart==1)) {
nerit 3:a469bbd294b5 1549 // accelera
nerit 3:a469bbd294b5 1550 countPicks=2;
nerit 3:a469bbd294b5 1551 }
nerit 3:a469bbd294b5 1552 }
nerit 8:310f9e4eac7b 1553 if ((sincroQui==1)&&(quincStart==1)) {
nerit 8:310f9e4eac7b 1554 if (countPicks==1) { //decelera
nerit 3:a469bbd294b5 1555 scostamento = oldQuincTimeSD;
nerit 17:9629eb019892 1556 if ((scostamento < (tempoBecchiPerQuinc*0.85f))) {
nerit 3:a469bbd294b5 1557 double scostPerc = (scostamento/tempoBecchiPerQuinc);
nerit 3:a469bbd294b5 1558 percento -= ((double)quincPIDminus/100.0f)*(scostPerc);
nerit 3:a469bbd294b5 1559 #if defined(pcSerial)
nerit 3:a469bbd294b5 1560 #if defined(laq)
nerit 3:a469bbd294b5 1561 pc.printf("RALL scos2: %f tbpq: %f percento: %f \n",scostamento,tempoBecchiPerQuinc,percento);
nerit 3:a469bbd294b5 1562 #endif
nerit 3:a469bbd294b5 1563 #endif
nerit 3:a469bbd294b5 1564 }
nerit 3:a469bbd294b5 1565 //if (scostamento <15.0f){percento=0.0f;}
nerit 3:a469bbd294b5 1566 }
nerit 8:310f9e4eac7b 1567 if (countPicks==2) { //accelera
nerit 3:a469bbd294b5 1568 scostamento = (double)quincTime.read_ms();
nerit 17:9629eb019892 1569 if (scostamento < (tempoBecchiPerQuinc*0.85f)) {
nerit 3:a469bbd294b5 1570 double scostPerc = (scostamento/tempoBecchiPerQuinc);
nerit 3:a469bbd294b5 1571 percento += ((double)quincPIDplus/100.0f)*(scostPerc);
nerit 3:a469bbd294b5 1572 #if defined(pcSerial)
nerit 3:a469bbd294b5 1573 #if defined(laq)
nerit 8:310f9e4eac7b 1574 pc.printf("ACCE scos1: %f tbpq: %f percento: %f \n",scostamento,tempoBecchiPerQuinc,percento);
nerit 3:a469bbd294b5 1575 #endif
nerit 3:a469bbd294b5 1576 #endif
nerit 3:a469bbd294b5 1577 }
nerit 3:a469bbd294b5 1578 //if (scostamento <15.0f){percento=0.0f;}
nerit 3:a469bbd294b5 1579 }
nerit 3:a469bbd294b5 1580 sincroQui=0;
nerit 3:a469bbd294b5 1581 quincStart=0;
nerit 3:a469bbd294b5 1582 countPicks=0;
nerit 5:2a3a64b52f54 1583 // questo e il primo
nerit 3:a469bbd294b5 1584 #if !defined(speedMaster)
nerit 8:310f9e4eac7b 1585 if (quincCnt>=3) {
nerit 8:310f9e4eac7b 1586 if (speedFromMaster>0.0f) {
nerit 8:310f9e4eac7b 1587 if (enableSimula==0) {
nerit 8:310f9e4eac7b 1588 tractorSpeed_MtS_timed = speedFromMaster + percento;
nerit 14:e2b5efa06c41 1589 /*if (tractorSpeed_MtS_timed > (oldLocalTractorSpeed*1.15f)){
nerit 10:9e70619e97ab 1590 tractorSpeed_MtS_timed = oldLocalTractorSpeed*1.15f;
nerit 10:9e70619e97ab 1591 }
nerit 13:d1030d4e51a8 1592 if (tractorSpeed_MtS_timed < (oldLocalTractorSpeed/1.15f)){
nerit 10:9e70619e97ab 1593 tractorSpeed_MtS_timed = oldLocalTractorSpeed/1.15f;
nerit 14:e2b5efa06c41 1594 }*/
nerit 3:a469bbd294b5 1595 }
nerit 8:310f9e4eac7b 1596 }
nerit 3:a469bbd294b5 1597 }
nerit 3:a469bbd294b5 1598 #endif
nerit 3:a469bbd294b5 1599 }
nerit 8:310f9e4eac7b 1600
nerit 3:a469bbd294b5 1601 //*******************************************************************
nerit 8:310f9e4eac7b 1602 if (canDataCheckEnable==true) {
nerit 8:310f9e4eac7b 1603 if (canDataCheck==1) { // sincro da comunicazione can del valore di posizione del tamburo master
nerit 3:a469bbd294b5 1604 canDataCheck=0;
nerit 17:9629eb019892 1605 double parametro = SDsectorStep/(double)quincSector;
nerit 3:a469bbd294b5 1606 double differenza=0.0f;
nerit 3:a469bbd294b5 1607 #if defined(mezzo)
nerit 8:310f9e4eac7b 1608 if (quinconceActive==1) {
nerit 3:a469bbd294b5 1609 differenza = (double)masterSinc - (double)prePosSD;
nerit 8:310f9e4eac7b 1610 } else {
nerit 3:a469bbd294b5 1611 differenza = (double)mast2_Sinc - (double)prePosSD;
nerit 3:a469bbd294b5 1612 }
nerit 3:a469bbd294b5 1613 #else
nerit 3:a469bbd294b5 1614 differenza = (double)mast2_Sinc - (double)prePosSD;
nerit 3:a469bbd294b5 1615 #endif
nerit 17:9629eb019892 1616 if (abs(differenza)<=50.0f){differenza=0.0f;}
nerit 8:310f9e4eac7b 1617 if ((differenza > 0.0f)&&(differenza < parametro)) {
nerit 8:310f9e4eac7b 1618 double diffPerc = differenza / parametro;
nerit 3:a469bbd294b5 1619 percento += ((double)quincPIDplus/100.0f)*abs(diffPerc);
nerit 3:a469bbd294b5 1620 }
nerit 8:310f9e4eac7b 1621 if ((differenza < 0.0f)&&(abs(differenza) < parametro)) {
nerit 8:310f9e4eac7b 1622 double diffPerc = (double)differenza / parametro;
nerit 3:a469bbd294b5 1623 percento -= ((double)quincPIDminus/100.0f)*abs(diffPerc);
nerit 3:a469bbd294b5 1624 }
nerit 5:2a3a64b52f54 1625 // questo e il secondo
nerit 3:a469bbd294b5 1626 #if !defined(speedMaster)
nerit 8:310f9e4eac7b 1627 if (quincCnt>=3) {
nerit 8:310f9e4eac7b 1628 if (speedFromMaster>0.0f) {
nerit 8:310f9e4eac7b 1629 if (enableSimula==0) {
nerit 5:2a3a64b52f54 1630 tractorSpeed_MtS_timed = speedFromMaster + percento;
nerit 14:e2b5efa06c41 1631 /*if (tractorSpeed_MtS_timed > (oldLocalTractorSpeed*1.15f)){
nerit 10:9e70619e97ab 1632 tractorSpeed_MtS_timed = oldLocalTractorSpeed*1.15f;
nerit 10:9e70619e97ab 1633 }
nerit 13:d1030d4e51a8 1634 if (tractorSpeed_MtS_timed < (oldLocalTractorSpeed/1.15f)){
nerit 10:9e70619e97ab 1635 tractorSpeed_MtS_timed = oldLocalTractorSpeed/1.15f;
nerit 14:e2b5efa06c41 1636 }*/
nerit 3:a469bbd294b5 1637 }
nerit 5:2a3a64b52f54 1638 }
nerit 3:a469bbd294b5 1639 }
nerit 3:a469bbd294b5 1640 #endif
nerit 3:a469bbd294b5 1641 }
nerit 3:a469bbd294b5 1642 }
nerit 8:310f9e4eac7b 1643
nerit 3:a469bbd294b5 1644 }
nerit 8:310f9e4eac7b 1645 if ((percento) > ((double) quincLIMplus/100.0f)) {
nerit 3:a469bbd294b5 1646 percento= (double)quincLIMplus/100.0f;
nerit 3:a469bbd294b5 1647 }
nerit 8:310f9e4eac7b 1648 if ((percento) < (((double)quincLIMminus*-1.0f)/100.0f)) {
nerit 3:a469bbd294b5 1649 percento=((double)quincLIMminus*-1.0f)/100.0f;
bcostm 0:5701b41769fd 1650 }
nerit 6:e8c18f0f399a 1651 #if defined(pcSerial)
nerit 6:e8c18f0f399a 1652 #if defined(checkLoop)
nerit 6:e8c18f0f399a 1653 pc.printf("26\n");
nerit 6:e8c18f0f399a 1654 #endif
nerit 6:e8c18f0f399a 1655 #endif
bcostm 0:5701b41769fd 1656 }
nerit 8:310f9e4eac7b 1657 // ----------------------------------------
nerit 3:a469bbd294b5 1658 #if defined(seedSensor)
nerit 3:a469bbd294b5 1659 void resetDelay(){
nerit 3:a469bbd294b5 1660 delaySeedCheck.reset();
nerit 3:a469bbd294b5 1661 delaySeedCheck.stop();
nerit 8:310f9e4eac7b 1662 #if defined(pcSerial)
nerit 8:310f9e4eac7b 1663 #if defined(checkLoop)
nerit 8:310f9e4eac7b 1664 pc.printf("27\n");
nerit 8:310f9e4eac7b 1665 #endif
nerit 6:e8c18f0f399a 1666 #endif
nerit 3:a469bbd294b5 1667 }
nerit 3:a469bbd294b5 1668 #endif
nerit 18:7c978f69cc51 1669 // ----------------------------------------
nerit 17:9629eb019892 1670 void aggiornati(){
nerit 17:9629eb019892 1671 #if defined(runner)
nerit 17:9629eb019892 1672 legPos.detach();
nerit 17:9629eb019892 1673 TBoldPosition= (uint32_t) motor->get_position();
nerit 17:9629eb019892 1674 legPos.attach(&step_Reading,0.002f);
nerit 17:9629eb019892 1675 #if defined(pcSerial)
nerit 17:9629eb019892 1676 #if defined(TBperS)
nerit 17:9629eb019892 1677 pc.printf("TBoldPos: %d\n",TBoldPosition);
nerit 17:9629eb019892 1678 #endif
nerit 17:9629eb019892 1679 #endif
nerit 17:9629eb019892 1680
nerit 17:9629eb019892 1681 #else
nerit 17:9629eb019892 1682 TBactualPosition=0;
nerit 17:9629eb019892 1683 #endif
nerit 17:9629eb019892 1684 }
nerit 17:9629eb019892 1685
nerit 3:a469bbd294b5 1686 // ------------------------------------------------------------------------------------------------------------------------------------------------------------------
nerit 3:a469bbd294b5 1687 // MAIN SECTION
nerit 3:a469bbd294b5 1688 // ---------------------------------------------------------------------------------------------------------------------------------------------------------------
bcostm 0:5701b41769fd 1689
nerit 3:a469bbd294b5 1690 //*******************************************************************************
nerit 8:310f9e4eac7b 1691 int main()
nerit 8:310f9e4eac7b 1692 {
nerit 6:e8c18f0f399a 1693 #if defined(pcSerial)
nerit 6:e8c18f0f399a 1694 #if defined(resetCpu)
nerit 6:e8c18f0f399a 1695 pc.printf("RESET\n");
nerit 6:e8c18f0f399a 1696 #endif
nerit 6:e8c18f0f399a 1697 #endif
nerit 8:310f9e4eac7b 1698
nerit 8:310f9e4eac7b 1699 #if defined(canbusActive)
nerit 8:310f9e4eac7b 1700 can1.attach(&leggiCAN, CAN::RxIrq);
nerit 8:310f9e4eac7b 1701 #endif
nerit 8:310f9e4eac7b 1702
nerit 8:310f9e4eac7b 1703 wait(1.0f);
nerit 8:310f9e4eac7b 1704 wait(1.0f);
nerit 19:231b8931cabc 1705 wait(ritAvv);
nerit 8:310f9e4eac7b 1706
nerit 37:0a225902cf48 1707 #if defined(oldStepperDriver)
nerit 37:0a225902cf48 1708 stepSetting();
nerit 37:0a225902cf48 1709 #endif
nerit 37:0a225902cf48 1710
nerit 37:0a225902cf48 1711 #if defined(runner)
nerit 37:0a225902cf48 1712 TBmotor_SW=1;
nerit 37:0a225902cf48 1713 #endif
nerit 8:310f9e4eac7b 1714 TBmotorDirecti=TBforward; // reset stepper direction
nerit 8:310f9e4eac7b 1715 //----- Initialization
nerit 8:310f9e4eac7b 1716 /* Initializing SPI bus. */
nerit 8:310f9e4eac7b 1717 // dev_spi(mosi,miso,sclk)
nerit 8:310f9e4eac7b 1718 // D11= PA7; D12= PA6; D13= PA5
nerit 35:3165f4c1c7bf 1719 #if defined(runner)
nerit 35:3165f4c1c7bf 1720 DevSPI dev_spi(D11, D12, D13);
nerit 35:3165f4c1c7bf 1721 dev_spi.frequency(5000000);
nerit 10:9e70619e97ab 1722
nerit 35:3165f4c1c7bf 1723 /* Initializing Motor Control Component. */
nerit 35:3165f4c1c7bf 1724 // powerstep01( flag, busy,stby, stck, cs, dev_spi)
nerit 35:3165f4c1c7bf 1725 // motor = new PowerStep01(D2, D4, D8, D9, D10, dev_spi); // linea standard per IHM03A1
nerit 35:3165f4c1c7bf 1726 motor = new PowerStep01(PA_8, PC_7, PC_4, PB_3, PB_6, dev_spi); // linea per scheda seminatrice V7
nerit 35:3165f4c1c7bf 1727 if (motor->init(&init) != COMPONENT_OK) {
nerit 35:3165f4c1c7bf 1728 exit(EXIT_FAILURE);
nerit 35:3165f4c1c7bf 1729 }
nerit 35:3165f4c1c7bf 1730
nerit 35:3165f4c1c7bf 1731 /* Attaching and enabling an interrupt handler. */
nerit 35:3165f4c1c7bf 1732 motor->attach_flag_irq(&my_flag_irq_handler);
nerit 35:3165f4c1c7bf 1733 motor->enable_flag_irq();
nerit 35:3165f4c1c7bf 1734 //motor->disable_flag_irq();
nerit 35:3165f4c1c7bf 1735
nerit 35:3165f4c1c7bf 1736 /* Attaching an error handler */
nerit 35:3165f4c1c7bf 1737 //motor->attach_error_handler(&my_error_handler);
nerit 35:3165f4c1c7bf 1738 #endif
nerit 8:310f9e4eac7b 1739 wait(1.0f);
nerit 8:310f9e4eac7b 1740 for (int a=0; a<5; a++) {
nerit 3:a469bbd294b5 1741 mediaSpeed[a]=0;
nerit 3:a469bbd294b5 1742 }
nerit 8:310f9e4eac7b 1743
nerit 3:a469bbd294b5 1744 // DC reset ad set
nerit 3:a469bbd294b5 1745 int decima = 100;
nerit 3:a469bbd294b5 1746 wait_ms(200);
nerit 3:a469bbd294b5 1747 SD_CurrentStart=floor(SDcurrent.read()*decima)/decima;
nerit 3:a469bbd294b5 1748 wait_ms(2);
nerit 3:a469bbd294b5 1749 SD_CurrentStart+=floor(SDcurrent.read()*decima)/decima;
nerit 3:a469bbd294b5 1750 wait_ms(1);
nerit 3:a469bbd294b5 1751 SD_CurrentStart+=floor(SDcurrent.read()*decima)/decima;
nerit 3:a469bbd294b5 1752 wait_ms(3);
nerit 3:a469bbd294b5 1753 SD_CurrentStart+=floor(SDcurrent.read()*decima)/decima;
nerit 8:310f9e4eac7b 1754 SD_CurrentStart=(floor((SD_CurrentStart/4.0f)*decima)/decima)-0.01f;
nerit 3:a469bbd294b5 1755 wait_ms(100);
nerit 3:a469bbd294b5 1756 DC_prepare();
nerit 8:310f9e4eac7b 1757
nerit 8:310f9e4eac7b 1758 speedTimer.start(); // speed pulse timer
nerit 3:a469bbd294b5 1759 intraPickTimer.start();
nerit 8:310f9e4eac7b 1760 speedTimeOut.start();
nerit 3:a469bbd294b5 1761 speedFilter.start();
nerit 3:a469bbd294b5 1762 seedFilter.start();
nerit 3:a469bbd294b5 1763 TBfilter.start();
nerit 3:a469bbd294b5 1764 sincroTimer.start();
nerit 3:a469bbd294b5 1765 rotationTimeOut.start();
nerit 3:a469bbd294b5 1766 metalTimer.start();
nerit 3:a469bbd294b5 1767 quincTime.start();
nerit 3:a469bbd294b5 1768 quincTimeSD.start();
nerit 8:310f9e4eac7b 1769 #if defined(runner)
nerit 8:310f9e4eac7b 1770 legPos.attach(&step_Reading,0.002f);
nerit 8:310f9e4eac7b 1771 #endif
nerit 8:310f9e4eac7b 1772
nerit 3:a469bbd294b5 1773 //*******************************************************
nerit 3:a469bbd294b5 1774 // controls for check DC motor current
nerit 8:310f9e4eac7b 1775 pwmCheck.rise(&startDelay);
nerit 3:a469bbd294b5 1776 wait_ms(500);
nerit 8:310f9e4eac7b 1777
nerit 12:b0fc1d313813 1778 dcVeri.attach(&controllaCorrente,2.0f);
nerit 13:d1030d4e51a8 1779 delaySpeedCheck.start();
nerit 12:b0fc1d313813 1780
nerit 8:310f9e4eac7b 1781 #if defined(runnerTos)
nerit 8:310f9e4eac7b 1782 thread.start(step_Reading);
nerit 8:310f9e4eac7b 1783 #endif
nerit 3:a469bbd294b5 1784
nerit 8:310f9e4eac7b 1785 if (inProva==0) {
nerit 3:a469bbd294b5 1786 tractorSpeedRead.rise(&tractorReadSpeed);
francescopistone 44:13c0c33f1554 1787 tractorSpeedRead.fall(&tractorReadSpeed2);
nerit 3:a469bbd294b5 1788 #if !defined(speedMaster)
nerit 3:a469bbd294b5 1789 quinconceIn.rise(&quincTrigon);
nerit 3:a469bbd294b5 1790 quinconceIn.fall(&quincTrigof);
nerit 3:a469bbd294b5 1791 #endif
nerit 3:a469bbd294b5 1792 #if defined(speedMaster)
nerit 3:a469bbd294b5 1793 tftUpdate.attach(&videoUpdate,0.50f);
nerit 3:a469bbd294b5 1794 #endif
nerit 3:a469bbd294b5 1795 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 1796 dcSetting();
nerit 3:a469bbd294b5 1797 #if defined(seedSensor)
nerit 3:a469bbd294b5 1798 seedCheck.fall(&seedSensorTask);
nerit 3:a469bbd294b5 1799 #endif
nerit 8:310f9e4eac7b 1800 } else {
nerit 8:310f9e4eac7b 1801 tftUpdate.attach(&videoUpdate,0.125f);
nerit 8:310f9e4eac7b 1802 }
nerit 8:310f9e4eac7b 1803
nerit 3:a469bbd294b5 1804 aggiornaParametri();
nerit 3:a469bbd294b5 1805
nerit 3:a469bbd294b5 1806 SDmotorPWM.period_us(periodoSD); // frequency 1KHz pilotaggio motore DC
nerit 3:a469bbd294b5 1807 SDmotorPWM.write(1.0f); // duty cycle = stop
nerit 3:a469bbd294b5 1808 TBmotorDirecti=TBforward; // tb motor direction set
nerit 3:a469bbd294b5 1809
nerit 3:a469bbd294b5 1810 #if defined(provaStepper)
nerit 3:a469bbd294b5 1811 TBmotorRst=1;
nerit 3:a469bbd294b5 1812 TBmotorDirecti=TBforward;
nerit 3:a469bbd294b5 1813 // definire il pin di clock che è PB_3
nerit 9:7f02256f6e8f 1814 #if defined(Zucca)
nerit 17:9629eb019892 1815 motor->run(StepperMotor::BWD,100.0f);
nerit 10:9e70619e97ab 1816 //motor->step_clock_mode_enable(StepperMotor::FWD);
nerit 9:7f02256f6e8f 1817 #else
nerit 18:7c978f69cc51 1818 motor->run(StepperMotor::BWD,100.0f);
nerit 10:9e70619e97ab 1819 //motor->step_clock_mode_enable(StepperMotor::BWD);
nerit 9:7f02256f6e8f 1820 #endif
nerit 3:a469bbd294b5 1821 // attiva l'out per il controllo dello stepper in stepClockMode
nerit 10:9e70619e97ab 1822 //DigitalOut TBmotorStepOut(PB_3); // PowerStep01 Step Input
nerit 6:e8c18f0f399a 1823 #if defined(pcSerial)
nerit 6:e8c18f0f399a 1824 #if defined(checkLoop)
nerit 6:e8c18f0f399a 1825 pc.printf("11f\n");
nerit 6:e8c18f0f399a 1826 #endif
nerit 6:e8c18f0f399a 1827 #endif
nerit 10:9e70619e97ab 1828 //TBticker.attach(&step_TBPulseOut,0.0005f); // clock time are seconds and attach seed motor stepper controls
nerit 18:7c978f69cc51 1829 TATicker.attach(&invertiLo,3.0f);
nerit 3:a469bbd294b5 1830 #else
nerit 35:3165f4c1c7bf 1831 #if !defined(oldStepperDriver)
nerit 35:3165f4c1c7bf 1832 // definire il pin di clock che è PB_3
nerit 35:3165f4c1c7bf 1833 motor->set_home();
nerit 35:3165f4c1c7bf 1834 motor->go_to(50);
nerit 35:3165f4c1c7bf 1835 motor->wait_while_active();
nerit 35:3165f4c1c7bf 1836 #if !defined(runner)
nerit 35:3165f4c1c7bf 1837 #if defined(Zucca)
nerit 35:3165f4c1c7bf 1838 motor->step_clock_mode_enable(StepperMotor::BWD);
nerit 35:3165f4c1c7bf 1839 #else
nerit 35:3165f4c1c7bf 1840 motor->step_clock_mode_enable(StepperMotor::FWD);
nerit 35:3165f4c1c7bf 1841 #endif
nerit 9:7f02256f6e8f 1842 #endif
nerit 35:3165f4c1c7bf 1843 #if defined(pcSerial)
nerit 35:3165f4c1c7bf 1844 #if defined(loStop)
nerit 35:3165f4c1c7bf 1845 pc.printf("A3\n");
nerit 35:3165f4c1c7bf 1846 #endif
nerit 6:e8c18f0f399a 1847 #endif
nerit 35:3165f4c1c7bf 1848 motor->soft_hiz();
nerit 35:3165f4c1c7bf 1849 // attiva l'out per il controllo dello stepper in stepClockMode
nerit 35:3165f4c1c7bf 1850 DigitalOut TBmotorStepOut(PB_3); // PowerStep01 Step Input
nerit 6:e8c18f0f399a 1851 #endif
nerit 3:a469bbd294b5 1852 #endif // end prova stepper
nerit 8:310f9e4eac7b 1853
nerit 8:310f9e4eac7b 1854 wd.Configure(2.0); //watchdog set at xx seconds
nerit 8:310f9e4eac7b 1855
nerit 8:310f9e4eac7b 1856
nerit 3:a469bbd294b5 1857 //**************************************************************************************************************
nerit 3:a469bbd294b5 1858 // MAIN LOOP
nerit 3:a469bbd294b5 1859 //**************************************************************************************************************
nerit 8:310f9e4eac7b 1860 while (true) {
nerit 6:e8c18f0f399a 1861 // ripetitore segnale di velocità. Il set a 1 è nella task ad interrupt
francescopistone 44:13c0c33f1554 1862 //if (tractorSpeedRead==0) {
francescopistone 44:13c0c33f1554 1863 // speedClock=0;
francescopistone 44:13c0c33f1554 1864 //}
nerit 31:92a9e183be8e 1865 if ((tractorSpeed_MtS_timed>0.0f)&&(oldTractStop==1)){
nerit 31:92a9e183be8e 1866 alarmDelay.start();
nerit 31:92a9e183be8e 1867 oldTractStop=0;
nerit 31:92a9e183be8e 1868 }
nerit 31:92a9e183be8e 1869 if (alarmDelay.read()>5){
nerit 31:92a9e183be8e 1870 alarmEnable=true;
nerit 31:92a9e183be8e 1871 }
nerit 31:92a9e183be8e 1872 if ((tractorSpeed_MtS_timed<=0.05f)){
nerit 31:92a9e183be8e 1873 oldTractStop=1;
nerit 31:92a9e183be8e 1874 alarmDelay.stop();
nerit 31:92a9e183be8e 1875 alarmDelay.reset();
nerit 31:92a9e183be8e 1876 alarmEnable=false;
nerit 31:92a9e183be8e 1877 }
nerit 8:310f9e4eac7b 1878
nerit 3:a469bbd294b5 1879 // inversione segnali ingressi
nerit 3:a469bbd294b5 1880 #if !defined(simulaBanco)
nerit 3:a469bbd294b5 1881 seedWheelZeroPinInput = !seedWheelZeroPinInputRev;
nerit 3:a469bbd294b5 1882 #else
nerit 8:310f9e4eac7b 1883 if ((prePosSD-500) >= SDsectorStep) {
nerit 3:a469bbd294b5 1884 seedWheelZeroPinInput=1;
nerit 3:a469bbd294b5 1885 }
nerit 8:310f9e4eac7b 1886 if ((prePosSD > 500)&&(prePosSD<580)) {
nerit 3:a469bbd294b5 1887 seedWheelZeroPinInput=0;
nerit 3:a469bbd294b5 1888 }
nerit 8:310f9e4eac7b 1889 #endif
nerit 25:4461bc76aaab 1890 if (drumSelect==true){
nerit 25:4461bc76aaab 1891 TBzeroPinInput = TBzeroPinInputRev;
nerit 25:4461bc76aaab 1892 }else{
nerit 25:4461bc76aaab 1893 TBzeroPinInput = !TBzeroPinInputRev;
nerit 25:4461bc76aaab 1894 }
nerit 3:a469bbd294b5 1895 // se quinconce attivo ed unita' master invia segnale di sincro
nerit 3:a469bbd294b5 1896 #if defined(speedMaster)
nerit 8:310f9e4eac7b 1897 if (seedWheelZeroPinInput==1) {
nerit 3:a469bbd294b5 1898 quinconceOut=0;
nerit 3:a469bbd294b5 1899 }
nerit 8:310f9e4eac7b 1900 if (((double)(prePosSD-500) >= (SDsectorStep/((double)quincSector)))&&(quinconceOut=1)) {
nerit 3:a469bbd294b5 1901 quinconceOut=1;
nerit 3:a469bbd294b5 1902 }
nerit 8:310f9e4eac7b 1903 if (quinconceActive==1) {
nerit 8:310f9e4eac7b 1904 if ((quinconceOut==1)&&(oldQuinconceOut==1)) {
nerit 3:a469bbd294b5 1905 posForQuinc=500;
nerit 3:a469bbd294b5 1906 oldQuinconceOut=0;
nerit 3:a469bbd294b5 1907 }
nerit 8:310f9e4eac7b 1908 if (((double)posForQuinc-500.0f)> (SDsectorStep-200)) {
nerit 3:a469bbd294b5 1909 oldQuinconceOut=1;
nerit 3:a469bbd294b5 1910 }
nerit 8:310f9e4eac7b 1911 }
nerit 3:a469bbd294b5 1912 #endif
nerit 3:a469bbd294b5 1913
nerit 3:a469bbd294b5 1914 // simulazione velocita
nerit 8:310f9e4eac7b 1915 if (enableSimula==1) {
nerit 3:a469bbd294b5 1916 double TMT = 0.0f;
nerit 8:310f9e4eac7b 1917 if (speedSimula > 0) {
nerit 3:a469bbd294b5 1918 TMT = (double)(speedSimula) * 100.0f /3600.0f;
nerit 3:a469bbd294b5 1919 pulseSpeedInterval = pulseDistance / TMT;
nerit 8:310f9e4eac7b 1920 } else {
nerit 3:a469bbd294b5 1921 pulseSpeedInterval = 10000.0f;
nerit 8:310f9e4eac7b 1922 }
nerit 8:310f9e4eac7b 1923 if (avviaSimula==1) {
nerit 8:310f9e4eac7b 1924 if(oldSimulaSpeed!=pulseSpeedInterval) {
nerit 3:a469bbd294b5 1925 spedSimclock.attach_us(&speedSimulationClock,pulseSpeedInterval);
nerit 3:a469bbd294b5 1926 oldSimulaSpeed=pulseSpeedInterval;
nerit 3:a469bbd294b5 1927 }
nerit 8:310f9e4eac7b 1928 } else {
nerit 3:a469bbd294b5 1929 oldSimulaSpeed=10000.0f;
nerit 3:a469bbd294b5 1930 spedSimclock.detach();
nerit 3:a469bbd294b5 1931 }
nerit 8:310f9e4eac7b 1932 } else {
nerit 3:a469bbd294b5 1933 spedSimclock.detach();
nerit 3:a469bbd294b5 1934 }
nerit 8:310f9e4eac7b 1935
nerit 3:a469bbd294b5 1936 //*******************************************************
nerit 3:a469bbd294b5 1937 // determina se sono in bassa velocità per il controllo di TB
nerit 8:310f9e4eac7b 1938 if (speedOfSeedWheel<=minSeedSpeed) {
nerit 8:310f9e4eac7b 1939 if (lowSpeedRequired==0) {
nerit 3:a469bbd294b5 1940 ritardaLowSpeed.reset();
nerit 3:a469bbd294b5 1941 ritardaLowSpeed.start();
nerit 3:a469bbd294b5 1942 }
nerit 3:a469bbd294b5 1943 lowSpeedRequired=1;
nerit 8:310f9e4eac7b 1944 } else {
nerit 8:310f9e4eac7b 1945 if (lowSpeedRequired==1) {
nerit 3:a469bbd294b5 1946 lowSpeedRequired=0;
nerit 3:a469bbd294b5 1947 ritardaLowSpeed.reset();
nerit 3:a469bbd294b5 1948 ritardaLowSpeed.stop();
nerit 3:a469bbd294b5 1949 }
nerit 3:a469bbd294b5 1950 }
nerit 3:a469bbd294b5 1951
nerit 8:310f9e4eac7b 1952 if (ritardaLowSpeed.read_ms()> 2000) {
nerit 3:a469bbd294b5 1953 lowSpeed=1;
nerit 8:310f9e4eac7b 1954 } else {
nerit 3:a469bbd294b5 1955 lowSpeed=0;
nerit 3:a469bbd294b5 1956 }
bcostm 2:35f13b7f3659 1957
nerit 3:a469bbd294b5 1958 //**************************************************************************************************************
nerit 3:a469bbd294b5 1959 //**************************************************************************************************************
nerit 3:a469bbd294b5 1960 // LOGICAL CONTROLS
nerit 3:a469bbd294b5 1961 //**************************************************************************************************************
nerit 3:a469bbd294b5 1962 //**************************************************************************************************************
nerit 8:310f9e4eac7b 1963
nerit 8:310f9e4eac7b 1964 if (inProva==0) {
nerit 20:4a400a4cc419 1965 // simulazione stepper
nerit 23:ccd253b36733 1966 if (simOk==1){
nerit 23:ccd253b36733 1967 if (simStepper==1){
nerit 23:ccd253b36733 1968 oldSimStepper=true;
nerit 39:6814c75dfa5c 1969 #if defined(runner)
nerit 39:6814c75dfa5c 1970 simStepSpeed= ((double)speedStepp*180.45f)/50.0f;
nerit 39:6814c75dfa5c 1971 #endif
nerit 39:6814c75dfa5c 1972 #if defined(oldStepperDriver)
nerit 39:6814c75dfa5c 1973 simStepSpeed= 1.0f/(((double)speedStepp/250.0f)/100.0f);
nerit 39:6814c75dfa5c 1974 #endif
nerit 23:ccd253b36733 1975 if (oldSimStepSpeed!=simStepSpeed){
nerit 35:3165f4c1c7bf 1976 #if defined(runner)
nerit 35:3165f4c1c7bf 1977 motor->run(StepperMotor::FWD,simStepSpeed);
nerit 35:3165f4c1c7bf 1978 #endif
nerit 35:3165f4c1c7bf 1979 #if defined(oldStepperDriver)
nerit 39:6814c75dfa5c 1980 //TBmotorRst=0;
nerit 39:6814c75dfa5c 1981 //TBticker.attach_us(&step_TBPulseOut,simStepSpeed/2.0f); // clock time are milliseconds and attach seed motor stepper controls
nerit 39:6814c75dfa5c 1982 attivaTb(simStepSpeed/2.0f);
nerit 35:3165f4c1c7bf 1983 #endif
nerit 23:ccd253b36733 1984 oldSimStepSpeed=simStepSpeed;
nerit 23:ccd253b36733 1985 }
nerit 23:ccd253b36733 1986 }else{
nerit 23:ccd253b36733 1987 if (oldSimStepper==true){
nerit 23:ccd253b36733 1988 oldSimStepper=false;
nerit 35:3165f4c1c7bf 1989 #if defined(runner)
nerit 35:3165f4c1c7bf 1990 motor->soft_hiz();
nerit 35:3165f4c1c7bf 1991 #endif
nerit 35:3165f4c1c7bf 1992 #if defined(oldStepperDriver)
nerit 39:6814c75dfa5c 1993 //TBticker.detach();
nerit 39:6814c75dfa5c 1994 //TBmotorRst=1;
nerit 39:6814c75dfa5c 1995 stopTb();
nerit 35:3165f4c1c7bf 1996 #endif
nerit 23:ccd253b36733 1997 oldSimStepSpeed=0.0f;
nerit 23:ccd253b36733 1998 }
nerit 23:ccd253b36733 1999 }
nerit 20:4a400a4cc419 2000 }else{
nerit 20:4a400a4cc419 2001 if (oldSimStepper==true){
nerit 20:4a400a4cc419 2002 oldSimStepper=false;
nerit 35:3165f4c1c7bf 2003 #if defined(runner)
nerit 35:3165f4c1c7bf 2004 motor->soft_hiz();
nerit 35:3165f4c1c7bf 2005 #endif
nerit 35:3165f4c1c7bf 2006 #if defined(oldStepperDriver)
nerit 39:6814c75dfa5c 2007 //TBticker.detach();
nerit 39:6814c75dfa5c 2008 //TBmotorRst=1;
nerit 39:6814c75dfa5c 2009 stopTb();
nerit 35:3165f4c1c7bf 2010 #endif
nerit 20:4a400a4cc419 2011 oldSimStepSpeed=0.0f;
nerit 20:4a400a4cc419 2012 }
nerit 20:4a400a4cc419 2013 }
nerit 8:310f9e4eac7b 2014 if ((startCycleSimulation==0)&&(enableSimula==0)) {
nerit 6:e8c18f0f399a 2015 runRequestBuf=1;//0;
nerit 8:310f9e4eac7b 2016 } else {
nerit 6:e8c18f0f399a 2017 runRequestBuf=1;//0;
nerit 6:e8c18f0f399a 2018 }
nerit 8:310f9e4eac7b 2019 if ((tractorSpeedRead==0)&&(speedFilter.read_ms()>3)) {
nerit 6:e8c18f0f399a 2020 oldTractorSpeedRead=0;
nerit 3:a469bbd294b5 2021 }
nerit 3:a469bbd294b5 2022 // ----------------------------------------
nerit 3:a469bbd294b5 2023 // filtra il segnale dei becchi per misurare il tempo di intervallo tra loro
nerit 3:a469bbd294b5 2024 // ----------------------------------------
nerit 8:310f9e4eac7b 2025 if ((seedWheelZeroPinInput==0)&&(oldSeedWheelZeroPinInput==1)) {
nerit 8:310f9e4eac7b 2026 if(seedFilter.read_ms()>=4) {
nerit 3:a469bbd294b5 2027 oldSeedWheelZeroPinInput=0;
nerit 3:a469bbd294b5 2028 SDzeroDebounced=0;
nerit 3:a469bbd294b5 2029 }
nerit 3:a469bbd294b5 2030 }
nerit 8:310f9e4eac7b 2031 if ((seedWheelZeroPinInput==1)&&(oldSeedWheelZeroPinInput==0)) {
nerit 3:a469bbd294b5 2032 timeIntraPick = (double)intraPickTimer.read_ms();
nerit 3:a469bbd294b5 2033 prePosSD=500; // preposizionamento SD
nerit 3:a469bbd294b5 2034 intraPickTimer.reset();
nerit 3:a469bbd294b5 2035 rotationTimeOut.reset();
nerit 8:310f9e4eac7b 2036 seedFilter.reset();
nerit 3:a469bbd294b5 2037 sincroTimer.reset();
nerit 3:a469bbd294b5 2038 oldSeedWheelZeroPinInput=1;
nerit 3:a469bbd294b5 2039 quincTime.reset();
nerit 3:a469bbd294b5 2040 quincTimeSD.reset();
nerit 3:a469bbd294b5 2041 SDzeroDebounced=1;
nerit 3:a469bbd294b5 2042 sincroQui=1;
nerit 3:a469bbd294b5 2043 SDwheelTimer.reset();
nerit 13:d1030d4e51a8 2044 dcEncoderCnt=0;
nerit 3:a469bbd294b5 2045 #if defined(speedMaster)
nerit 8:310f9e4eac7b 2046 if (quinconceActive==0) {
nerit 3:a469bbd294b5 2047 posForQuinc=500;
nerit 3:a469bbd294b5 2048 }
nerit 17:9629eb019892 2049 if (checkSDrotation==0) {
nerit 17:9629eb019892 2050 checkSDrotation=1;
nerit 17:9629eb019892 2051 SDwheelTimer.start();
nerit 17:9629eb019892 2052 }
nerit 3:a469bbd294b5 2053 #endif
nerit 8:310f9e4eac7b 2054 if (quincCnt<10) {
nerit 3:a469bbd294b5 2055 quincCnt++;
nerit 3:a469bbd294b5 2056 }
nerit 8:310f9e4eac7b 2057 if ((aspettaStart==0)&&(lowSpeed==1)) {
nerit 3:a469bbd294b5 2058 beccoPronto=1;
nerit 3:a469bbd294b5 2059 }
nerit 3:a469bbd294b5 2060 lockStart=0;
nerit 3:a469bbd294b5 2061 double fase1=0.0f;
nerit 3:a469bbd294b5 2062 forzaFase=0;
nerit 3:a469bbd294b5 2063 double limite=fixedStepGiroSD/pickNumber;
nerit 8:310f9e4eac7b 2064 if (tamburoStandard==0) {
nerit 8:310f9e4eac7b 2065 fase1=TBdeltaStep;
nerit 8:310f9e4eac7b 2066 } else {
nerit 8:310f9e4eac7b 2067 if(speedForCorrection >= speedOfSeedWheel) {
nerit 3:a469bbd294b5 2068 fase1=TBdeltaStep;
nerit 8:310f9e4eac7b 2069 } else {
nerit 3:a469bbd294b5 2070 fase1=(TBdeltaStep)-(((speedOfSeedWheel)/maxWorkSpeed)*(TBfaseStep));
nerit 3:a469bbd294b5 2071 }
nerit 8:310f9e4eac7b 2072 if (fase1 > limite) {
nerit 3:a469bbd294b5 2073 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 2074 }
nerit 8:310f9e4eac7b 2075 if ((fase1 > (limite -20.0f))&&(fase1<(limite +5.0f))) {
nerit 3:a469bbd294b5 2076 fase1 = limite -20.0f; // se la fase è molto vicina al limite interpone uno spazio minimo di lavoro del sincronismo
nerit 3:a469bbd294b5 2077 forzaFase=1;
nerit 3:a469bbd294b5 2078 }
nerit 3:a469bbd294b5 2079 trigRepos=1;
nerit 3:a469bbd294b5 2080 }
nerit 3:a469bbd294b5 2081 fase = (uint32_t)fase1+500;
nerit 3:a469bbd294b5 2082 #if defined(pcSerial)
nerit 3:a469bbd294b5 2083 #if defined(inCorre)
nerit 3:a469bbd294b5 2084 pc.printf(" limite %f", limite);
nerit 3:a469bbd294b5 2085 pc.printf(" delta %f", TBdeltaStep);
nerit 3:a469bbd294b5 2086 pc.printf(" faseStep %f", TBfaseStep);
nerit 3:a469bbd294b5 2087 pc.printf(" fase %d",fase);
nerit 3:a469bbd294b5 2088 pc.printf(" forzaFase %d",forzaFase);
nerit 3:a469bbd294b5 2089 pc.printf(" trigRepos %d", trigRepos);
nerit 3:a469bbd294b5 2090 pc.printf(" ActualSD: %d",SDactualPosition);
nerit 3:a469bbd294b5 2091 pc.printf(" SpeedWheel: %f",speedOfSeedWheel);
nerit 3:a469bbd294b5 2092 pc.printf(" SPEED: %f \n",tractorSpeed_MtS_timed);
nerit 3:a469bbd294b5 2093 #endif
nerit 3:a469bbd294b5 2094 #endif
nerit 19:231b8931cabc 2095 if (tractorSpeed_MtS_timed>0.2f){
nerit 17:9629eb019892 2096 if (timeIntraPick >= (memoIntraPick*2)) {
nerit 31:92a9e183be8e 2097 if ((aspettaStart==0)&&(alarmEnable==true)) {
nerit 17:9629eb019892 2098 if (firstStart==0) {
nerit 39:6814c75dfa5c 2099 #if defined(pcSerial)
nerit 39:6814c75dfa5c 2100 #if defined(tempoTb)
nerit 39:6814c75dfa5c 2101 pc.printf("intra: %f memo: %f\n" , timeIntraPick, memoIntraPick);
nerit 39:6814c75dfa5c 2102 #endif
nerit 39:6814c75dfa5c 2103 #endif
nerit 17:9629eb019892 2104 all_pickSignal=1;
nerit 17:9629eb019892 2105 }
nerit 3:a469bbd294b5 2106 }
nerit 3:a469bbd294b5 2107 }
nerit 3:a469bbd294b5 2108 }
nerit 3:a469bbd294b5 2109 memoIntraPick = timeIntraPick;
nerit 8:310f9e4eac7b 2110 if ((speedFromPick==1)&&(encoder==false)) {
nerit 3:a469bbd294b5 2111 speedOfSeedWheel=((seedPerimeter / pickNumber)/timeIntraPick)*1000.0f;
nerit 10:9e70619e97ab 2112 #if defined(pcSerial)
nerit 10:9e70619e97ab 2113 #if defined(Qnca)
nerit 10:9e70619e97ab 2114 pc.printf("perim: %f pickN: %f sped: %f\n", seedPerimeter, pickNumber,speedOfSeedWheel);
nerit 10:9e70619e97ab 2115 #endif
nerit 3:a469bbd294b5 2116 #endif
nerit 3:a469bbd294b5 2117 }
nerit 8:310f9e4eac7b 2118 if (encoder==false) {
nerit 3:a469bbd294b5 2119 pulseRised2=1;
nerit 3:a469bbd294b5 2120 }
nerit 3:a469bbd294b5 2121 #if defined(speedMaster)
nerit 8:310f9e4eac7b 2122 if ((tractorSpeed_MtS_timed==0.0f)) {
nerit 8:310f9e4eac7b 2123 if ((firstStart==0)&&(simOk==0)&&(enableSimula==0)) {
nerit 10:9e70619e97ab 2124 cntSpeedError++;
nerit 18:7c978f69cc51 2125 if (cntSpeedError >= 5){
nerit 10:9e70619e97ab 2126 all_noSpeedSen=1;
nerit 10:9e70619e97ab 2127 }
nerit 3:a469bbd294b5 2128 }
nerit 3:a469bbd294b5 2129 }
nerit 31:92a9e183be8e 2130 if ((tractorSpeed_MtS_timed>0.2f)&&(alarmEnable==true)){
nerit 18:7c978f69cc51 2131 double oldLastPr = (double)oldLastPulseRead*2.8f;
nerit 17:9629eb019892 2132 if((double)speedTimeOut.read_us()> (oldLastPr)) {
nerit 17:9629eb019892 2133 if ((firstStart==0)&&(simOk==0)&&(enableSimula==0)) {
nerit 17:9629eb019892 2134 all_speedError =1;
nerit 17:9629eb019892 2135 }
nerit 3:a469bbd294b5 2136 }
nerit 3:a469bbd294b5 2137 }
nerit 3:a469bbd294b5 2138 #endif
nerit 3:a469bbd294b5 2139 //*******************************************
nerit 3:a469bbd294b5 2140 // esegue calcolo clock per la generazione della posizione teorica
nerit 3:a469bbd294b5 2141 // la realtà in base al segnale di presenza del becco
nerit 8:310f9e4eac7b 2142 if (speedOfSeedWheel < 0.002f) {
nerit 10:9e70619e97ab 2143 #if defined(pcSerial)
nerit 10:9e70619e97ab 2144 #if defined(checkLoopb)
nerit 10:9e70619e97ab 2145 pc.printf("forza\n");
nerit 10:9e70619e97ab 2146 #endif
nerit 8:310f9e4eac7b 2147 #endif
nerit 6:e8c18f0f399a 2148 speedOfSeedWheel=0.001f;
nerit 6:e8c18f0f399a 2149 }
nerit 8:310f9e4eac7b 2150 aggioVelocita();
nerit 3:a469bbd294b5 2151 }
nerit 3:a469bbd294b5 2152 // ----------------------------------------
nerit 8:310f9e4eac7b 2153 // check SD fase
nerit 8:310f9e4eac7b 2154 if ((prePosSD >= fase)||(forzaFase==1)) { //&&(prePosSD < (fase +30))){
nerit 3:a469bbd294b5 2155 forzaFase=0;
nerit 8:310f9e4eac7b 2156 if (trigRepos==1) {
nerit 3:a469bbd294b5 2157 SDactualPosition=0;
nerit 8:310f9e4eac7b 2158 if ((countCicli<30)&&(trigCicli==0)) {
nerit 6:e8c18f0f399a 2159 countCicli++;
nerit 6:e8c18f0f399a 2160 trigCicli=1;
nerit 6:e8c18f0f399a 2161 }
nerit 8:310f9e4eac7b 2162 if(countCicli>=cicliAspettaStart) {
nerit 6:e8c18f0f399a 2163 aspettaStart=0;
nerit 6:e8c18f0f399a 2164 }
nerit 8:310f9e4eac7b 2165 if ((lowSpeed==0)&&(aspettaStart==0)&&(lockStart==0)) {
nerit 6:e8c18f0f399a 2166 syncroCheck=1;
nerit 6:e8c18f0f399a 2167 beccoPronto=0;
nerit 6:e8c18f0f399a 2168 }
nerit 8:310f9e4eac7b 2169 if (trigTB==0) {
nerit 3:a469bbd294b5 2170 inhibit=1;
nerit 3:a469bbd294b5 2171 trigSD=1;
nerit 8:310f9e4eac7b 2172 } else {
nerit 3:a469bbd294b5 2173 inhibit=0;
nerit 3:a469bbd294b5 2174 trigTB=0;
nerit 3:a469bbd294b5 2175 trigSD=0;
nerit 3:a469bbd294b5 2176 }
nerit 3:a469bbd294b5 2177 trigRepos=0;
nerit 3:a469bbd294b5 2178 }
nerit 8:310f9e4eac7b 2179 } else {
nerit 3:a469bbd294b5 2180 trigCicli=0;
nerit 3:a469bbd294b5 2181 }
nerit 8:310f9e4eac7b 2182 // ----------------------------------------
nerit 3:a469bbd294b5 2183 // filtra il segnale del tamburo per lo stop in fase del tamburo stesso
nerit 8:310f9e4eac7b 2184 if (TBzeroPinInput==0) {
nerit 8:310f9e4eac7b 2185 if (TBfilter.read_ms()>=2) {
nerit 8:310f9e4eac7b 2186 oldTBzeroPinInput=0;
nerit 8:310f9e4eac7b 2187 }
nerit 8:310f9e4eac7b 2188 }
nerit 20:4a400a4cc419 2189 if (((TBzeroPinInput==1)&&(oldTBzeroPinInput==0))&&(simStepper==0)) {
nerit 3:a469bbd294b5 2190 oldTBzeroPinInput=1;
nerit 8:310f9e4eac7b 2191 if (loadDaCanInCorso==0) {
nerit 3:a469bbd294b5 2192 stopCicloTB=1;
nerit 3:a469bbd294b5 2193 startCicloTB=0;
nerit 3:a469bbd294b5 2194 }
nerit 3:a469bbd294b5 2195 TBfilter.reset();
nerit 3:a469bbd294b5 2196 TBzeroCyclePulse=1;
nerit 8:310f9e4eac7b 2197 #if defined(runner)
nerit 8:310f9e4eac7b 2198 legPos.detach();
nerit 8:310f9e4eac7b 2199 TBoldPosition= (uint32_t) motor->get_position();
nerit 8:310f9e4eac7b 2200 legPos.attach(&step_Reading,0.002f);
nerit 8:310f9e4eac7b 2201 #if defined(pcSerial)
nerit 8:310f9e4eac7b 2202 #if defined(TBperS)
nerit 8:310f9e4eac7b 2203 pc.printf("TBoldPos: %d\n",TBoldPosition);
nerit 8:310f9e4eac7b 2204 #endif
nerit 8:310f9e4eac7b 2205 #endif
nerit 37:0a225902cf48 2206 #if defined(perProva)
nerit 37:0a225902cf48 2207 pc.printf(" ildato %d Actual %d\n", ildato, TBactualPosition);
nerit 37:0a225902cf48 2208 #endif
nerit 8:310f9e4eac7b 2209 #else
nerit 8:310f9e4eac7b 2210 TBactualPosition=0;
nerit 8:310f9e4eac7b 2211 #endif
nerit 8:310f9e4eac7b 2212 if (cntTbError>0) {
nerit 3:a469bbd294b5 2213 cntCellsCorrect++;
nerit 3:a469bbd294b5 2214 }
nerit 19:231b8931cabc 2215 if (cntCellsCorrect>0) {
nerit 3:a469bbd294b5 2216 cntTbError=0;
nerit 3:a469bbd294b5 2217 cntCellsCorrect=0;
nerit 3:a469bbd294b5 2218 }
nerit 3:a469bbd294b5 2219 // conteggio celle erogate
nerit 8:310f9e4eac7b 2220 if (cellsCounterLow < 0xFF) {
nerit 3:a469bbd294b5 2221 cellsCounterLow++;
nerit 8:310f9e4eac7b 2222 } else {
nerit 3:a469bbd294b5 2223 cellsCounterHig++;
nerit 3:a469bbd294b5 2224 cellsCounterLow=0;
nerit 3:a469bbd294b5 2225 }
nerit 3:a469bbd294b5 2226 // ciclo conteggio celle per carico manuale
nerit 8:310f9e4eac7b 2227 if (loadDaCanInCorso==1) {
nerit 3:a469bbd294b5 2228 cntCellsForLoad++;
nerit 27:3d382b6e60ba 2229 double cellsToCount = ceil(((cellsNumber/360.0)*180.0f)+1.0f);
nerit 26:77955c5d16a3 2230 if (cntCellsForLoad >= cellsToCount) {
nerit 8:310f9e4eac7b 2231 stopCicloTB=1;
nerit 3:a469bbd294b5 2232 cntCellsForLoad=0;
nerit 3:a469bbd294b5 2233 }
nerit 8:310f9e4eac7b 2234 } else {
nerit 3:a469bbd294b5 2235 cntCellsForLoad=0;
nerit 3:a469bbd294b5 2236 }
nerit 3:a469bbd294b5 2237 // inibizione controllo di sincro per fuori fase
nerit 8:310f9e4eac7b 2238 if (trigSD==0) {
nerit 3:a469bbd294b5 2239 inhibit=1;
nerit 3:a469bbd294b5 2240 trigTB=1;
nerit 8:310f9e4eac7b 2241 } else {
nerit 3:a469bbd294b5 2242 inhibit=0;
nerit 3:a469bbd294b5 2243 trigTB=0;
nerit 3:a469bbd294b5 2244 trigSD=0;
nerit 3:a469bbd294b5 2245 }
nerit 3:a469bbd294b5 2246 // conta le celle indietro per sbloccare il tamburo
nerit 8:310f9e4eac7b 2247 if ((TBmotorDirecti==TBreverse)&&(erroreTamburo==1)) {
nerit 3:a469bbd294b5 2248 cntCellsForReload++;
nerit 8:310f9e4eac7b 2249 if (cntCellsForReload >= cellsCountSet) {
nerit 3:a469bbd294b5 2250 TBmotorDirecti=TBforward; // rotazione normale
nerit 21:3e6576c2e126 2251 if (tractorSpeed_MtS_timed>0.0f){
nerit 21:3e6576c2e126 2252 #if defined(runner)
nerit 21:3e6576c2e126 2253 #if defined(Zucca)
nerit 21:3e6576c2e126 2254 motor->run(StepperMotor::BWD);
nerit 21:3e6576c2e126 2255 #else
nerit 21:3e6576c2e126 2256 motor->run(StepperMotor::FWD);
nerit 21:3e6576c2e126 2257 #endif
nerit 9:7f02256f6e8f 2258 #else
nerit 35:3165f4c1c7bf 2259 #if !defined(oldStepperDriver)
nerit 35:3165f4c1c7bf 2260 #if defined(Zucca)
nerit 35:3165f4c1c7bf 2261 motor->step_clock_mode_enable(StepperMotor::FWD);
nerit 35:3165f4c1c7bf 2262 #else
nerit 35:3165f4c1c7bf 2263 motor->step_clock_mode_enable(StepperMotor::FWD);
nerit 35:3165f4c1c7bf 2264 #endif
nerit 21:3e6576c2e126 2265 #endif
nerit 9:7f02256f6e8f 2266 #endif
nerit 21:3e6576c2e126 2267 }
nerit 3:a469bbd294b5 2268 erroreTamburo=0;
nerit 3:a469bbd294b5 2269 cntCellsCorrect=0;
nerit 8:310f9e4eac7b 2270 }
nerit 3:a469bbd294b5 2271 }
nerit 3:a469bbd294b5 2272 #if defined(seedSensor)
nerit 3:a469bbd294b5 2273 resetDelay();
nerit 3:a469bbd294b5 2274 delaySeedCheck.start();
nerit 3:a469bbd294b5 2275 #endif
nerit 3:a469bbd294b5 2276 }
nerit 20:4a400a4cc419 2277 if (((double)TBactualPosition > ((TBgiroStep/cellsNumber)*1.8f)&&(erroreTamburo==0))&&(simStepper==0)) {
nerit 31:92a9e183be8e 2278 if ((firstStart==0)&&(alarmEnable==true)) {
nerit 18:7c978f69cc51 2279 if (tractorSpeed_MtS_timed>0.4f){
nerit 17:9629eb019892 2280 if (cntTbError>5) {
nerit 17:9629eb019892 2281 all_cellSignal=1;
nerit 17:9629eb019892 2282 #if defined(seedSensor)
nerit 17:9629eb019892 2283 resetDelay();
nerit 17:9629eb019892 2284 #endif
nerit 17:9629eb019892 2285 }
nerit 3:a469bbd294b5 2286 }
nerit 3:a469bbd294b5 2287 }
nerit 8:310f9e4eac7b 2288 if (erroreTamburo==0) {
nerit 17:9629eb019892 2289 if (cellsCountSet>1){
nerit 17:9629eb019892 2290 erroreTamburo=1;
nerit 17:9629eb019892 2291 TBmotorDirecti=TBreverse; // rotazione inversa
nerit 21:3e6576c2e126 2292 if (tractorSpeed_MtS_timed>0.0f){
nerit 21:3e6576c2e126 2293 #if defined(runner)
nerit 21:3e6576c2e126 2294 #if defined(Zucca)
nerit 21:3e6576c2e126 2295 motor->run(StepperMotor::FWD);
nerit 21:3e6576c2e126 2296 #else
nerit 21:3e6576c2e126 2297 motor->run(StepperMotor::BWD);
nerit 21:3e6576c2e126 2298 #endif
nerit 17:9629eb019892 2299 #else
nerit 35:3165f4c1c7bf 2300 #if !defined(oldStepperDriver)
nerit 35:3165f4c1c7bf 2301 #if defined(Zucca)
nerit 35:3165f4c1c7bf 2302 motor->step_clock_mode_enable(StepperMotor::FWD);
nerit 35:3165f4c1c7bf 2303 #else
nerit 35:3165f4c1c7bf 2304 motor->step_clock_mode_enable(StepperMotor::BWD);
nerit 35:3165f4c1c7bf 2305 #endif
nerit 21:3e6576c2e126 2306 #endif
nerit 17:9629eb019892 2307 #endif
nerit 21:3e6576c2e126 2308 }
nerit 17:9629eb019892 2309 }
nerit 3:a469bbd294b5 2310 cntCellsForReload=0;
nerit 17:9629eb019892 2311 if (tractorSpeed_MtS_timed>0.0f){
nerit 17:9629eb019892 2312 cntTbError++;
nerit 30:32e770e91998 2313 //aggiornati();
nerit 17:9629eb019892 2314 }else{
nerit 17:9629eb019892 2315 cntTbError=0;
nerit 17:9629eb019892 2316 }
nerit 3:a469bbd294b5 2317 #if defined(seedSensor)
nerit 3:a469bbd294b5 2318 resetDelay();
nerit 3:a469bbd294b5 2319 #endif
nerit 3:a469bbd294b5 2320 }
nerit 3:a469bbd294b5 2321 }
nerit 31:92a9e183be8e 2322 if ((tractorSpeed_MtS_timed>0.0f)&&(alarmEnable==true)){
nerit 17:9629eb019892 2323 if (((double)TBactualPosition > ((TBgiroStep/cellsNumber)*3.0f))||(cntTbError>5)) {
nerit 17:9629eb019892 2324 if (firstStart==0) {
nerit 39:6814c75dfa5c 2325 #if defined(pcSerial)
nerit 39:6814c75dfa5c 2326 #if defined(posit)
nerit 39:6814c75dfa5c 2327 pc.printf("actual: %d giro: %f cells: %f cnt: %d", TBactualPosition,TBgiroStep, cellsNumber,cntTbError);
nerit 39:6814c75dfa5c 2328 #endif
nerit 39:6814c75dfa5c 2329 #endif
nerit 17:9629eb019892 2330 all_noStepRota=1;
nerit 17:9629eb019892 2331 #if defined(seedSensor)
nerit 17:9629eb019892 2332 resetDelay();
nerit 17:9629eb019892 2333 #endif
nerit 17:9629eb019892 2334 }
nerit 17:9629eb019892 2335 cntTbError=0;
nerit 3:a469bbd294b5 2336 }
nerit 3:a469bbd294b5 2337 }
nerit 8:310f9e4eac7b 2338 // ----------------------------------------
nerit 3:a469bbd294b5 2339 // read and manage joystick
nerit 20:4a400a4cc419 2340 if ((loadDaCan==1)&(simStepper==0)) {
nerit 8:310f9e4eac7b 2341 if (tractorSpeed_MtS_timed==0.0f) {
nerit 8:310f9e4eac7b 2342 #if defined(pcSerial)
nerit 8:310f9e4eac7b 2343 #if defined(checkLoop)
nerit 8:310f9e4eac7b 2344 pc.printf("daCAN\n");
nerit 6:e8c18f0f399a 2345 #endif
nerit 8:310f9e4eac7b 2346 #endif
nerit 6:e8c18f0f399a 2347 ciclaTB();
nerit 8:310f9e4eac7b 2348 }
nerit 3:a469bbd294b5 2349 }
nerit 8:310f9e4eac7b 2350
nerit 8:310f9e4eac7b 2351 //***************************************************************************************************
nerit 8:310f9e4eac7b 2352 // pulseRised define the event of speed wheel pulse occurs
nerit 8:310f9e4eac7b 2353 //
nerit 3:a469bbd294b5 2354 //double maxInterval = pulseDistance/minWorkSpeed;
nerit 3:a469bbd294b5 2355 //double minIntervalPulse = pulseDistance/maxWorkSpeed;
nerit 8:310f9e4eac7b 2356 if (pulseRised==1) {
nerit 12:b0fc1d313813 2357 if (enableSpeed<10) {enableSpeed++;}
nerit 3:a469bbd294b5 2358 pulseRised=0;
nerit 3:a469bbd294b5 2359 pulseRised1=1;
nerit 3:a469bbd294b5 2360 speedMediaCalc();
nerit 3:a469bbd294b5 2361 // calcola velocità trattore
nerit 8:310f9e4eac7b 2362 if(enableSpeed>=2) {
nerit 8:310f9e4eac7b 2363 if ((pulseSpeedInterval>=0.0f)) { //minIntervalPulse)&&(pulseSpeedInterval<maxInterval)){
nerit 32:cfc3d7420fa5 2364 // todo: separare il controllo su speedfrommaster per attivarlo solo sullo slave
nerit 8:310f9e4eac7b 2365 if((quincCnt<3)||(speedFromMaster==0.0f)||(enableSimula==1)) {
nerit 3:a469bbd294b5 2366 tractorSpeed_MtS_timed = ((pulseDistance / pulseSpeedInterval)); // tractor speed (unit= Mt/S) from pulse time interval
nerit 13:d1030d4e51a8 2367 if (delaySpeedCheck.read()>=5.0f){
nerit 13:d1030d4e51a8 2368 if ((tractorSpeed_MtS_timed<=(oldLocalTractorSpeed+hiLimitSpeed))&&(tractorSpeed_MtS_timed>=(oldLocalTractorSpeed-loLimitSpeed))){
nerit 13:d1030d4e51a8 2369 tractorSpeed_MtS_timed=oldLocalTractorSpeed;
nerit 13:d1030d4e51a8 2370 }
nerit 13:d1030d4e51a8 2371 }
nerit 10:9e70619e97ab 2372 oldLocalTractorSpeed = tractorSpeed_MtS_timed;
nerit 3:a469bbd294b5 2373 }
nerit 17:9629eb019892 2374 /*if (checkSDrotation==0) {
nerit 3:a469bbd294b5 2375 checkSDrotation=1;
nerit 3:a469bbd294b5 2376 SDwheelTimer.start();
nerit 17:9629eb019892 2377 }*/
nerit 3:a469bbd294b5 2378 }
nerit 3:a469bbd294b5 2379 }
nerit 3:a469bbd294b5 2380 speedTimeOut.reset();
nerit 8:310f9e4eac7b 2381 } else {
nerit 3:a469bbd294b5 2382 double oldLastPr = (double)oldLastPulseRead*1.7f;
nerit 8:310f9e4eac7b 2383 if((double)speedTimeOut.read_us()> (oldLastPr)) {
nerit 3:a469bbd294b5 2384 tractorSpeed_MtS_timed = 0.0f;
nerit 10:9e70619e97ab 2385 oldLocalTractorSpeed=0.0f;
nerit 10:9e70619e97ab 2386 #if defined(seedSensor)
nerit 10:9e70619e97ab 2387 resetDelay();
nerit 10:9e70619e97ab 2388 #endif
nerit 3:a469bbd294b5 2389 pntMedia=0;
nerit 3:a469bbd294b5 2390 speedTimeOut.reset();
nerit 3:a469bbd294b5 2391 enableSpeed=0;
nerit 3:a469bbd294b5 2392 quincCnt=0;
nerit 3:a469bbd294b5 2393 }
nerit 3:a469bbd294b5 2394 }
nerit 8:310f9e4eac7b 2395
nerit 10:9e70619e97ab 2396 #if defined(seedSensor)
nerit 10:9e70619e97ab 2397 if (seedSensorEnable==true) {
nerit 10:9e70619e97ab 2398 if (delaySeedCheck.read_ms()>100) {
nerit 10:9e70619e97ab 2399 if (seedSee==0) {
nerit 10:9e70619e97ab 2400 all_noSeedOnCe=1;
nerit 10:9e70619e97ab 2401 }
nerit 10:9e70619e97ab 2402 resetDelay();
nerit 3:a469bbd294b5 2403 }
nerit 3:a469bbd294b5 2404 }
nerit 10:9e70619e97ab 2405 #endif
nerit 3:a469bbd294b5 2406 // esegue il controllo di velocità minima
nerit 3:a469bbd294b5 2407 /*if ((double)speedTimer.read_ms()>=maxInterval){
nerit 3:a469bbd294b5 2408 tractorSpeed_MtS_timed = 0.0f;
nerit 3:a469bbd294b5 2409 enableSpeed=0;
nerit 3:a469bbd294b5 2410 }*/
nerit 3:a469bbd294b5 2411 // esegue il controllo di velocità massima
nerit 3:a469bbd294b5 2412 /*if ((double)speedTimer.read_ms()<=minIntervalPulse){
nerit 3:a469bbd294b5 2413 tractorSpeed_MtS_timed = 4.5f;
nerit 3:a469bbd294b5 2414 }*/
nerit 8:310f9e4eac7b 2415 //***************************************************************************************************************
nerit 8:310f9e4eac7b 2416 // cycle logic control section
nerit 8:310f9e4eac7b 2417 //***************************************************************************************************************
nerit 8:310f9e4eac7b 2418 if (enableSimula==1) {
nerit 8:310f9e4eac7b 2419 if(simOk==0) {
nerit 8:310f9e4eac7b 2420 tractorSpeed_MtS_timed=0.0f;
nerit 10:9e70619e97ab 2421 oldLocalTractorSpeed=0.0f;
nerit 8:310f9e4eac7b 2422 }
nerit 8:310f9e4eac7b 2423 }
nerit 20:4a400a4cc419 2424 if ((tractorSpeed_MtS_timed>0.01f)&&(simStepper==0)) {
nerit 8:310f9e4eac7b 2425 #if defined(pcSerial)
nerit 8:310f9e4eac7b 2426 #if defined(Qncc)
nerit 8:310f9e4eac7b 2427 pc.printf("TsP: %f SpW: %f InPic: %f EPerc: %f Duty:%f \n",tractorSpeed_MtS_timed,speedOfSeedWheel,timeIntraPick, errorePercentuale, dcActualDuty);
nerit 8:310f9e4eac7b 2428
nerit 8:310f9e4eac7b 2429 #endif
nerit 8:310f9e4eac7b 2430 #endif
nerit 8:310f9e4eac7b 2431 cycleStopRequest=1;
nerit 8:310f9e4eac7b 2432 // calcola il tempo teorico di passaggio becchi sulla base della velocità del trattore
nerit 8:310f9e4eac7b 2433 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 2434 if (encoder==false) {
nerit 8:310f9e4eac7b 2435 if (speedFromPick==1) {
nerit 8:310f9e4eac7b 2436 tempoTraBecchi_mS = (tempoGiroSD / pickNumber)*1000.0f; // tempo tra due impulsi dai becchi in millisecondi ( circa 157mS a 4,5Km/h)
nerit 8:310f9e4eac7b 2437 } else {
nerit 8:310f9e4eac7b 2438 tempoTraBecchi_mS = (tempoGiroSD / 25.0f)*1000.0f; // tempo tra due impulsi dai becchi in millisecondi ( circa 157mS a 4,5Km/h)
nerit 8:310f9e4eac7b 2439 }
nerit 8:310f9e4eac7b 2440 } else {
nerit 17:9629eb019892 2441 tempoTraBecchi_mS = (tempoGiroSD / (SDreductionRatio*dcPulseTurn))*1000.0f; // tempo tra due impulsi dai becchi in millisecondi ( circa 157mS a 4,5Km/h)
nerit 5:2a3a64b52f54 2442 #if defined(pcSerial)
nerit 8:310f9e4eac7b 2443 #if defined(Qnce)
nerit 8:310f9e4eac7b 2444 pc.printf("tempoGiroSD: %f SDreductionRatio: %f tempoBecchi:%f\n",tempoGiroSD,SDreductionRatio,tempoTraBecchi_mS);
nerit 5:2a3a64b52f54 2445 #endif
nerit 5:2a3a64b52f54 2446 #endif
nerit 8:310f9e4eac7b 2447 #if !defined(speedMaster)
nerit 10:9e70619e97ab 2448 double tempoGiroSDfomMaster = seedPerimeter / speedFromMaster;
nerit 10:9e70619e97ab 2449 tempoBecchiPerQuinc = (tempoGiroSDfomMaster / pickNumber)*1000.0f;
nerit 8:310f9e4eac7b 2450 #endif
nerit 8:310f9e4eac7b 2451 }
nerit 8:310f9e4eac7b 2452 //*******************************************
nerit 8:310f9e4eac7b 2453 // segue calcolo duty cycle comando motore DC per allinearsi con la velocità del trattore
nerit 8:310f9e4eac7b 2454 double dutyTeorico = 0.00;
nerit 12:b0fc1d313813 2455 double deltaV=0.0f;
nerit 12:b0fc1d313813 2456 double deltaD=0.0f;
nerit 12:b0fc1d313813 2457 double setV=0.0f;
nerit 12:b0fc1d313813 2458 double teoriaC=0.0f;
nerit 14:e2b5efa06c41 2459 double speedCorrectionMachine=0.0f;
nerit 17:9629eb019892 2460 //#if defined(speedMaster)
nerit 14:e2b5efa06c41 2461 speedCorrectionMachine=(seedWheelDiameter/(seedWheelDiameter-(deepOfSeed*2.0f)))*tractorSpeed_MtS_timed;
nerit 17:9629eb019892 2462 //#else
nerit 17:9629eb019892 2463 // speedCorrectionMachine=tractorSpeed_MtS_timed;
nerit 17:9629eb019892 2464 //#endif
nerit 14:e2b5efa06c41 2465 //if ((tractorSpeed_MtS_timed>0.0)&&(tractorSpeed_MtS_timed<tabSpeed[0])) {
nerit 17:9629eb019892 2466 if ((speedCorrectionMachine>0.0f)&&(speedCorrectionMachine<tabSpeed[0])) {
nerit 8:310f9e4eac7b 2467 dutyTeorico = tabComan[0];
nerit 8:310f9e4eac7b 2468 }
nerit 38:79af1c65dd6f 2469 #if defined(oldStepperDriver)
nerit 39:6814c75dfa5c 2470 for (int ii = 0; ii<tabeling; ii++) { // era ii<16
nerit 38:79af1c65dd6f 2471 #endif
nerit 38:79af1c65dd6f 2472 #if defined(runner)
nerit 38:79af1c65dd6f 2473 for (int ii = 0; ii<tabeling; ii++) { // era ii<16
nerit 38:79af1c65dd6f 2474 #endif
nerit 14:e2b5efa06c41 2475 if ((speedCorrectionMachine>=tabSpeed[ii])&&(speedCorrectionMachine<tabSpeed[ii+1])) {
nerit 12:b0fc1d313813 2476 // esegue l'interpolazione dei valori stimati di duty in base alla velocità
nerit 12:b0fc1d313813 2477 deltaV=tabSpeed[ii+1]-tabSpeed[ii];
nerit 12:b0fc1d313813 2478 deltaD=tabComan[ii+1]-tabComan[ii];
nerit 14:e2b5efa06c41 2479 //setV = tractorSpeed_MtS_timed-tabSpeed[ii];
nerit 14:e2b5efa06c41 2480 setV = speedCorrectionMachine-tabSpeed[ii];
nerit 12:b0fc1d313813 2481 teoriaC=(setV/deltaV)*deltaD;
nerit 12:b0fc1d313813 2482 dutyTeorico = tabComan[ii]+teoriaC; // era ii+1 al 23/03/19
nerit 14:e2b5efa06c41 2483 #if defined(pcSerial)
nerit 14:e2b5efa06c41 2484 #if defined(boost1)
nerit 14:e2b5efa06c41 2485 pc.printf("tractor: %f tabspeed1: %f tabspeed2: %f\n",tractorSpeed_MtS_timed, tabSpeed[ii],tabSpeed[ii+1]);
nerit 14:e2b5efa06c41 2486 pc.printf("deltaV: %f deltaD: %f setV: %f teoriaC %f \n",deltaV,deltaD,setV,teoriaC);
nerit 14:e2b5efa06c41 2487 #endif
nerit 14:e2b5efa06c41 2488 #endif
nerit 3:a469bbd294b5 2489 }
nerit 8:310f9e4eac7b 2490 }
nerit 38:79af1c65dd6f 2491 #if defined(oldStepperDriver)
nerit 38:79af1c65dd6f 2492 if (tractorSpeed_MtS_timed > tabSpeed[16]) {
nerit 38:79af1c65dd6f 2493 dutyTeorico=tractorSpeed_MtS_timed/maxWorkSpeed;
nerit 38:79af1c65dd6f 2494 }
nerit 38:79af1c65dd6f 2495 #endif
nerit 38:79af1c65dd6f 2496 #if defined(runner)
nerit 38:79af1c65dd6f 2497 if (speedCorrectionMachine > tabSpeed[tabeling]) {
nerit 38:79af1c65dd6f 2498 dutyTeorico=speedCorrectionMachine/maxWorkSpeed;
nerit 38:79af1c65dd6f 2499 }
nerit 38:79af1c65dd6f 2500 #endif
nerit 8:310f9e4eac7b 2501 #if !defined(speedMaster)
nerit 8:310f9e4eac7b 2502 quinCalc();
nerit 8:310f9e4eac7b 2503 #endif
nerit 8:310f9e4eac7b 2504 #if defined(pcSerial)
nerit 8:310f9e4eac7b 2505 #if defined(Qncd)
nerit 8:310f9e4eac7b 2506 pc.printf("enableSpeed: %d pulseRised2: %d quincCnt: %d\n",enableSpeed,pulseRised2,quincCnt);
nerit 3:a469bbd294b5 2507 #endif
nerit 8:310f9e4eac7b 2508 #endif
nerit 8:310f9e4eac7b 2509 if ((enableSpeed>3)&&(pulseRised2==1)&&(quincCnt>=2)) {
nerit 8:310f9e4eac7b 2510 double erroreTempo = 0.0f;
nerit 8:310f9e4eac7b 2511 if(encoder==false) {
nerit 8:310f9e4eac7b 2512 if(speedFromPick==1) {
nerit 8:310f9e4eac7b 2513 erroreTempo = (double)timeIntraPick - tempoTraBecchi_mS;
nerit 8:310f9e4eac7b 2514 } else {
nerit 8:310f9e4eac7b 2515 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 2516 }
nerit 8:310f9e4eac7b 2517 } else {
nerit 8:310f9e4eac7b 2518 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 2519 #if defined(pcSerial)
nerit 8:310f9e4eac7b 2520 #if defined(Qnce)
nerit 8:310f9e4eac7b 2521 pc.printf("timeHole: %d TempoBecchi: %f erroreTempo: %f\n",memoTimeHole,tempoTraBecchi_mS,erroreTempo);
nerit 3:a469bbd294b5 2522 #endif
nerit 3:a469bbd294b5 2523 #endif
nerit 3:a469bbd294b5 2524 }
nerit 8:310f9e4eac7b 2525 double errorePercentuale = erroreTempo / tempoTraBecchi_mS;
nerit 8:310f9e4eac7b 2526 double k3=0.0f;
nerit 8:310f9e4eac7b 2527 double k4=0.0f;
nerit 8:310f9e4eac7b 2528 double k5=0.0f;
nerit 8:310f9e4eac7b 2529 double k6=0.0f;
nerit 8:310f9e4eac7b 2530 #if defined(speedMaster)
nerit 8:310f9e4eac7b 2531 k3=0.010f;
nerit 8:310f9e4eac7b 2532 #else
nerit 8:310f9e4eac7b 2533 k3=0.050f;
nerit 8:310f9e4eac7b 2534 #endif
nerit 8:310f9e4eac7b 2535 k4=1.103f;
nerit 8:310f9e4eac7b 2536 k5=10.00f;
nerit 8:310f9e4eac7b 2537 k6=20.50f;
nerit 8:310f9e4eac7b 2538 double L1 = 0.045f;
nerit 8:310f9e4eac7b 2539 double L_1=-0.045f;
nerit 8:310f9e4eac7b 2540 double L2 = 0.150f;
nerit 8:310f9e4eac7b 2541 double L_2=-0.150f;
nerit 8:310f9e4eac7b 2542 double L3 = 0.301f;
nerit 8:310f9e4eac7b 2543 double L_3=-0.301f;
nerit 8:310f9e4eac7b 2544 double k1=0.0f;
nerit 8:310f9e4eac7b 2545 if ((errorePercentuale > L3)||(errorePercentuale < L_3)) {
nerit 8:310f9e4eac7b 2546 k1=errorePercentuale*k6;
nerit 8:310f9e4eac7b 2547 }
nerit 8:310f9e4eac7b 2548 if (((errorePercentuale >= L2)&&(errorePercentuale<=L3))||((errorePercentuale <= L_2)&&(errorePercentuale>=L_3))) {
nerit 8:310f9e4eac7b 2549 k1=errorePercentuale*k5;
nerit 8:310f9e4eac7b 2550 }
nerit 8:310f9e4eac7b 2551 if (((errorePercentuale < L2)&&(errorePercentuale>L1))||((errorePercentuale > L_2)&&(errorePercentuale<L_1))) {
nerit 8:310f9e4eac7b 2552 k1=errorePercentuale*k4;
nerit 8:310f9e4eac7b 2553 }
nerit 8:310f9e4eac7b 2554 if ((errorePercentuale < L1)||(errorePercentuale > L_1)) {
nerit 8:310f9e4eac7b 2555 k1=errorePercentuale*k3;
nerit 8:310f9e4eac7b 2556 }
nerit 8:310f9e4eac7b 2557 double memoCorrezione = k1;
nerit 8:310f9e4eac7b 2558 if (quincCnt >= 2) {
nerit 8:310f9e4eac7b 2559 correzione = correzione + memoCorrezione;
nerit 8:310f9e4eac7b 2560 if (correzione > (1.0f - dutyTeorico)) {
nerit 8:310f9e4eac7b 2561 correzione = (1.0f - dutyTeorico);
nerit 34:eb04f4f41dfd 2562 //#if defined(pcSerial)
nerit 34:eb04f4f41dfd 2563 // pc.printf("limite\n");
nerit 34:eb04f4f41dfd 2564 //#endif
nerit 3:a469bbd294b5 2565 }
nerit 8:310f9e4eac7b 2566 if ((correzione < 0.0f)&&(dutyTeorico+correzione<0.0f)) {
nerit 8:310f9e4eac7b 2567 correzione = -1.0f*dutyTeorico;
nerit 34:eb04f4f41dfd 2568 //#if defined(pcSerial)
nerit 34:eb04f4f41dfd 2569 // pc.printf("limite\n");
nerit 34:eb04f4f41dfd 2570 //#endif
nerit 3:a469bbd294b5 2571 }
nerit 3:a469bbd294b5 2572 }
nerit 8:310f9e4eac7b 2573 pulseRised1=0;
nerit 8:310f9e4eac7b 2574 pulseRised2=0;
nerit 8:310f9e4eac7b 2575 #if defined(pcSerial)
nerit 8:310f9e4eac7b 2576 #if defined(Qnca)
nerit 8:310f9e4eac7b 2577 pc.printf("ErTem: %f K1: %f Corr: %f MemoCorr:%f DutyTeo: %f \n",erroreTempo, k1,correzione, memoCorrezione, dutyTeorico);
nerit 8:310f9e4eac7b 2578 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 2579 #endif
nerit 8:310f9e4eac7b 2580 #endif
nerit 8:310f9e4eac7b 2581 #if defined(pcSerial)
nerit 8:310f9e4eac7b 2582 #if defined(Qncb)
nerit 8:310f9e4eac7b 2583 pc.printf("TsP: %f SpW: %f InPic: %f EPerc: %f Duty:%f \n",tractorSpeed_MtS_timed,speedOfSeedWheel,timeIntraPick, errorePercentuale, dcActualDuty);
nerit 8:310f9e4eac7b 2584 #endif
nerit 8:310f9e4eac7b 2585 #endif
nerit 12:b0fc1d313813 2586 #if defined(pcSerial)
nerit 12:b0fc1d313813 2587 #if defined(boost)
nerit 12:b0fc1d313813 2588 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 2589 #endif
nerit 12:b0fc1d313813 2590 #endif
nerit 8:310f9e4eac7b 2591 }
nerit 12:b0fc1d313813 2592 // introduce il controllo di corrente
nerit 14:e2b5efa06c41 2593 double variazione=0.0f;
nerit 12:b0fc1d313813 2594 if (correggiCorrente==1){
nerit 12:b0fc1d313813 2595 if (SD_CurrentScaled < 1.6f){
nerit 12:b0fc1d313813 2596 boostDcOut +=0.01f;
nerit 12:b0fc1d313813 2597 }
nerit 12:b0fc1d313813 2598 if (SD_CurrentScaled > 2.6f){
nerit 12:b0fc1d313813 2599 boostDcOut -=0.01f;
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 12:b0fc1d313813 2605 if (boostDcOut <=-0.2f){
nerit 12:b0fc1d313813 2606 boostDcOut=-0.2f;
nerit 14:e2b5efa06c41 2607 //all_genericals=1;
nerit 12:b0fc1d313813 2608 }
nerit 14:e2b5efa06c41 2609 variazione=boostDcOut;
nerit 12:b0fc1d313813 2610 correggiCorrente=0;
nerit 8:310f9e4eac7b 2611 }
nerit 12:b0fc1d313813 2612 if (currentCheckEnable==true){
nerit 14:e2b5efa06c41 2613 correzione += variazione;
nerit 14:e2b5efa06c41 2614 variazione=0.0f;
nerit 8:310f9e4eac7b 2615 }
nerit 8:310f9e4eac7b 2616 DC_brake=0;
nerit 8:310f9e4eac7b 2617 DC_forward=1;
nerit 8:310f9e4eac7b 2618 DC_prepare();
bcostm 2:35f13b7f3659 2619
nerit 8:310f9e4eac7b 2620 // il semiperiodoreale è calcolato sulla lettura del passaggio becchi reale
nerit 8:310f9e4eac7b 2621 seedWheelPeriod = semiPeriodoReale;
nerit 8:310f9e4eac7b 2622 if (seedWheelPeriod < 180.0f) {
nerit 8:310f9e4eac7b 2623 seedWheelPeriod = 180.0f;
nerit 8:310f9e4eac7b 2624 }
nerit 8:310f9e4eac7b 2625 if ((oldSeedWheelPeriod!=seedWheelPeriod)&&(seedWheelPeriod >=180.0f )) {
nerit 8:310f9e4eac7b 2626 SDticker.attach_us(&step_SDPulseOut,seedWheelPeriod); // clock time are microseconds and attach seed motor stepper controls
nerit 8:310f9e4eac7b 2627 oldSeedWheelPeriod=seedWheelPeriod;
nerit 8:310f9e4eac7b 2628 }
nerit 3:a469bbd294b5 2629
nerit 8:310f9e4eac7b 2630 if((quincCnt>=3)) {
nerit 8:310f9e4eac7b 2631 if (correzioneAttiva==1) {
nerit 8:310f9e4eac7b 2632 dcActualDuty = dutyTeorico + correzione;
nerit 8:310f9e4eac7b 2633 } else {
nerit 3:a469bbd294b5 2634 dcActualDuty = dutyTeorico;
nerit 3:a469bbd294b5 2635 }
nerit 8:310f9e4eac7b 2636 } else {
nerit 8:310f9e4eac7b 2637 dcActualDuty = dutyTeorico;
nerit 8:310f9e4eac7b 2638 }
nerit 8:310f9e4eac7b 2639 if (dcActualDuty <=0.0f) {
nerit 8:310f9e4eac7b 2640 dcActualDuty=0.05f;
nerit 8:310f9e4eac7b 2641 }
nerit 8:310f9e4eac7b 2642 if (dcActualDuty > 0.95f) {
nerit 8:310f9e4eac7b 2643 dcActualDuty = 0.95f;
nerit 8:310f9e4eac7b 2644 }
nerit 17:9629eb019892 2645 if (dcActualDuty > (dutyTeorico *1.15f)){dcActualDuty=dutyTeorico*1.15f;}
nerit 14:e2b5efa06c41 2646 if (dcActualDuty < (dutyTeorico *0.85f)){dcActualDuty=dutyTeorico*0.85f;}
nerit 8:310f9e4eac7b 2647 if (olddcActualDuty!=dcActualDuty) {
nerit 8:310f9e4eac7b 2648 SDmotorPWM.write(1.0f-dcActualDuty);
nerit 8:310f9e4eac7b 2649 olddcActualDuty=dcActualDuty;
nerit 8:310f9e4eac7b 2650 }
nerit 8:310f9e4eac7b 2651 // allarme
nerit 31:92a9e183be8e 2652 if ((tractorSpeed_MtS_timed>0.0f)&&(alarmEnable==true)){
nerit 17:9629eb019892 2653 if (SDwheelTimer.read_ms()>4000) {
nerit 17:9629eb019892 2654 if (firstStart==0) {
nerit 17:9629eb019892 2655 all_noDcRotati=1;
nerit 17:9629eb019892 2656 }
nerit 17:9629eb019892 2657 #if defined(pcSerial)
nerit 17:9629eb019892 2658 #if defined(VediAllarmi)
nerit 17:9629eb019892 2659 pc.printf("allarme no DC rotation");
nerit 17:9629eb019892 2660 #endif
nerit 17:9629eb019892 2661 #endif
nerit 3:a469bbd294b5 2662 }
nerit 8:310f9e4eac7b 2663 }
nerit 3:a469bbd294b5 2664
nerit 8:310f9e4eac7b 2665 //***************************************************************************************************************
nerit 8:310f9e4eac7b 2666 // CONTROLLA TAMBURO
nerit 8:310f9e4eac7b 2667 //***************************************************************************************************************
nerit 8:310f9e4eac7b 2668 if(lowSpeed==0) {
nerit 8:310f9e4eac7b 2669 if (syncroCheck==1) {
nerit 8:310f9e4eac7b 2670 syncroCheck=0;
nerit 8:310f9e4eac7b 2671 lockStart=1;
nerit 8:310f9e4eac7b 2672 periodo = TBperiod;
nerit 35:3165f4c1c7bf 2673 #if !defined(oldStepperDriver)
nerit 35:3165f4c1c7bf 2674 #if !defined(runner)
nerit 35:3165f4c1c7bf 2675 #if defined(Zucca)
nerit 35:3165f4c1c7bf 2676 motor->step_clock_mode_enable(StepperMotor::BWD);
nerit 35:3165f4c1c7bf 2677 #else
nerit 35:3165f4c1c7bf 2678 motor->step_clock_mode_enable(StepperMotor::FWD);
nerit 35:3165f4c1c7bf 2679 #endif
nerit 9:7f02256f6e8f 2680 #endif
nerit 8:310f9e4eac7b 2681 #endif
nerit 8:310f9e4eac7b 2682 if (aspettaStart==0) {
nerit 6:e8c18f0f399a 2683 #if defined(pcSerial)
nerit 6:e8c18f0f399a 2684 #if defined(checkLoop)
nerit 8:310f9e4eac7b 2685 pc.printf("da sincro\n");
nerit 6:e8c18f0f399a 2686 #endif
nerit 6:e8c18f0f399a 2687 #endif
nerit 39:6814c75dfa5c 2688 cambiaTB(periodo,3);
nerit 3:a469bbd294b5 2689 }
nerit 8:310f9e4eac7b 2690 }
nerit 8:310f9e4eac7b 2691 // controllo di stop
nerit 8:310f9e4eac7b 2692 double memoIntraP = (double)memoIntraPick*1.8f;
nerit 8:310f9e4eac7b 2693 if ((double)rotationTimeOut.read_ms()> (memoIntraP)) {
nerit 3:a469bbd294b5 2694 syncroCheck=0;
nerit 8:310f9e4eac7b 2695 aspettaStart=1;
nerit 8:310f9e4eac7b 2696 countCicli=0;
nerit 6:e8c18f0f399a 2697 #if defined(pcSerial)
nerit 6:e8c18f0f399a 2698 #if defined(checkLoop)
nerit 8:310f9e4eac7b 2699 pc.printf("AspettaSI\n");
nerit 6:e8c18f0f399a 2700 #endif
nerit 6:e8c18f0f399a 2701 #endif
nerit 8:310f9e4eac7b 2702 if (TBzeroCyclePulse==1) {
nerit 8:310f9e4eac7b 2703 #if !defined(runner)
nerit 39:6814c75dfa5c 2704 //TBticker.detach();
nerit 39:6814c75dfa5c 2705 //TBmotorRst=1;
nerit 39:6814c75dfa5c 2706 stopTb();
nerit 8:310f9e4eac7b 2707 #endif
nerit 35:3165f4c1c7bf 2708 #if !defined(oldStepperDriver)
nerit 35:3165f4c1c7bf 2709 #if defined(runner)
nerit 35:3165f4c1c7bf 2710 motor->soft_hiz();
nerit 35:3165f4c1c7bf 2711 #endif
nerit 35:3165f4c1c7bf 2712 #endif
nerit 8:310f9e4eac7b 2713 }
nerit 8:310f9e4eac7b 2714 }
nerit 8:310f9e4eac7b 2715 } else { // fine ciclo fuori da low speed
nerit 8:310f9e4eac7b 2716 syncroCheck=0;
nerit 8:310f9e4eac7b 2717 lockStart=0;
nerit 8:310f9e4eac7b 2718 if (beccoPronto==1) {
nerit 8:310f9e4eac7b 2719 if (tamburoStandard==1) {
nerit 8:310f9e4eac7b 2720 double ritardoMassimo = 0.0f;
nerit 8:310f9e4eac7b 2721 if (encoder==false) {
nerit 8:310f9e4eac7b 2722 if(speedFromPick==1) {
nerit 8:310f9e4eac7b 2723 ritardoMassimo = (double)timeIntraPick;
nerit 8:310f9e4eac7b 2724 } else {
nerit 8:310f9e4eac7b 2725 ritardoMassimo = (double)memoTimeHole;
nerit 8:310f9e4eac7b 2726 }
nerit 8:310f9e4eac7b 2727 } else {
nerit 8:310f9e4eac7b 2728 ritardoMassimo = (double)timeIntraPick;
nerit 8:310f9e4eac7b 2729 }
nerit 8:310f9e4eac7b 2730 int tempoDiSincro = (int)((double)(ritardoMassimo - ((tempoBecco/1.8f)+((speedOfSeedWheel/maxWorkSpeed)*ritardoMassimo)))); //
nerit 8:310f9e4eac7b 2731 if (tempoDiSincro <= 1) {
nerit 8:310f9e4eac7b 2732 tempoDiSincro=1;
nerit 8:310f9e4eac7b 2733 }
nerit 8:310f9e4eac7b 2734 if ((sincroTimer.read_ms()>= tempoDiSincro)) {
nerit 8:310f9e4eac7b 2735 if (tractorSpeed_MtS_timed >= minWorkSpeed) {
nerit 8:310f9e4eac7b 2736 startCicloTB=1;
nerit 8:310f9e4eac7b 2737 }
nerit 8:310f9e4eac7b 2738 beccoPronto=0;
nerit 8:310f9e4eac7b 2739 }
nerit 8:310f9e4eac7b 2740 } else {
nerit 8:310f9e4eac7b 2741 // tamburo per zucca
nerit 8:310f9e4eac7b 2742 if (speedOfSeedWheel >= minWorkSpeed) {
nerit 8:310f9e4eac7b 2743 startCicloTB=1;
nerit 8:310f9e4eac7b 2744 }
nerit 8:310f9e4eac7b 2745 beccoPronto=0;
nerit 8:310f9e4eac7b 2746 }
nerit 8:310f9e4eac7b 2747 }
nerit 8:310f9e4eac7b 2748 ciclaTB();
nerit 8:310f9e4eac7b 2749 }
nerit 8:310f9e4eac7b 2750 //*************************************************************
nerit 8:310f9e4eac7b 2751 } else { // fine ciclo con velocita maggiore di 0
nerit 13:d1030d4e51a8 2752 delaySpeedCheck.reset();
nerit 8:310f9e4eac7b 2753 if (cycleStopRequest==1) {
nerit 8:310f9e4eac7b 2754 SDwheelTimer.stop();
nerit 8:310f9e4eac7b 2755 SDwheelTimer.reset();
nerit 8:310f9e4eac7b 2756 #if defined(seedSensor)
nerit 8:310f9e4eac7b 2757 resetDelay();
nerit 8:310f9e4eac7b 2758 #endif
nerit 8:310f9e4eac7b 2759 checkSDrotation=0;
nerit 8:310f9e4eac7b 2760 oldFaseLavoro=0;
nerit 8:310f9e4eac7b 2761 aspettaStart=1;
nerit 8:310f9e4eac7b 2762 countCicli=0;
nerit 8:310f9e4eac7b 2763 oldSeedWheelPeriod=0.0f;
nerit 8:310f9e4eac7b 2764 oldPeriodoTB=0.0f;
nerit 8:310f9e4eac7b 2765 correzione=0.0f;
nerit 8:310f9e4eac7b 2766 OLDpulseSpeedInterval=1000.01f;
nerit 8:310f9e4eac7b 2767 cicloTbinCorso=0;
nerit 8:310f9e4eac7b 2768 cntTbError=0;
nerit 8:310f9e4eac7b 2769 olddcActualDuty=0.0f;
nerit 8:310f9e4eac7b 2770 #if defined(pcSerial)
nerit 8:310f9e4eac7b 2771 #if defined(checkLoopb)
nerit 8:310f9e4eac7b 2772 pc.printf("forza\n");
nerit 8:310f9e4eac7b 2773 #endif
nerit 8:310f9e4eac7b 2774 #endif
nerit 8:310f9e4eac7b 2775 speedOfSeedWheel=0.0f;
nerit 8:310f9e4eac7b 2776 cycleStopRequest=0;
nerit 8:310f9e4eac7b 2777 DC_brake=1;
nerit 8:310f9e4eac7b 2778 DC_prepare();
nerit 8:310f9e4eac7b 2779 metalTimer.reset();
nerit 8:310f9e4eac7b 2780 #if defined(pcSerial)
nerit 8:310f9e4eac7b 2781 #if defined(checkLoop)
nerit 8:310f9e4eac7b 2782 pc.printf("17h\n");
nerit 8:310f9e4eac7b 2783 #endif
nerit 8:310f9e4eac7b 2784 #endif
nerit 8:310f9e4eac7b 2785 #if !defined(runner)
nerit 39:6814c75dfa5c 2786 //TBticker.detach();
nerit 39:6814c75dfa5c 2787 //TBmotorRst=1;
nerit 39:6814c75dfa5c 2788 stopTb();
nerit 8:310f9e4eac7b 2789 #endif
nerit 8:310f9e4eac7b 2790 #if defined(pcSerial)
nerit 8:310f9e4eac7b 2791 #if defined(loStop)
nerit 8:310f9e4eac7b 2792 pc.printf("A5\n");
nerit 6:e8c18f0f399a 2793 #endif
nerit 8:310f9e4eac7b 2794 #endif
nerit 20:4a400a4cc419 2795 if(simStepper==0){
nerit 35:3165f4c1c7bf 2796 #if defined(runner)
nerit 35:3165f4c1c7bf 2797 motor->soft_hiz();
nerit 35:3165f4c1c7bf 2798 motor->set_home();
nerit 35:3165f4c1c7bf 2799 #endif
nerit 35:3165f4c1c7bf 2800 #if defined(oldStepperDriver)
nerit 39:6814c75dfa5c 2801 //TBticker.detach();
nerit 39:6814c75dfa5c 2802 //TBmotorRst=1;
nerit 39:6814c75dfa5c 2803 stopTb();
nerit 35:3165f4c1c7bf 2804 #endif
nerit 20:4a400a4cc419 2805 }
nerit 18:7c978f69cc51 2806 cntSpeedError=0;
nerit 18:7c978f69cc51 2807 timeIntraPick=0.0f;
nerit 19:231b8931cabc 2808 memoIntraPick=0.001f;
nerit 19:231b8931cabc 2809 intraPickTimer.reset();
nerit 8:310f9e4eac7b 2810 pntMedia=0;
nerit 8:310f9e4eac7b 2811 #if defined(pcSerial)
nerit 8:310f9e4eac7b 2812 #if defined(stopSignal)
nerit 8:310f9e4eac7b 2813 pc.printf("stop\n");
nerit 6:e8c18f0f399a 2814 #endif
nerit 8:310f9e4eac7b 2815 #endif
nerit 3:a469bbd294b5 2816 }
nerit 8:310f9e4eac7b 2817 }
nerit 8:310f9e4eac7b 2818
nerit 8:310f9e4eac7b 2819 //*************************************************************************************************
nerit 3:a469bbd294b5 2820 TBzeroCyclePulse=0;
nerit 8:310f9e4eac7b 2821 //*************************************************************************************************
nerit 6:e8c18f0f399a 2822 } //end inProva==0
nerit 3:a469bbd294b5 2823 wd.Service(); // kick the dog before the timeout
nerit 3:a469bbd294b5 2824 } // end while
nerit 3:a469bbd294b5 2825 } // end main