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

Forigo » Code » FORIGO_Modula_V7_3_VdcStep_maggio2020

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@38:79af1c65dd6f, 2020-04-27 (annotated)

Committer:: nerit
Date:: Mon Apr 27 15:01:00 2020 +0000
Revision:: 38:79af1c65dd6f
Parent:: 37:0a225902cf48
Child:: 39:6814c75dfa5c

Modificato file parametri e rimessi come nella versione V6_R2C_DE_ZUCCA

Who changed what in which revision?

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

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	26 Jun 2020
Imports:	4
Forks:	0
Commits:	46
Dependents:	0
Dependencies:	2
Followers:	3

main.cpp@38:79af1c65dd6f, 2020-04-27 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning