Forigo / Mbed 2 deprecated FORIGO_Modula_V7_3_VdcStep_maggio2020

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

Dependencies:   mbed X_NUCLEO_IHM03A1_for

Fork of FORIGO_Modula_V7_3_VdcStep_maggio2020 by Francesco Pistone

main.cpp@4:de1b296e9757, 2019-02-13 (annotated)

Committer:
nerit
Date:
Wed Feb 13 07:29:30 2019 +0000
Revision:
4:de1b296e9757
Parent:
3:a469bbd294b5
Child:
5:2a3a64b52f54
V1.1

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