Forigo / Mbed 2 deprecated FORIGO_Modula_V7_3_VdcStep_Modula2021

Important changes to repositories hosted on mbed.com

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

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

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

Dependencies:   mbed X_NUCLEO_IHM03A1_for

main.cpp@8:310f9e4eac7b, 2019-03-11 (annotated)

Committer:
nerit
Date:
Mon Mar 11 06:44:59 2019 +0000
Revision:
8:310f9e4eac7b
Parent:
6:e8c18f0f399a
Child:
9:7f02256f6e8f
a2

Who changed what in which revision?

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