Forigo / Mbed 2 deprecated FORIGO_Modula_V7_3_VdcStep_maggio2020

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

Dependencies:   mbed X_NUCLEO_IHM03A1_for

Fork of FORIGO_Modula_V7_3_VdcStep_maggio2020 by Francesco Pistone

main.cpp@31:92a9e183be8e, 2019-05-24 (annotated)

Committer:
nerit
Date:
Fri May 24 10:29:09 2019 +0000
Revision:
31:92a9e183be8e
Parent:
30:32e770e91998
Child:
32:cfc3d7420fa5
Versione del 24 maggio 2019 per Francia con modifica allarmi. Gli allarmi sono attivi solo dopo 3 secondi che la macchina rileva la velocita di avanzamento. L'allarme di mancata lettura sensore di velocita e' invece sempre attivo.

Who changed what in which revision?

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