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@45:162116db828a, 2020-07-20 (annotated)

Committer:: nerit
Date:: Mon Jul 20 09:06:29 2020 +0000
Revision:: 45:162116db828a
Parent:: 44:13c0c33f1554

versione di prova per verifica accelerazione tamburo

Who changed what in which revision?

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

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning