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@35:3165f4c1c7bf, 2020-04-22 (annotated)

Committer:
nerit
Date:
Wed Apr 22 07:37:41 2020 +0000
Revision:
35:3165f4c1c7bf
Parent:
34:eb04f4f41dfd
Child:
36:896cd03314f1
Aggiornamento per funzionamento nuovo sw con vecchie schede

Who changed what in which revision?

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