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@30:32e770e91998, 2019-05-23 (annotated)

Committer:: nerit
Date:: Thu May 23 14:12:06 2019 +0000
Revision:: 30:32e770e91998
Parent:: 29:25a875d09817
Child:: 31:92a9e183be8e

versione del 23-05

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

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning