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@34:eb04f4f41dfd, 2020-03-11 (annotated)

Committer:
nerit
Date:
Wed Mar 11 09:35:36 2020 +0000
Revision:
34:eb04f4f41dfd
Parent:
33:81b406a911b6
Child:
35:3165f4c1c7bf
Versione Marzo 2020

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