FORIGO_Modula_V6_R2C_DE_HwV5_OTTOBRE2018 - tr

Users » francescopistone » Code » FORIGO_Modula_V6_R2C_DE_HwV5_OTTOBRE2018

Francesco Pistone / Mbed 2 deprecated FORIGO_Modula_V6_R2C_DE_HwV5_OTTOBRE2018

main.cpp@1:e88bf5011af6, 2018-06-15 (annotated)

Committer:: nerit
Date:: Fri Jun 15 07:01:13 2018 +0000
Revision:: 1:e88bf5011af6
Parent:: 0:1e09cd7d66b4
Child:: 2:d9c7430ae953

update prima del quinconcio

Who changed what in which revision?

User	Revision	Line number	New contents of line
nerit	1:e88bf5011af6	1
nerit	1:e88bf5011af6	2	//********************************************************************************************************************
nerit	0:1e09cd7d66b4	3	//********************************************************************************************************************
nerit	0:1e09cd7d66b4	4	// FIRMWARE SEMINATRICE MODULA
nerit	0:1e09cd7d66b4	5	// VERSIONE PER SCHEDA DI CONTROLLO CON DRIVER INTEGRATI
nerit	0:1e09cd7d66b4	6	// V4 - ATTENZIONE - LA VERSIONE V4 HA IL DRIVER STEPPER LV8727
nerit	0:1e09cd7d66b4	7	// IL MOTORE DC E' GESTITO CON IL DRIVER VNH3SP30-E E CON LA LETTURA
nerit	0:1e09cd7d66b4	8	// DELLA CORRENTE ASSORBITA TRAMITE IL CONVERTITORE MLX91210-CAS102 CON 50A FONDOSCALA
nerit	0:1e09cd7d66b4	9	// CHE FORNISCE UNA TENSIONE DI USCITA PARI A 40mV/A
nerit	0:1e09cd7d66b4	10	// FIRST RELEASE OF BOARD DEC 2017
nerit	0:1e09cd7d66b4	11	// FIRST RELEASE OF FIRMWARE JAN 2018
nerit	0:1e09cd7d66b4	12	//
nerit	0:1e09cd7d66b4	13	// THIS RELEASE: 09 JULY 2018
nerit	0:1e09cd7d66b4	14	//
nerit	0:1e09cd7d66b4	15	// APPLICATION: MODULA CON DISTRIBUTORE ZUCCA OPPURE RISO E PUO' FUNZIONARE ANCHE CON SENSORE A 25 FORI SUL DISCO O
nerit	0:1e09cd7d66b4	16	// ENCODER MOTORE SETTANDO GLI APPOSITI FLAGS
nerit	0:1e09cd7d66b4	17	//
nerit	0:1e09cd7d66b4	18	// 29 05 2018 - INSERITO SECONDO ENCODER VIRTUALE PER LA GESTIONE DEL SINCRONISMO TRA TAMBURO E RUOTA DI SEMINA
nerit	0:1e09cd7d66b4	19	// IN PRATICA IL PRIMO ENCODER è SINCRONO CON IL SEGNALE DEI BECCHI E VIENE AZZERATO DA QUESTI, MENTRE
nerit	0:1e09cd7d66b4	20	// IL SECONDO E' INCREMENTATO IN SINCRONO CON IL PRIMO MA AZZERATO DALLA FASE. IL SUO VALORE E' POI DIVISO
nerit	0:1e09cd7d66b4	21	// PER IL RAPPORTO RUOTE E LA CORREZIONE AGISCE SULLA VELOCITA' DEL TAMBURO PER MANTENERE LA FASE DEL SECONDO
nerit	0:1e09cd7d66b4	22	// ENCODER
nerit	0:1e09cd7d66b4	23	// 05 06 2018 - INSERITO IL CONTROLLO DI GESTIONE DEL QUINCONCE SENZA ENCODER
nerit	0:1e09cd7d66b4	24	// 09 06 2018 - INSERITO CONTROLLO DI FASE CON ENCODER MASTER PER QUINCONCE - DATO SCAMBIATO IN CAN
nerit	0:1e09cd7d66b4	25	//
nerit	0:1e09cd7d66b4	26	/********************
nerit	0:1e09cd7d66b4	27	IL FIRMWARE SI COMPONE DI 7 FILES:
nerit	0:1e09cd7d66b4	28	- main.cpp
nerit	0:1e09cd7d66b4	29	- main.hpp
nerit	0:1e09cd7d66b4	30	- iodefinition.hpp
nerit	0:1e09cd7d66b4	31	- canbus.hpp
nerit	0:1e09cd7d66b4	32	- parameters.hpp
nerit	0:1e09cd7d66b4	33	- timeandtick.hpp
nerit	0:1e09cd7d66b4	34	- variables.hpp
nerit	0:1e09cd7d66b4	35	ED UTILIZZA LE LIBRERIE STANDARD MBED PIU' UNA LIBRERIA MODIFICATA E DEDICATA PER IL CAN
nerit	0:1e09cd7d66b4	36	*********************
nerit	0:1e09cd7d66b4	37	LA MACCHINA UTILIZZA SEMPRE 2 SOLI SENSORI; UNO PER SENTIRE LE CELLE DI CARICO SEME ED UNO PER SENTIRE I BECCHI DI SEMINA.
nerit	0:1e09cd7d66b4	38	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	0:1e09cd7d66b4	39	UN SENSORE AGGIUNTIVO SULL'ELEMENTO MASTER RILEVA LA VELOCITA' DI AVANZAMENTO
nerit	0:1e09cd7d66b4	40	UN SENSORE AGGIUNTIVO SULLA RUOTA DI SEMINA RILEVA LA ROTAZIONE DELLA RUOTA STESSA ATTRAVERSO FORI PRESENTI SUL DISCO DI SEMINA
nerit	0:1e09cd7d66b4	41	*********************
nerit	0:1e09cd7d66b4	42	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	0:1e09cd7d66b4	43	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	0:1e09cd7d66b4	44	PROPRIO PERCHE' L'AVANZAMANETO NEL TERRENO IMPRIME UNA VELOCITA' PROPRIA AL BECCO E QUINDI ANCHE ALLA RUOTA.
nerit	0:1e09cd7d66b4	45	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	0:1e09cd7d66b4	46	IL VALORE DI RIFERIMENTO DELL'ANALOGICA DI INGRESSO VIENE AGGIORNATO OGNI VOLTA CHE LA RUOTA DI SEMINA E' FERMA
nerit	0:1e09cd7d66b4	47	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	0:1e09cd7d66b4	48	LA FASE VIENE DETERMINATA DAL PASSAGGIO DEI BECCHI SUL SENSORE RELATIVO.
nerit	0:1e09cd7d66b4	49	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	0:1e09cd7d66b4	50	SOPRATUTTO QUANDO I BECCHI SONO MOLTO DISTANZIATI.
nerit	0:1e09cd7d66b4	51	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	0:1e09cd7d66b4	52	TARATO SULLA REALE VELOCITA' DI ROTAZIONE DELLA RUOTA DI SEMINA.
nerit	0:1e09cd7d66b4	53	IL PRIMO ENCODER VIRTUALE SI OCCUPA DI DETERMINARE LA POSIZIONE FISICA DELLA RUOTA DI SEMINA E SI AZZERA AL PASSAGGIO DI OGNI BECCO.
nerit	0:1e09cd7d66b4	54	IL SECONDO VIENE AZZERATO DALL'IMPULSO DI FASE DEL PRIMO ENCODER DETERMINATO DAI VALORI IMPOSTI SUL TERMINALE TRITECNICA
nerit	0:1e09cd7d66b4	55	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	0:1e09cd7d66b4	56	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	0:1e09cd7d66b4	57	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	0:1e09cd7d66b4	58	DI CAMBIARE L'ANGOLO DI ANTICIPO DI RILASCIO DEL SEME IN FUNZIONE DELLA VELOCITA' E RECUPERARE COSI' IL TEMPO DI VOLO DEL SEME.
nerit	0:1e09cd7d66b4	59	IL TAMBURO HA DUE TIPI DI FUNZIONAMENTO: CONTINUO E AD IMPULSI. E' SELEZIONABILE IN FUNZIONE DELLA VELOCITA' E DEL TIPO DI DISTRIBUTORE MONTATO.
nerit	0:1e09cd7d66b4	60	**********************
nerit	0:1e09cd7d66b4	61	TUTTI I VALORI, CELLE, BECCHI, IMPULSI VELOCITA', ANCGOLO DI AVVIO, FASE DI SEMINA, ECC.. SONO IMPOSTABILI DA PANNELLO OPERATORE
nerit	0:1e09cd7d66b4	62	I DATI SONO SCAMBIATI CON IL PANNELLO OPERATORE E CON GLI ALTRI MODULI ATTRAVERSO RETE CAN CON PROTOCOLLO FREESTYLE ATTRAVERSO INDIRIZZAMENTI DEDICATI
nerit	0:1e09cd7d66b4	63	AL MOMENTO NON E' POSSIBILE ATTRIBUIRE L'INIDIRIZZO BASE DELL'ELEMENTO DA TERMINALE OPERATORE MA SOLO IN FASE DI COMPILAZIONE DEL FIRMWARE.
nerit	0:1e09cd7d66b4	64	**********************
nerit	0:1e09cd7d66b4	65	ALTRE SEZIONI RIGUARDANO LA GENERAZIONE DEGLI ALLARMI, LA COMUNICAZIONE CAN, LA SIMULAZIONE DI LAVORO, LA GESTIONE DELLA DIAGNOSI ECC..
nerit	0:1e09cd7d66b4	66	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	0:1e09cd7d66b4	67	LO STEPPER SEGUE IL DC.
nerit	0:1e09cd7d66b4	68	**********************
nerit	0:1e09cd7d66b4	69	IN FASE DI ACCENSIONE ED OGNI QUALVOLTA SI ARRIVA A VELOCITA' ZERO, LA MACCHINA ESEGUE UN CICLO DI AZZERAMENTO
nerit	0:1e09cd7d66b4	70	NON ESISTE PULSANTE DI MARCIA/STOP; E' SEMPRE ATTIVA.
nerit	0:1e09cd7d66b4	71	**********************
nerit	0:1e09cd7d66b4	72	NEL PROGRAMMA E' PRESENTE UNA SEZIONE DI TEST FISICO DELLA SCHEDA ATTIVABILE SOLO IN FASE DI COMPILAZIONE
nerit	0:1e09cd7d66b4	73	**********************
nerit	0:1e09cd7d66b4	74	ALTRE FUNZIONI: PRECARICAMENTO DEL TAMBURO
nerit	0:1e09cd7d66b4	75	AZZERAMENTO MANUALE
nerit	0:1e09cd7d66b4	76	STATISTICA DI SEMINA (CONTA LE CELLE)
nerit	0:1e09cd7d66b4	77	*/
nerit	0:1e09cd7d66b4	78	//********************************************************************************************************************
nerit	0:1e09cd7d66b4	79	//********************************************************************************************************************
nerit	0:1e09cd7d66b4	80
nerit	0:1e09cd7d66b4	81	#include "main.hpp"
nerit	0:1e09cd7d66b4	82	#include "timeandtick.hpp"
nerit	0:1e09cd7d66b4	83	#include "canbus.hpp"
nerit	0:1e09cd7d66b4	84	#include "watchdog.h"
nerit	0:1e09cd7d66b4	85	#include "iodefinition.hpp"
nerit	0:1e09cd7d66b4	86	#include "parameters.hpp"
nerit	0:1e09cd7d66b4	87	#include "variables.hpp"
nerit	0:1e09cd7d66b4	88	//********************************************************************************************************************
nerit	0:1e09cd7d66b4	89	//********************************************************************************************************************
nerit	0:1e09cd7d66b4	90	// ------------------------------------------------------------------------------------------------------------------------------------------------------------------
nerit	0:1e09cd7d66b4	91	// TASK SECTION
nerit	0:1e09cd7d66b4	92	// ------------------------------------------------------------------------------------------------------------------------------------------------------------------
nerit	0:1e09cd7d66b4	93	//************************************************************************
nerit	0:1e09cd7d66b4	94	// rise of seed speed 25 pulse sensor
nerit	0:1e09cd7d66b4	95	void sd25Fall(){
nerit	0:1e09cd7d66b4	96	timeHole=metalTimer.read_ms();
nerit	0:1e09cd7d66b4	97	int memo_TimeHole= (memoTimeHole + timeHole)/ (int)2;
nerit	0:1e09cd7d66b4	98	memoTimeHole = timeHole;
nerit	0:1e09cd7d66b4	99	metalTimer.reset();
nerit	0:1e09cd7d66b4	100	if (speedFromPick==0){
nerit	0:1e09cd7d66b4	101	speedOfSeedWheel=((seedPerimeter/25.0f)/(double)memo_TimeHole)*1000.0f; //mtS
nerit	0:1e09cd7d66b4	102	}
nerit	0:1e09cd7d66b4	103	}
nerit	0:1e09cd7d66b4	104	//**************************************************
nerit	0:1e09cd7d66b4	105	// generate speed clock when speed is simulated from Tritecnica display
nerit	0:1e09cd7d66b4	106	void speedSimulationClock(){
nerit	0:1e09cd7d66b4	107	lastPulseRead=speedTimer.read_us();
nerit	0:1e09cd7d66b4	108	oldLastPulseRead=lastPulseRead;
nerit	0:1e09cd7d66b4	109	speedTimer.reset();
nerit	0:1e09cd7d66b4	110	pulseRised=1;
nerit	0:1e09cd7d66b4	111	}
nerit	0:1e09cd7d66b4	112	//*******************************************************
nerit	0:1e09cd7d66b4	113	// interrupt task for tractor speed reading
nerit	0:1e09cd7d66b4	114	//*******************************************************
nerit	0:1e09cd7d66b4	115	void tractorReadSpeed(){
nerit	0:1e09cd7d66b4	116	if ((oldTractorSpeedRead==0)){
nerit	0:1e09cd7d66b4	117	lastPulseRead=speedTimer.read_us();
nerit	0:1e09cd7d66b4	118	oldLastPulseRead=lastPulseRead;
nerit	0:1e09cd7d66b4	119	speedTimer.reset();
nerit	0:1e09cd7d66b4	120	pulseRised=1;
nerit	0:1e09cd7d66b4	121	oldTractorSpeedRead=1;
nerit	0:1e09cd7d66b4	122	}
nerit	0:1e09cd7d66b4	123	speedFilter.reset();
nerit	0:1e09cd7d66b4	124	speedClock=1;
nerit	0:1e09cd7d66b4	125	}
nerit	0:1e09cd7d66b4	126	//*******************************************************
nerit	0:1e09cd7d66b4	127	void speedMediaCalc(){
nerit	0:1e09cd7d66b4	128	double lastPd=(double) lastPulseRead/1000.0f;
nerit	0:1e09cd7d66b4	129	pulseSpeedInterval = (mediaSpeed[0]+lastPd)/2.0f;
nerit	0:1e09cd7d66b4	130	if (enableSimula==1){
nerit	0:1e09cd7d66b4	131	double TMT = (double)(speedSimula) * 100.0f /3600.0f;
nerit	0:1e09cd7d66b4	132	pulseSpeedInterval = pulseDistance / TMT;
nerit	0:1e09cd7d66b4	133	}
nerit	0:1e09cd7d66b4	134	mediaSpeed[0]=lastPd;
nerit	0:1e09cd7d66b4	135	OLDpulseSpeedInterval=pulseSpeedInterval;
nerit	0:1e09cd7d66b4	136	}
nerit	0:1e09cd7d66b4	137
nerit	0:1e09cd7d66b4	138	//*******************************************************
nerit	0:1e09cd7d66b4	139	// clocked task for manage virtual encoder of seed wheel i/o
nerit	0:1e09cd7d66b4	140	//*******************************************************
nerit	0:1e09cd7d66b4	141	//*******************************************************
nerit	0:1e09cd7d66b4	142	void step_SDPulseOut(){
nerit	0:1e09cd7d66b4	143	SDactualPosition++;
nerit	0:1e09cd7d66b4	144	prePosSD++;
nerit	0:1e09cd7d66b4	145	}
nerit	0:1e09cd7d66b4	146	//*******************************************************
nerit	0:1e09cd7d66b4	147	void step_TBPulseOut(){
nerit	0:1e09cd7d66b4	148	TBmotorStepOut=!TBmotorStepOut;
nerit	0:1e09cd7d66b4	149	if (TBmotorStepOut==0){
nerit	0:1e09cd7d66b4	150	TBactualPosition++;
nerit	0:1e09cd7d66b4	151	}
nerit	0:1e09cd7d66b4	152	}
nerit	0:1e09cd7d66b4	153	//*******************************************************
nerit	0:1e09cd7d66b4	154	// aggiornamento parametri di lavoro fissi e da Tritecnica
nerit	0:1e09cd7d66b4	155	void aggiornaParametri(){
nerit	0:1e09cd7d66b4	156	speedPerimeter = Pi * speedWheelDiameter ; // perimeter of speed wheel
nerit	0:1e09cd7d66b4	157	pulseDistance = (speedPerimeter / speedWheelPulse)*1000.0f; // linear space between speed wheel pulse
nerit	0:1e09cd7d66b4	158	seedPerimeter = Pi * (seedWheelDiameter-(deepOfSeed*2.0f)); // perimeter of seed wheel
nerit	0:1e09cd7d66b4	159	intraPickDistance = seedPerimeter/pickNumber;
nerit	0:1e09cd7d66b4	160	K_WheelRPM = 60.0f/seedPerimeter; // calcola il K per i giri al minuto della ruota di semina
nerit	0:1e09cd7d66b4	161	K_WhellFrequency = (seedWheelMotorSteps*SDreductionRatio)/60.0f; // calcola il K per la frequenza di comando del motore di semina
nerit	0:1e09cd7d66b4	162	rapportoRuote = pickNumber/cellsNumber; // calcola il rapporto tra il numero di becchi ed il numero di celle
nerit	0:1e09cd7d66b4	163	K_TBfrequency = (TBmotorSteps*TBreductionRatio)/240.0f;
nerit	0:1e09cd7d66b4	164	K_percentuale = TBmotorSteps*TBreductionRatio;
nerit	0:1e09cd7d66b4	165	SDsectorStep = (double)fixedStepGiroSD / (double)pickNumber;
nerit	0:1e09cd7d66b4	166	TBsectorStep = (TBmotorSteps*TBreductionRatio)/cellsNumber;
nerit	0:1e09cd7d66b4	167	KcorT = (SDsectorStep/TBsectorStep);///2.0f;
nerit	0:1e09cd7d66b4	168	angoloFase=angoloPh;
nerit	0:1e09cd7d66b4	169	avvioGradi=angoloAv;
nerit	0:1e09cd7d66b4	170	stepGrado=fixedStepGiroSD/360.0f;
nerit	0:1e09cd7d66b4	171	TBdeltaStep=(fixedStepGiroSD/pickNumber)+(stepGrado*avvioGradi);
nerit	0:1e09cd7d66b4	172	TBfaseStep = (stepGrado*angoloFase);
nerit	0:1e09cd7d66b4	173	K_WheelRPM = 60.0f/seedPerimeter; // calcola il K per i giri al minuto della ruota di semina 25.4 25,40
nerit	0:1e09cd7d66b4	174	if (speedFromPick==1) {
nerit	0:1e09cd7d66b4	175	intraPickDistance = seedPerimeter/pickNumber;
nerit	0:1e09cd7d66b4	176	}else{
nerit	0:1e09cd7d66b4	177	intraPickDistance = seedPerimeter/25.0f; // 25 è il numero di fori presenti nel disco di semina
nerit	0:1e09cd7d66b4	178	}
nerit	0:1e09cd7d66b4	179	if (anticipoMax > ((360.0f/pickNumber)-(avvioGradi+16.0f))){
nerit	0:1e09cd7d66b4	180	anticipoMax=(360.0f/pickNumber)-(avvioGradi+16.0f);
nerit	0:1e09cd7d66b4	181	}
nerit	0:1e09cd7d66b4	182	}
nerit	0:1e09cd7d66b4	183	//*******************************************************
nerit	0:1e09cd7d66b4	184	void cambiaTB(double perio){
nerit	0:1e09cd7d66b4	185	// update TB frequency
nerit	0:1e09cd7d66b4	186	double TBper=0.0f;
nerit	0:1e09cd7d66b4	187	if (aspettaStart==0){
nerit	0:1e09cd7d66b4	188	if (perio<250.0f){perio=500.0f;}
nerit	0:1e09cd7d66b4	189	double scala =0.0f;
nerit	0:1e09cd7d66b4	190	if (lowSpeed==1){
nerit	0:1e09cd7d66b4	191	scala =2.0f;
nerit	0:1e09cd7d66b4	192	}else{
nerit	0:1e09cd7d66b4	193	scala =1.8f;
nerit	0:1e09cd7d66b4	194	}
nerit	0:1e09cd7d66b4	195	TBper=perio/scala;
nerit	0:1e09cd7d66b4	196	if (oldPeriodoTB!=TBper){
nerit	0:1e09cd7d66b4	197	if (TBper >= 250.0f){
nerit	0:1e09cd7d66b4	198	TBticker.attach_us(&step_TBPulseOut,TBper); // clock time are milliseconds and attach seed motor stepper controls
nerit	0:1e09cd7d66b4	199	}else{
nerit	0:1e09cd7d66b4	200	TBticker.detach();
nerit	0:1e09cd7d66b4	201	}
nerit	0:1e09cd7d66b4	202	oldPeriodoTB=TBper;
nerit	0:1e09cd7d66b4	203	}
nerit	0:1e09cd7d66b4	204	}
nerit	0:1e09cd7d66b4	205	}
nerit	0:1e09cd7d66b4	206	//*******************************************************
nerit	0:1e09cd7d66b4	207	void seedCorrect(){
nerit	0:1e09cd7d66b4	208	/*
nerit	0:1e09cd7d66b4	209	posError determina la posizione relativa di TB rispetto ad SD
nerit	0:1e09cd7d66b4	210	la reale posizione di SD viene modificata in funzione della velocità per
nerit	0:1e09cd7d66b4	211	traslare la posizione relativa di TB. All'aumentare della velocità la posizione
nerit	0:1e09cd7d66b4	212	di SD viene incrementata così che TB acceleri per raggiungerla in modo da rilasciare il seme prima
nerit	0:1e09cd7d66b4	213	La taratura del sistema avviene determinando prima il valore di angoloFase alla minima velocità,
nerit	0:1e09cd7d66b4	214	poi, alla massima velocità, dovrebbe spostarsi la posizione relativa con una variabile proporzionale alla velocità, ma c'è un però.
nerit	0:1e09cd7d66b4	215	Il problema è che il momento di avvio determina una correzione dell'angolo di partenza del tamburo
nerit	0:1e09cd7d66b4	216	angolo che viene rideterminato ogni volta che il sensore becchi legge un transito.
nerit	0:1e09cd7d66b4	217	Di fatto c'è una concorrenza tra l'angolo di avvio determinato e la correzione di posizione relativa
nerit	0:1e09cd7d66b4	218	del tamburo. E' molto probabile che convenga modificare solo la posizione relativa e non anche l'angolo di avvio
nerit	0:1e09cd7d66b4	219	Ancora di più se viene eliminata la parte gestita da ciclata.
nerit	0:1e09cd7d66b4	220	In questo modo dovrebbe esserci solo un andamento in accelerazione di TB che viene poi eventualmente decelerato
nerit	0:1e09cd7d66b4	221	dal passaggio sul sensore di TB. Funzione corretta perchè il sincronismo tra i sensori genera l'inibizione della correzione
nerit	0:1e09cd7d66b4	222	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	0:1e09cd7d66b4	223	Appena il sensore di TB viene impegnato allora viene abilitato il controllo di fase del tamburo.
nerit	0:1e09cd7d66b4	224	Questo si traduce nel fatto che il controllo di posizione viene gestito solo all'interno di uno slot di semina in modo che
nerit	0:1e09cd7d66b4	225	il tamburo non risenta della condizione di reset della posizione di SD mentre lui è ancora nella fase precedente. Si fermerebbe.
nerit	0:1e09cd7d66b4	226
nerit	0:1e09cd7d66b4	227	// La considerazione finale è che mantenendo l'angolo di avvio fisso e regolato sulla bassa velocità, intervenendo solo sulla correzione
nerit	0:1e09cd7d66b4	228	// di posizione in questa routine, dovrebbe essere possibile seminare correttamente a tutte le velocità regolando solo 2 parametri.
nerit	0:1e09cd7d66b4	229	*/
nerit	0:1e09cd7d66b4	230	/*
nerit	0:1e09cd7d66b4	231	SDsectorStep = (double)fixedStepGiroSD / (double)pickNumber;
nerit	0:1e09cd7d66b4	232	TBsectorStep = (TBmotorSteps*TBreductionRatio)/cellsNumber;
nerit	0:1e09cd7d66b4	233	KcorT = (SDsectorStep/TBsectorStep);
nerit	0:1e09cd7d66b4	234	angoloFase=angoloPh;
nerit	0:1e09cd7d66b4	235	stepGrado=fixedStepGiroSD/360.0f;
nerit	0:1e09cd7d66b4	236	avvioGradi = costante da terminale tritecnica
nerit	0:1e09cd7d66b4	237	TBdeltaStep=(fixedStepGiroSD/pickNumber)-(stepGrado*avvioGradi);
nerit	0:1e09cd7d66b4	238	TBfaseStep = (stepGrado*angoloFase);
nerit	0:1e09cd7d66b4	239	anticipoMax = valore in gradi del massimo anticipo proporzionale alla velocità della ruota di semina
nerit	0:1e09cd7d66b4	240	*/
nerit	0:1e09cd7d66b4	241
nerit	0:1e09cd7d66b4	242	if ((tractorSpeed_MtS_timed>0.01f)){
nerit	0:1e09cd7d66b4	243	if (inhibit==0){
nerit	0:1e09cd7d66b4	244	double posError =0.0f;
nerit	0:1e09cd7d66b4	245	double posSD=((double)SDactualPosition)/KcorT;
nerit	0:1e09cd7d66b4	246	posError = posSD - (double)TBactualPosition;
nerit	0:1e09cd7d66b4	247	// interviene sulla velocità di TB per raggiungere la corretta posizione relativa
nerit	0:1e09cd7d66b4	248	if((lowSpeed==0)&&(aspettaStart==0)){
nerit	0:1e09cd7d66b4	249	if (posError>50.0f){posError=50.0f;}
nerit	0:1e09cd7d66b4	250	if (posError<-50.0f){posError=-50.0f;}
nerit	0:1e09cd7d66b4	251	if ((posError >=1.0f)\|\|(posError<=-1.0f)){
nerit	0:1e09cd7d66b4	252	ePpos = periodo /(1.0f+ ((posError/100.0f)));
nerit	0:1e09cd7d66b4	253	cambiaTB(ePpos);
nerit	0:1e09cd7d66b4	254	}
nerit	0:1e09cd7d66b4	255	}
nerit	0:1e09cd7d66b4	256	}
nerit	0:1e09cd7d66b4	257	#if defined(pcSerial)
nerit	0:1e09cd7d66b4	258	#if defined(SDreset)
nerit	0:1e09cd7d66b4	259	pc.printf("Fase: %d",fase);
nerit	0:1e09cd7d66b4	260	pc.printf(" PrePosSD: %d",prePosSD);
nerit	0:1e09cd7d66b4	261	pc.printf(" PosSD: %d",SDactualPosition);
nerit	0:1e09cd7d66b4	262	pc.printf(" speed: %f",tractorSpeed_MtS_timed);
nerit	0:1e09cd7d66b4	263	pc.printf(" Trigger: %d \n", trigRepos);
nerit	0:1e09cd7d66b4	264	#endif
nerit	0:1e09cd7d66b4	265	#endif
nerit	0:1e09cd7d66b4	266	}
nerit	0:1e09cd7d66b4	267	}
nerit	0:1e09cd7d66b4	268	//*******************************************************
nerit	0:1e09cd7d66b4	269	void videoUpdate(){
nerit	0:1e09cd7d66b4	270	for(int aa=0;aa<4;aa++){speedForDisplay[aa]=speedForDisplay[aa+1];}
nerit	0:1e09cd7d66b4	271	speedForDisplay[4]=tractorSpeed_MtS_timed;
nerit	0:1e09cd7d66b4	272	totalSpeed=0.0f;
nerit	0:1e09cd7d66b4	273	for (int aa=0; aa<5; aa++){totalSpeed += speedForDisplay[aa];}
nerit	0:1e09cd7d66b4	274	totalSpeed = totalSpeed / 5.0f;
nerit	0:1e09cd7d66b4	275	}
nerit	0:1e09cd7d66b4	276	//*******************************************************
nerit	0:1e09cd7d66b4	277	void ciclaTB(){
nerit	0:1e09cd7d66b4	278	if ((startCicloTB==1)&&(cicloTbinCorso==0)){
nerit	0:1e09cd7d66b4	279	TBticker.attach_us(&step_TBPulseOut,TBperiod/2.5f); // clock time are milliseconds and attach seed motor stepper controls
nerit	0:1e09cd7d66b4	280	cicloTbinCorso = 1;
nerit	0:1e09cd7d66b4	281	startCicloTB=0;
nerit	0:1e09cd7d66b4	282	}
nerit	0:1e09cd7d66b4	283	if ((loadDaCan==1)&&(loadDaCanInCorso==0)){
nerit	0:1e09cd7d66b4	284	TBticker.attach_us(&step_TBPulseOut,1000.0f); // clock time are milliseconds and attach seed motor stepper controls
nerit	0:1e09cd7d66b4	285	loadDaCanInCorso=1;
nerit	0:1e09cd7d66b4	286	stopCicloTB=0;
nerit	0:1e09cd7d66b4	287	}
nerit	0:1e09cd7d66b4	288	if ((stopCicloTB==1)&&(TBactualPosition>5)){
nerit	0:1e09cd7d66b4	289	TBticker.detach();
nerit	0:1e09cd7d66b4	290	cicloTbinCorso = 0;
nerit	0:1e09cd7d66b4	291	stopCicloTB=0;
nerit	0:1e09cd7d66b4	292	loadDaCanInCorso=0;
nerit	0:1e09cd7d66b4	293	loadDaCan=0;
nerit	0:1e09cd7d66b4	294	}
nerit	0:1e09cd7d66b4	295	}
nerit	0:1e09cd7d66b4	296	//*******************************************************
nerit	0:1e09cd7d66b4	297	void allarmi(){
nerit	0:1e09cd7d66b4	298	uint8_t alarmLowRegister1=0x00;
nerit	0:1e09cd7d66b4	299	alarmLowRegister=0x00;
nerit	0:1e09cd7d66b4	300	alarmHighRegister=0x80;
nerit	0:1e09cd7d66b4	301
nerit	0:1e09cd7d66b4	302	//alarmLowRegister=alarmLowRegister+(all_semiFiniti*0x01); // manca il sensore
nerit	0:1e09cd7d66b4	303	alarmLowRegister=alarmLowRegister+(all_pickSignal*0x02); // fatto
nerit	0:1e09cd7d66b4	304	alarmLowRegister=alarmLowRegister+(all_cellSignal*0x04); // fatto
nerit	0:1e09cd7d66b4	305	alarmLowRegister=alarmLowRegister+(all_lowBattery*0x08); // fatto
nerit	0:1e09cd7d66b4	306	alarmLowRegister=alarmLowRegister+(all_overCurrDC*0x10); // fatto
nerit	0:1e09cd7d66b4	307	alarmLowRegister=alarmLowRegister+(all_stopSistem*0x20); // verificarne la necessità
nerit	0:1e09cd7d66b4	308	//alarmLowRegister=alarmLowRegister+(all_upElements*0x40); // manca il sensore
nerit	0:1e09cd7d66b4	309	//alarmLowRegister=alarmLowRegister+(all_noSeedOnCe*0x80); // manca il sensore
nerit	0:1e09cd7d66b4	310
nerit	0:1e09cd7d66b4	311	//alarmLowRegister1=alarmLowRegister1+(all_cfgnErrors*0x01); // da scrivere
nerit	0:1e09cd7d66b4	312	alarmLowRegister1=alarmLowRegister1+(all_noDcRotati*0x02); // fatto
nerit	0:1e09cd7d66b4	313	alarmLowRegister1=alarmLowRegister1+(all_noStepRota*0x04); // fatto
nerit	0:1e09cd7d66b4	314	alarmLowRegister1=alarmLowRegister1+(all_speedError*0x08); // fatto
nerit	0:1e09cd7d66b4	315	alarmLowRegister1=alarmLowRegister1+(all_noSpeedSen*0x10); // fatto
nerit	0:1e09cd7d66b4	316	alarmLowRegister1=alarmLowRegister1+(all_no_Zeroing*0x20); // fatto
nerit	0:1e09cd7d66b4	317	alarmLowRegister1=alarmLowRegister1+(all_genericals*0x40);
nerit	0:1e09cd7d66b4	318	if (alarmLowRegister1 > 0){
nerit	0:1e09cd7d66b4	319	alarmHighRegister = 0x81;
nerit	0:1e09cd7d66b4	320	alarmLowRegister = alarmLowRegister1;
nerit	0:1e09cd7d66b4	321	}
nerit	0:1e09cd7d66b4	322
nerit	0:1e09cd7d66b4	323	#if defined(pcSerial)
nerit	0:1e09cd7d66b4	324	#if defined(VediAllarmi)
nerit	0:1e09cd7d66b4	325	if (all_pickSignal==1){pc.printf("AllarmeBecchi\n");}
nerit	0:1e09cd7d66b4	326	if (all_cellSignal==1){pc.printf("AllarmeCelle\n");}
nerit	0:1e09cd7d66b4	327	if (all_lowBattery==1){pc.printf("AllarmeBassaCorrente\n");}
nerit	0:1e09cd7d66b4	328	if (all_overCurrDC==1){pc.printf("AllarmeAltaCorrente\n");}
nerit	0:1e09cd7d66b4	329	if (all_stopSistem==1){pc.printf("AllarmeStop\n");}
nerit	0:1e09cd7d66b4	330	if (all_noDcRotati==1){pc.printf("AllarmeDCnoRotation\n");}
nerit	0:1e09cd7d66b4	331	if (all_noStepRota==1){pc.printf("AllarmeNoStepRotation\n");}
nerit	0:1e09cd7d66b4	332	if (all_speedError==1){pc.printf("AllarmeSpeedError\n");}
nerit	0:1e09cd7d66b4	333	if (all_noSpeedSen==1){pc.printf("AllarmeNoSpeedSensor\n");}
nerit	0:1e09cd7d66b4	334	if (all_no_Zeroing==1){pc.printf("AllarmeNoZero\n");}
nerit	0:1e09cd7d66b4	335	if (all_genericals==1){pc.printf("AllarmeGenerico\n");}
nerit	0:1e09cd7d66b4	336	pc.printf("Code: 0x%x%x\n",alarmHighRegister,alarmLowRegister);
nerit	0:1e09cd7d66b4	337	#endif
nerit	0:1e09cd7d66b4	338	#endif
nerit	0:1e09cd7d66b4	339	all_semiFiniti=0;
nerit	0:1e09cd7d66b4	340	all_pickSignal=0;
nerit	0:1e09cd7d66b4	341	all_cellSignal=0;
nerit	0:1e09cd7d66b4	342	all_lowBattery=0;
nerit	0:1e09cd7d66b4	343	all_overCurrDC=0;
nerit	0:1e09cd7d66b4	344	all_stopSistem=0;
nerit	0:1e09cd7d66b4	345	all_upElements=0;
nerit	0:1e09cd7d66b4	346	all_noSeedOnCe=0;
nerit	0:1e09cd7d66b4	347	all_cfgnErrors=0;
nerit	0:1e09cd7d66b4	348	all_noDcRotati=0;
nerit	0:1e09cd7d66b4	349	all_noStepRota=0;
nerit	0:1e09cd7d66b4	350	all_speedError=0;
nerit	0:1e09cd7d66b4	351	all_noSpeedSen=0;
nerit	0:1e09cd7d66b4	352	all_no_Zeroing=0;
nerit	0:1e09cd7d66b4	353	all_genericals=0;
nerit	0:1e09cd7d66b4	354	}
nerit	0:1e09cd7d66b4	355	//*******************************************************
nerit	0:1e09cd7d66b4	356	#if defined(speedMaster)
nerit	0:1e09cd7d66b4	357	void upDateSincro(){
nerit	0:1e09cd7d66b4	358	char val1[8]={0,0,0,0,0,0,0,0};
nerit	0:1e09cd7d66b4	359	val1[0]=(prePosSD /0x00FF0000)&0x000000FF;
nerit	0:1e09cd7d66b4	360	val1[1]=(prePosSD /0x0000FF00)&0x000000FF;
nerit	0:1e09cd7d66b4	361	val1[2]=(prePosSD /0x000000FF)&0x000000FF;
nerit	0:1e09cd7d66b4	362	val1[3]=prePosSD & 0x000000FF;
nerit	0:1e09cd7d66b4	363	#if defined(canbusActive)
nerit	0:1e09cd7d66b4	364	#if defined(speedMaster)
nerit	0:1e09cd7d66b4	365	if(can1.write(CANMessage(TX_SI, *&val1,8))){
nerit	0:1e09cd7d66b4	366	checkState=0;
nerit	0:1e09cd7d66b4	367	}
nerit	0:1e09cd7d66b4	368	#endif
nerit	0:1e09cd7d66b4	369	#endif
nerit	0:1e09cd7d66b4	370	}
nerit	0:1e09cd7d66b4	371	#endif
nerit	0:1e09cd7d66b4	372	//*******************************************************
nerit	0:1e09cd7d66b4	373	void upDateSpeed(){
nerit	0:1e09cd7d66b4	374	/*
nerit	0:1e09cd7d66b4	375	aggiorna dati OPUSA3
nerit	0:1e09cd7d66b4	376	val1[0] contiene il dato di velocità
nerit	0:1e09cd7d66b4	377	val1[1] contiene il byte basso della tabella allarmi
nerit	0:1e09cd7d66b4	378	uint8_t all_semiFiniti = 0; // semi finiti (generato dal relativo sensore)
nerit	0:1e09cd7d66b4	379	uint8_t all_pickSignal = 0; // errore segnale becchi (generato dal tempo tra un becco ed il successivo)
nerit	0:1e09cd7d66b4	380	uint8_t all_cellSignal = 0; // errore segnale celle (generato dal sensore tamburo )
nerit	0:1e09cd7d66b4	381	uint8_t all_lowBattery = 0; // allarme batteria (valore interno di tritecnica)
nerit	0:1e09cd7d66b4	382	uint8_t all_overCurrDC = 0; // sovracorrente motore DC (generato dal sensore di corrente)
nerit	0:1e09cd7d66b4	383	uint8_t all_stopSistem = 0; // sistema in stop (a tempo con ruota ferma)
nerit	0:1e09cd7d66b4	384	uint8_t all_upElements = 0; // elementi sollevati (generato dal relativo sensore)
nerit	0:1e09cd7d66b4	385	uint8_t all_noSeedOnCe = 0; // fallanza di semina (manca il seme nella cella)
nerit	0:1e09cd7d66b4	386	uint8_t all_cfgnErrors = 0; // errore di configurazione (incongruenza dei parametri impostati)
nerit	0:1e09cd7d66b4	387	uint8_t all_noDcRotati = 0; // ruota di semina bloccata (arriva la velocità ma non i becchi)
nerit	0:1e09cd7d66b4	388	uint8_t all_noStepRota = 0; // tamburo di semina bloccato (comando il tamburo ma non ricevo il sensore)
nerit	0:1e09cd7d66b4	389	uint8_t all_speedError = 0; // errore sensore velocità (tempo impulsi non costante)
nerit	0:1e09cd7d66b4	390	uint8_t all_noSpeedSen = 0; // mancanza segnale di velocità (sento i becchi ma non la velocita)
nerit	0:1e09cd7d66b4	391	uint8_t all_no_Zeroing = 0; // mancato azzeramento sistema (generato dal timeout del comando motore DC)
nerit	0:1e09cd7d66b4	392	uint8_t all_genericals = 0; // allarme generico
nerit	0:1e09cd7d66b4	393	val1[2] contiene il byte alto della tabella di allarmi
nerit	0:1e09cd7d66b4	394	val1[3] contiene i segnali per la diagnostica
nerit	0:1e09cd7d66b4	395	bit 0= sensore ruota fonica
nerit	0:1e09cd7d66b4	396	bit 1= sensore celle
nerit	0:1e09cd7d66b4	397	bit 2= sensore posizione
nerit	0:1e09cd7d66b4	398	bit 3= sensore becchi
nerit	0:1e09cd7d66b4	399	bit 4= motore DC
nerit	0:1e09cd7d66b4	400	bit 5= controller
nerit	0:1e09cd7d66b4	401	bit 6= motore stepper
nerit	0:1e09cd7d66b4	402	*/
nerit	0:1e09cd7d66b4	403	char val1[8]={0,0,0,0,0,0,0,0};
nerit	0:1e09cd7d66b4	404
nerit	0:1e09cd7d66b4	405	val1[3]=0;
nerit	0:1e09cd7d66b4	406	val1[3]=val1[3]+(tractorSpeedRead*0x01);
nerit	0:1e09cd7d66b4	407	val1[3]=val1[3]+(TBzeroPinInputRev*0x02);
nerit	0:1e09cd7d66b4	408	val1[3]=val1[3]+(ElementPosition*0x04);
nerit	0:1e09cd7d66b4	409	val1[3]=val1[3]+(seedWheelZeroPinInputRev*0x08);
nerit	0:1e09cd7d66b4	410	#if defined(simulaPerCan)
nerit	0:1e09cd7d66b4	411	if (buttonUser==0){
nerit	0:1e09cd7d66b4	412	val1[1]=0x02;
nerit	0:1e09cd7d66b4	413	}else{
nerit	0:1e09cd7d66b4	414	val1[1]=0x00;
nerit	0:1e09cd7d66b4	415	}
nerit	0:1e09cd7d66b4	416	val1[3]=valore;
nerit	0:1e09cd7d66b4	417	valore+=1;
nerit	0:1e09cd7d66b4	418	if(valore>50){
nerit	0:1e09cd7d66b4	419	valore=0;
nerit	0:1e09cd7d66b4	420	}
nerit	0:1e09cd7d66b4	421	tractorSpeed_MtS_timed=valore;
nerit	0:1e09cd7d66b4	422	#endif
nerit	0:1e09cd7d66b4	423	allarmi();
nerit	0:1e09cd7d66b4	424	valore = totalSpeed36.0f; // tractorSpeed_MtS_timed36.0f;
nerit	0:1e09cd7d66b4	425	val1[0]=valore;
nerit	0:1e09cd7d66b4	426	val1[1]=alarmHighRegister; // registro alto allarmi. Parte sempre da 0x80
nerit	0:1e09cd7d66b4	427	val1[2]=alarmLowRegister; // registro basso allarmi
nerit	0:1e09cd7d66b4	428	//val1[3]=val1[3];// registro di stato degli ingressi
nerit	0:1e09cd7d66b4	429	val1[4]=resetComandi;
nerit	0:1e09cd7d66b4	430	val1[5]=cellsCounterLow;
nerit	0:1e09cd7d66b4	431	val1[6]=cellsCounterHig;
nerit	0:1e09cd7d66b4	432	#if defined(canbusActive)
nerit	0:1e09cd7d66b4	433	if(can1.write(CANMessage(TX_ID, *&val1,8))){
nerit	0:1e09cd7d66b4	434	checkState=0;
nerit	0:1e09cd7d66b4	435	}
nerit	0:1e09cd7d66b4	436	#endif
nerit	0:1e09cd7d66b4	437	}
nerit	0:1e09cd7d66b4	438	//*******************************************************
nerit	0:1e09cd7d66b4	439	void leggiCAN(){
nerit	0:1e09cd7d66b4	440	#if defined(canbusActive)
nerit	0:1e09cd7d66b4	441	if(can1.read(rxMsg)) {
nerit	0:1e09cd7d66b4	442	if (firstStart==1){
nerit	0:1e09cd7d66b4	443	#if defined(speedMaster)
nerit	0:1e09cd7d66b4	444	sincUpdate.attach(&upDateSincro,0.01f);
nerit	0:1e09cd7d66b4	445	#endif
nerit	0:1e09cd7d66b4	446	canUpdate.attach(&upDateSpeed,0.11f);
nerit	0:1e09cd7d66b4	447	firstStart=0;
nerit	0:1e09cd7d66b4	448	}
nerit	0:1e09cd7d66b4	449
nerit	0:1e09cd7d66b4	450	if (rxMsg.id==RX_ID){
nerit	0:1e09cd7d66b4	451	#if defined(pcSerial)
nerit	0:1e09cd7d66b4	452	#if defined(canDataReceived)
nerit	0:1e09cd7d66b4	453	pc.printf("Messaggio ricevuto\n");
nerit	0:1e09cd7d66b4	454	#endif
nerit	0:1e09cd7d66b4	455	#endif
nerit	0:1e09cd7d66b4	456	}
nerit	0:1e09cd7d66b4	457	if (rxMsg.id==RX_Broadcast){
nerit	0:1e09cd7d66b4	458	#if defined(pcSerial)
nerit	0:1e09cd7d66b4	459	#if defined(canDataReceived)
nerit	0:1e09cd7d66b4	460	pc.printf("BroadCast ricevuto\n");
nerit	0:1e09cd7d66b4	461	#endif
nerit	0:1e09cd7d66b4	462	#endif
nerit	0:1e09cd7d66b4	463	enableSimula= rxMsg.data[0];
nerit	0:1e09cd7d66b4	464	speedSimula = rxMsg.data[1];
nerit	0:1e09cd7d66b4	465	avviaSimula = rxMsg.data[2];
nerit	0:1e09cd7d66b4	466	selezionato = rxMsg.data[3];
nerit	0:1e09cd7d66b4	467	comandiDaCan = rxMsg.data[4];
nerit	0:1e09cd7d66b4	468	#if defined(pcSerial)
nerit	0:1e09cd7d66b4	469	#if defined(canDataReceived)
nerit	0:1e09cd7d66b4	470	pc.printf("Comandi: %x\n",comandiDaCan);
nerit	0:1e09cd7d66b4	471	#endif
nerit	0:1e09cd7d66b4	472	#endif
nerit	0:1e09cd7d66b4	473	switch (comandiDaCan){
nerit	0:1e09cd7d66b4	474	case 1:
nerit	0:1e09cd7d66b4	475	case 3:
nerit	0:1e09cd7d66b4	476	azzeraDaCan=1;
nerit	0:1e09cd7d66b4	477	resetComandi=0x01;
nerit	0:1e09cd7d66b4	478	comandiDaCan=0;
nerit	0:1e09cd7d66b4	479	break;
nerit	0:1e09cd7d66b4	480	case 2:
nerit	0:1e09cd7d66b4	481	loadDaCan=1;
nerit	0:1e09cd7d66b4	482	resetComandi=0x02;
nerit	0:1e09cd7d66b4	483	comandiDaCan=0;
nerit	0:1e09cd7d66b4	484	break;
nerit	0:1e09cd7d66b4	485	default:
nerit	0:1e09cd7d66b4	486	comandiDaCan=0;
nerit	0:1e09cd7d66b4	487	resetComandi=0xFF;
nerit	0:1e09cd7d66b4	488	break;
nerit	0:1e09cd7d66b4	489	}
nerit	0:1e09cd7d66b4	490	#if defined(pcSerial)
nerit	0:1e09cd7d66b4	491	#if defined(canDataReceived)
nerit	0:1e09cd7d66b4	492	pc.printf("Comandi: %x\n",resetComandi);
nerit	0:1e09cd7d66b4	493	#endif
nerit	0:1e09cd7d66b4	494	#endif
nerit	0:1e09cd7d66b4	495	if (speedSimula>45){
nerit	0:1e09cd7d66b4	496	speedSimula=45;
nerit	0:1e09cd7d66b4	497	}
nerit	0:1e09cd7d66b4	498	// modulo 1
nerit	0:1e09cd7d66b4	499	if (RX_ID==0x100){
nerit	0:1e09cd7d66b4	500	if ((selezionato&0x01)==0x01){
nerit	0:1e09cd7d66b4	501	simOk=1;
nerit	0:1e09cd7d66b4	502	}else{
nerit	0:1e09cd7d66b4	503	simOk=0;
nerit	0:1e09cd7d66b4	504	}
nerit	0:1e09cd7d66b4	505	}
nerit	0:1e09cd7d66b4	506	// modulo 2
nerit	0:1e09cd7d66b4	507	if (RX_ID==0x102){
nerit	0:1e09cd7d66b4	508	if ((selezionato&0x02)==0x02){
nerit	0:1e09cd7d66b4	509	simOk=1;
nerit	0:1e09cd7d66b4	510	}else{
nerit	0:1e09cd7d66b4	511	simOk=0;
nerit	0:1e09cd7d66b4	512	}
nerit	0:1e09cd7d66b4	513	}
nerit	0:1e09cd7d66b4	514	// modulo 3
nerit	0:1e09cd7d66b4	515	if (RX_ID==0x104){
nerit	0:1e09cd7d66b4	516	if ((selezionato&0x04)==0x04){
nerit	0:1e09cd7d66b4	517	simOk=1;
nerit	0:1e09cd7d66b4	518	}else{
nerit	0:1e09cd7d66b4	519	simOk=0;
nerit	0:1e09cd7d66b4	520	}
nerit	0:1e09cd7d66b4	521	}
nerit	0:1e09cd7d66b4	522	// modulo 4
nerit	0:1e09cd7d66b4	523	if (RX_ID==0x106){
nerit	0:1e09cd7d66b4	524	if ((selezionato&0x08)==0x08){
nerit	0:1e09cd7d66b4	525	simOk=1;
nerit	0:1e09cd7d66b4	526	}else{
nerit	0:1e09cd7d66b4	527	simOk=0;
nerit	0:1e09cd7d66b4	528	}
nerit	0:1e09cd7d66b4	529	}
nerit	0:1e09cd7d66b4	530	// modulo 5
nerit	0:1e09cd7d66b4	531	if (RX_ID==0x108){
nerit	0:1e09cd7d66b4	532	if ((selezionato&0x10)==0x10){
nerit	0:1e09cd7d66b4	533	simOk=1;
nerit	0:1e09cd7d66b4	534	}else{
nerit	0:1e09cd7d66b4	535	simOk=0;
nerit	0:1e09cd7d66b4	536	}
nerit	0:1e09cd7d66b4	537	}
nerit	0:1e09cd7d66b4	538
nerit	0:1e09cd7d66b4	539	}
nerit	0:1e09cd7d66b4	540	if (rxMsg.id==RX_Settings){
nerit	0:1e09cd7d66b4	541	if (tractorSpeed_MtS_timed==0.0f){
nerit	0:1e09cd7d66b4	542	pickNumber = rxMsg.data[0]; // numero di becchi installati sulla ruota di semina
nerit	0:1e09cd7d66b4	543	cellsNumber = rxMsg.data[1]; // numero di celle del tamburo
nerit	0:1e09cd7d66b4	544	deepOfSeed=(rxMsg.data[2]/10000.0f); // deep of seeding
nerit	0:1e09cd7d66b4	545	seedWheelDiameter = ((rxMsg.data[4]*0xFF)+rxMsg.data[3])/10000.0f; // seed wheel diameter setting
nerit	0:1e09cd7d66b4	546	speedWheelDiameter = ((rxMsg.data[6]*0xFF)+rxMsg.data[5])/10000.0f; // variable for tractor speed calculation (need to be set from UI) ( Unit= meters )
nerit	0:1e09cd7d66b4	547	speedWheelPulse = rxMsg.data[7]; // variable which define the number of pulse each turn of tractor speed wheel (need to be set from UI)
nerit	0:1e09cd7d66b4	548	aggiornaParametri();
nerit	0:1e09cd7d66b4	549	#if defined(pcSerial)
nerit	0:1e09cd7d66b4	550	#if defined(canDataReceived)
nerit	0:1e09cd7d66b4	551	pc.printf("Seed wheel diameter %f \n",seedWheelDiameter);
nerit	0:1e09cd7d66b4	552	pc.printf("Speed wheel diameter %f \n",speedWheelDiameter);
nerit	0:1e09cd7d66b4	553	#endif
nerit	0:1e09cd7d66b4	554	#endif
nerit	0:1e09cd7d66b4	555	}
nerit	0:1e09cd7d66b4	556	}
nerit	0:1e09cd7d66b4	557	if (rxMsg.id==RX_AngoloPh){
nerit	0:1e09cd7d66b4	558	if (tractorSpeed_MtS_timed==0.0f){
nerit	0:1e09cd7d66b4	559	#if defined(M1)
nerit	0:1e09cd7d66b4	560	angoloPh = (double) rxMsg.data[0] ;
nerit	0:1e09cd7d66b4	561	aggiornaParametri();
nerit	0:1e09cd7d66b4	562	#endif
nerit	0:1e09cd7d66b4	563	#if defined(M2)
nerit	0:1e09cd7d66b4	564	angoloPh = (double) rxMsg.data[1] ;
nerit	0:1e09cd7d66b4	565	aggiornaParametri();
nerit	0:1e09cd7d66b4	566	#endif
nerit	0:1e09cd7d66b4	567	#if defined(M3)
nerit	0:1e09cd7d66b4	568	angoloPh = (double) rxMsg.data[2] ;
nerit	0:1e09cd7d66b4	569	aggiornaParametri();
nerit	0:1e09cd7d66b4	570	#endif
nerit	0:1e09cd7d66b4	571	#if defined(M4)
nerit	0:1e09cd7d66b4	572	angoloPh = (double) rxMsg.data[3] ;
nerit	0:1e09cd7d66b4	573	aggiornaParametri();
nerit	0:1e09cd7d66b4	574	#endif
nerit	0:1e09cd7d66b4	575	#if defined(M5)
nerit	0:1e09cd7d66b4	576	angoloPh = (double) rxMsg.data[4] ;
nerit	0:1e09cd7d66b4	577	aggiornaParametri();
nerit	0:1e09cd7d66b4	578	#endif
nerit	0:1e09cd7d66b4	579	#if defined(M6)
nerit	0:1e09cd7d66b4	580	angoloPh = (double) rxMsg.data[5] ;
nerit	0:1e09cd7d66b4	581	aggiornaParametri();
nerit	0:1e09cd7d66b4	582	#endif
nerit	0:1e09cd7d66b4	583	}
nerit	0:1e09cd7d66b4	584	}
nerit	0:1e09cd7d66b4	585	if (rxMsg.id==RX_AngoloAv){
nerit	0:1e09cd7d66b4	586	if (tractorSpeed_MtS_timed==0.0f){
nerit	0:1e09cd7d66b4	587	#if defined(M1)
nerit	0:1e09cd7d66b4	588	angoloAv = (double) rxMsg.data[0] ;
nerit	0:1e09cd7d66b4	589	aggiornaParametri();
nerit	0:1e09cd7d66b4	590	#endif
nerit	0:1e09cd7d66b4	591	#if defined(M2)
nerit	0:1e09cd7d66b4	592	angoloAv = (double) rxMsg.data[1] ;
nerit	0:1e09cd7d66b4	593	aggiornaParametri();
nerit	0:1e09cd7d66b4	594	#endif
nerit	0:1e09cd7d66b4	595	#if defined(M3)
nerit	0:1e09cd7d66b4	596	angoloAv = (double) rxMsg.data[2] ;
nerit	0:1e09cd7d66b4	597	aggiornaParametri();
nerit	0:1e09cd7d66b4	598	#endif
nerit	0:1e09cd7d66b4	599	#if defined(M4)
nerit	0:1e09cd7d66b4	600	angoloAv = (double) rxMsg.data[3] ;
nerit	0:1e09cd7d66b4	601	aggiornaParametri();
nerit	0:1e09cd7d66b4	602	#endif
nerit	0:1e09cd7d66b4	603	#if defined(M5)
nerit	0:1e09cd7d66b4	604	angoloAv = (double) rxMsg.data[4] ;
nerit	0:1e09cd7d66b4	605	aggiornaParametri();
nerit	0:1e09cd7d66b4	606	#endif
nerit	0:1e09cd7d66b4	607	#if defined(M6)
nerit	0:1e09cd7d66b4	608	angoloAv = (double) rxMsg.data[5] ;
nerit	0:1e09cd7d66b4	609	aggiornaParametri();
nerit	0:1e09cd7d66b4	610	#endif
nerit	0:1e09cd7d66b4	611	}
nerit	0:1e09cd7d66b4	612	}
nerit	0:1e09cd7d66b4	613	if (rxMsg.id==RX_Quinconce){
nerit	0:1e09cd7d66b4	614	if (tractorSpeed_MtS_timed==0.0f){
nerit	0:1e09cd7d66b4	615	quinconceActive = (uint8_t) rxMsg.data[0];
nerit	0:1e09cd7d66b4	616	aggiornaParametri();
nerit	0:1e09cd7d66b4	617	}
nerit	0:1e09cd7d66b4	618	}
nerit	0:1e09cd7d66b4	619	if (rxMsg.id==RX_QuincSinc){
nerit	0:1e09cd7d66b4	620	masterSinc = (uint32_t) rxMsg.data[0] * 0x00FF0000;
nerit	0:1e09cd7d66b4	621	masterSinc = masterSinc + ((uint32_t) rxMsg.data[1] * 0x0000FF00);
nerit	0:1e09cd7d66b4	622	masterSinc = masterSinc + ((uint32_t) rxMsg.data[2] * 0x000000FF);
nerit	0:1e09cd7d66b4	623	masterSinc = masterSinc + ((uint32_t) rxMsg.data[3]);
nerit	0:1e09cd7d66b4	624	quincVerify=1;
nerit	0:1e09cd7d66b4	625	//pc.printf("pippo");
nerit	0:1e09cd7d66b4	626	}
nerit	0:1e09cd7d66b4	627	}
nerit	0:1e09cd7d66b4	628	#endif
nerit	0:1e09cd7d66b4	629	#if defined(overWriteCanSimulation)
nerit	0:1e09cd7d66b4	630	enableSimula=1;
nerit	0:1e09cd7d66b4	631	speedSimula=25;
nerit	0:1e09cd7d66b4	632	avviaSimula=1;
nerit	0:1e09cd7d66b4	633	simOk=1;
nerit	0:1e09cd7d66b4	634	#endif
nerit	0:1e09cd7d66b4	635
nerit	0:1e09cd7d66b4	636	}
nerit	0:1e09cd7d66b4	637
nerit	0:1e09cd7d66b4	638	//*******************************************************
nerit	0:1e09cd7d66b4	639	void DC_CheckCurrent(){
nerit	0:1e09cd7d66b4	640
nerit	0:1e09cd7d66b4	641	// TODO: tabella di riferimento assorbimenti alle varie velocità al fine di gestire
nerit	0:1e09cd7d66b4	642	// gli allarmi e le correzioni di velocità
nerit	0:1e09cd7d66b4	643
nerit	0:1e09cd7d66b4	644	//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	0:1e09cd7d66b4	645	//int SD_analogIndex[10]={0,0,0,0,0,0,0,0,0,0};
nerit	0:1e09cd7d66b4	646	// Analog reading
nerit	0:1e09cd7d66b4	647	//number = floor(number * 100) / 100;
nerit	0:1e09cd7d66b4	648	//if (pwmCheck==1){
nerit	0:1e09cd7d66b4	649	timeout.detach();
nerit	0:1e09cd7d66b4	650	for (int ii=1;ii<20;ii++){
nerit	0:1e09cd7d66b4	651	SD_analogMatrix[ii]=SD_analogMatrix[ii+1];
nerit	0:1e09cd7d66b4	652	}
nerit	0:1e09cd7d66b4	653	SD_CurrentAnalog = floor(SDcurrent.read()*100)/100; // valore in ingresso compreso tra 0.00 e 1.00
nerit	0:1e09cd7d66b4	654	SD_analogMatrix[20]=SD_CurrentAnalog;
nerit	0:1e09cd7d66b4	655	if (SDmotorPWM==0.0f){
nerit	0:1e09cd7d66b4	656	SD_CurrentStart=SD_CurrentAnalog;
nerit	0:1e09cd7d66b4	657	}
nerit	0:1e09cd7d66b4	658	float sommaTutto=0.0f;
nerit	0:1e09cd7d66b4	659	for (int ii=1;ii<21;ii++){
nerit	0:1e09cd7d66b4	660	sommaTutto=sommaTutto+SD_analogMatrix[ii];
nerit	0:1e09cd7d66b4	661	}
nerit	0:1e09cd7d66b4	662	float SD_CurrentAnalogica=sommaTutto/20.0f;
nerit	0:1e09cd7d66b4	663	SD_CurrentScaled = floor(( (SD_CurrentAnalogica - SD_CurrentStart)3.3f) / (SD_CurrentFactor/1000.0f)10)/10;
nerit	0:1e09cd7d66b4	664	#if defined(pcSerial)
nerit	0:1e09cd7d66b4	665	#if defined(correnteAssorbita)
nerit	0:1e09cd7d66b4	666	pc.printf("CorrenteStart: %f ",SD_CurrentStart);
nerit	0:1e09cd7d66b4	667	pc.printf(" CorrenteScaled: %f ",SD_CurrentScaled);
nerit	0:1e09cd7d66b4	668	pc.printf(" CorrenteMedia: %f \n",SD_CurrentAnalogica);
nerit	0:1e09cd7d66b4	669	#endif
nerit	0:1e09cd7d66b4	670	#endif
nerit	0:1e09cd7d66b4	671	reduceCurrent=false;
nerit	0:1e09cd7d66b4	672	incrementCurrent=false;
nerit	0:1e09cd7d66b4	673	/*
nerit	0:1e09cd7d66b4	674	if (SD_CurrentScaled < 3.0f){
nerit	0:1e09cd7d66b4	675	#if defined(canbusActive)
nerit	0:1e09cd7d66b4	676	all_lowBattery=1;
nerit	0:1e09cd7d66b4	677	#endif
nerit	0:1e09cd7d66b4	678	incrementCurrent=true;
nerit	0:1e09cd7d66b4	679	}
nerit	0:1e09cd7d66b4	680	*/
nerit	0:1e09cd7d66b4	681	if (SD_CurrentScaled > 13.0f){
nerit	0:1e09cd7d66b4	682	#if defined(canbusActive)
nerit	0:1e09cd7d66b4	683	all_overCurrDC=1;
nerit	0:1e09cd7d66b4	684	#endif
nerit	0:1e09cd7d66b4	685	reduceCurrent=true;
nerit	0:1e09cd7d66b4	686	}
nerit	0:1e09cd7d66b4	687	//}
nerit	0:1e09cd7d66b4	688	}
nerit	0:1e09cd7d66b4	689	//*******************************************************
nerit	0:1e09cd7d66b4	690	void DC_prepare(){
nerit	0:1e09cd7d66b4	691	// direction or brake preparation
nerit	0:1e09cd7d66b4	692	if (DC_brake==1){SDmotorInA=1;SDmotorInB=1;}else{if (DC_forward==1){SDmotorInA=1;SDmotorInB=0;}else{SDmotorInA=0;SDmotorInB=1;}}
nerit	0:1e09cd7d66b4	693	// fault reading
nerit	0:1e09cd7d66b4	694	if (SDmotorInA==1){SD_faultA=1;}else{SD_faultA=0;}
nerit	0:1e09cd7d66b4	695	if (SDmotorInB==1){SD_faultB=1;}else{SD_faultB=0;}
nerit	0:1e09cd7d66b4	696	}
nerit	0:1e09cd7d66b4	697	//*******************************************************
nerit	0:1e09cd7d66b4	698	void startDelay(){
nerit	0:1e09cd7d66b4	699	int ritardo =0;
nerit	0:1e09cd7d66b4	700	ritardo = (int)((float)(dcActualDuty*800.0f));
nerit	0:1e09cd7d66b4	701	timeout.attach_us(&DC_CheckCurrent,ritardo);
nerit	0:1e09cd7d66b4	702	}
nerit	0:1e09cd7d66b4	703	//*******************************************************
nerit	0:1e09cd7d66b4	704	void quincTrigga(){
nerit	0:1e09cd7d66b4	705	if (quinconceIn==1){
nerit	0:1e09cd7d66b4	706	quincClock=true;
nerit	0:1e09cd7d66b4	707	}else{
nerit	0:1e09cd7d66b4	708	quincClock=false;
nerit	0:1e09cd7d66b4	709	}
nerit	0:1e09cd7d66b4	710	}
nerit	0:1e09cd7d66b4	711	//*******************************************************
nerit	0:1e09cd7d66b4	712	void quinCalc(double parametro){
nerit	1:e88bf5011af6	713	//double riferimento=tempoTraBecchi_mS/parametro;
nerit	0:1e09cd7d66b4	714	// riceve l'impulso di sincro dal master e fa partire il timer di verifica dell'errore
nerit	0:1e09cd7d66b4	715	if (quinconceActive==0){
nerit	0:1e09cd7d66b4	716	if ((quincClock==true)&&(oldQuincIn==0)){
nerit	0:1e09cd7d66b4	717	oldQuincIn=1;
nerit	1:e88bf5011af6	718	oldQuincTimeSD = (double) quincTimeSD.read_ms();
nerit	0:1e09cd7d66b4	719	if (trigSlave==false){
nerit	0:1e09cd7d66b4	720	quincInibit=true;
nerit	0:1e09cd7d66b4	721	trigDaMaster=true;
nerit	0:1e09cd7d66b4	722	}else{
nerit	0:1e09cd7d66b4	723	quincInibit=false;
nerit	0:1e09cd7d66b4	724	trigDaMaster=false;
nerit	0:1e09cd7d66b4	725	trigSlave=false;
nerit	0:1e09cd7d66b4	726	}
nerit	0:1e09cd7d66b4	727	if (quincStart==0){
nerit	0:1e09cd7d66b4	728	quincTime.reset();
nerit	0:1e09cd7d66b4	729	quincStart=1;
nerit	0:1e09cd7d66b4	730	}
nerit	0:1e09cd7d66b4	731	}
nerit	0:1e09cd7d66b4	732	if( quincClock==false){
nerit	0:1e09cd7d66b4	733	oldQuincIn=0;
nerit	0:1e09cd7d66b4	734	}
nerit	0:1e09cd7d66b4	735	}else{
nerit	0:1e09cd7d66b4	736	if ((quincClock==false)&&(oldQuincIn==0)){
nerit	0:1e09cd7d66b4	737	oldQuincIn=1;
nerit	1:e88bf5011af6	738	oldQuincTimeSD = (double) quincTimeSD.read_ms();
nerit	0:1e09cd7d66b4	739	if (trigSlave==false){
nerit	0:1e09cd7d66b4	740	quincInibit=true;
nerit	0:1e09cd7d66b4	741	trigDaMaster=true;
nerit	0:1e09cd7d66b4	742	}else{
nerit	0:1e09cd7d66b4	743	quincInibit=false;
nerit	0:1e09cd7d66b4	744	trigDaMaster=false;
nerit	0:1e09cd7d66b4	745	trigSlave=false;
nerit	0:1e09cd7d66b4	746	}
nerit	0:1e09cd7d66b4	747	if (quincStart==0){
nerit	0:1e09cd7d66b4	748	quincTime.reset();
nerit	0:1e09cd7d66b4	749	quincStart=1;
nerit	0:1e09cd7d66b4	750	}
nerit	0:1e09cd7d66b4	751	}
nerit	0:1e09cd7d66b4	752	if( quincClock==true){
nerit	0:1e09cd7d66b4	753	oldQuincIn=0;
nerit	0:1e09cd7d66b4	754	}
nerit	0:1e09cd7d66b4	755	}
nerit	0:1e09cd7d66b4	756	// riceve l'impulso di passaggio del becco locale e determina il valore dell'errore
nerit	0:1e09cd7d66b4	757	// controllo di linea e di quinconcio
nerit	0:1e09cd7d66b4	758	if ((quinconceActive==0)\|\|(quinconceActive==1)){
nerit	0:1e09cd7d66b4	759	if ((quincStart==1)&&(SDzeroDebounced==1)){
nerit	1:e88bf5011af6	760	if (quincMemoEnable==false){
nerit	0:1e09cd7d66b4	761	memoDuty=correzione;
nerit	1:e88bf5011af6	762	oldCorrezione=correzione;
nerit	1:e88bf5011af6	763	//quincMemoEnable=true;
nerit	1:e88bf5011af6	764	}
nerit	1:e88bf5011af6	765	quincStart=0;
nerit	1:e88bf5011af6	766	percento=1.0f;
nerit	1:e88bf5011af6	767	scostamento = (double)quincTime.read_ms();
nerit	1:e88bf5011af6	768	scostamento2 = oldQuincTimeSD;
nerit	1:e88bf5011af6	769	/*if ((scostamento <= scostamento2)&&(scostamento>10.0f)&&(scostamento <200.0f)){
nerit	1:e88bf5011af6	770	quincError = scostamento;
nerit	1:e88bf5011af6	771	percento = 1.0f + (abs(quincError)/100.0f);
nerit	0:1e09cd7d66b4	772	}
nerit	1:e88bf5011af6	773	if ((scostamento > scostamento2)&&(scostamento2>10.0f)&&(scostamento2 <200.0f)){
nerit	1:e88bf5011af6	774	quincError = scostamento2;
nerit	1:e88bf5011af6	775	percento = 1.0f - (abs(quincError)/100.0f);
nerit	1:e88bf5011af6	776	}*/
nerit	1:e88bf5011af6	777	double pippo = (tractorSpeed_MtS_timed/1.25f)*0.05f;
nerit	1:e88bf5011af6	778	double halfPippo= tempoTraBecchi_mS/2.0f;
nerit	1:e88bf5011af6	779	if ((scostamento > 5.0f)&&(scostamento<halfPippo)){
nerit	1:e88bf5011af6	780	percento = 1.0f-pippo;
nerit	1:e88bf5011af6	781	}
nerit	1:e88bf5011af6	782	if ((scostamento2 > 5.0f)&&(scostamento2<halfPippo)){
nerit	1:e88bf5011af6	783	percento = 1.0f+pippo;
nerit	1:e88bf5011af6	784	}
nerit	1:e88bf5011af6	785	if (abs(percento) > 1.15f){percento=1.15f;}
nerit	1:e88bf5011af6	786	if (abs(percento) < 0.80f){percento=0.80f;}
nerit	1:e88bf5011af6	787	#if defined(pcSerial)
nerit	1:e88bf5011af6	788	#if defined(Qnc)
nerit	1:e88bf5011af6	789	pc.printf(" s1: %f", scostamento);
nerit	1:e88bf5011af6	790	pc.printf(" s2: %f", scostamento2);
nerit	1:e88bf5011af6	791	pc.printf(" er: %f", quincError);
nerit	1:e88bf5011af6	792	pc.printf(" pc: %f", percento);
nerit	1:e88bf5011af6	793	pc.printf(" cr: %f\n",correzione);
nerit	1:e88bf5011af6	794	#endif
nerit	1:e88bf5011af6	795	#endif
nerit	0:1e09cd7d66b4	796	}
nerit	1:e88bf5011af6	797	/*if ((abs(percento) != 1.0f)&&(abs(percento) > 0.0f)){
nerit	0:1e09cd7d66b4	798	correzione = memoDuty* abs(percento);
nerit	0:1e09cd7d66b4	799	correggiSD=true;
nerit	1:e88bf5011af6	800	cntQuinc=0;
nerit	0:1e09cd7d66b4	801	}else{
nerit	0:1e09cd7d66b4	802	correzione = memoDuty;
nerit	0:1e09cd7d66b4	803	correggiSD=false;
nerit	1:e88bf5011af6	804	if (cntQuinc>=2){
nerit	1:e88bf5011af6	805	quincMemoEnable=false;
nerit	1:e88bf5011af6	806	}
nerit	1:e88bf5011af6	807	}*/
nerit	0:1e09cd7d66b4	808	}
nerit	0:1e09cd7d66b4	809	/*
nerit	0:1e09cd7d66b4	810	// gestione con confronto encoder
nerit	0:1e09cd7d66b4	811	// serve l'encoder fisico sugli elementi slave perchè altrimenti la ruota cambia velocità ma non cambia
nerit	0:1e09cd7d66b4	812	// il clock dell'encoder virtuale e quindi l'errore non può essere rilevato correttamente
nerit	0:1e09cd7d66b4	813	if ((quinconceActive==0)&&(quincEnable==1)){
nerit	0:1e09cd7d66b4	814	// da encoder master
nerit	0:1e09cd7d66b4	815	if (quincVerify==1){
nerit	0:1e09cd7d66b4	816	quincVerify=0;
nerit	0:1e09cd7d66b4	817	quincStart=0;
nerit	0:1e09cd7d66b4	818	memoDuty=dcActualDuty;
nerit	0:1e09cd7d66b4	819	if (quincInibit==false){
nerit	0:1e09cd7d66b4	820	percento=0.0f;
nerit	0:1e09cd7d66b4	821	quincError = 0.0f;
nerit	0:1e09cd7d66b4	822	if (masterSinc > prePosSD){
nerit	0:1e09cd7d66b4	823	quincError = masterSinc - prePosSD;
nerit	0:1e09cd7d66b4	824	percento = 1.0f + ((abs(quincError)/SDsectorStep));
nerit	0:1e09cd7d66b4	825	}
nerit	0:1e09cd7d66b4	826	if (masterSinc < prePosSD){
nerit	0:1e09cd7d66b4	827	quincError = prePosSD -masterSinc;
nerit	0:1e09cd7d66b4	828	percento = 1.0f - ((abs(quincError)/SDsectorStep));
nerit	0:1e09cd7d66b4	829	}
nerit	0:1e09cd7d66b4	830	// se quinconce richiesto deve andare al valore di riferimento
nerit	0:1e09cd7d66b4	831	if (abs(percento) > 1.55f){percento=1.55f;}
nerit	0:1e09cd7d66b4	832	if (abs(percento) < 0.40f){percento=0.40f;}
nerit	0:1e09cd7d66b4	833	if (abs(percento) >0.0f){
nerit	0:1e09cd7d66b4	834	if (abs(percento)>1.0f){
nerit	0:1e09cd7d66b4	835	correzione=correzione+(abs(percento)*0.01f);
nerit	0:1e09cd7d66b4	836	}else{
nerit	0:1e09cd7d66b4	837	correzione=correzione-(abs(percento)*0.01f);
nerit	0:1e09cd7d66b4	838	}
nerit	0:1e09cd7d66b4	839	//dcActualDuty = memoDuty * abs(percento);
nerit	0:1e09cd7d66b4	840	}else{
nerit	0:1e09cd7d66b4	841	correzione=correzione;
nerit	0:1e09cd7d66b4	842	//dcActualDuty = memoDuty;
nerit	0:1e09cd7d66b4	843	}
nerit	0:1e09cd7d66b4	844	}
nerit	0:1e09cd7d66b4	845	#if defined(pcSerial)
nerit	0:1e09cd7d66b4	846	#if defined(Qnc)
nerit	0:1e09cd7d66b4	847	pc.printf(" trigMaster: %d", trigDaMaster);
nerit	0:1e09cd7d66b4	848	pc.printf(" trigSlave: %d", trigSlave);
nerit	0:1e09cd7d66b4	849	pc.printf(" quincinibit: %d", quincInibit);
nerit	0:1e09cd7d66b4	850	pc.printf(" masterSinc: %d", masterSinc);
nerit	0:1e09cd7d66b4	851	pc.printf(" prePosSD: %d",prePosSD);
nerit	0:1e09cd7d66b4	852	pc.printf(" percento: %f",percento);
nerit	0:1e09cd7d66b4	853	pc.printf(" correzione: %f\n",correzione);
nerit	0:1e09cd7d66b4	854	#endif
nerit	0:1e09cd7d66b4	855	#endif
nerit	0:1e09cd7d66b4	856	}
nerit	0:1e09cd7d66b4	857	}
nerit	0:1e09cd7d66b4	858	*/
nerit	0:1e09cd7d66b4	859	}
nerit	0:1e09cd7d66b4	860	// ------------------------------------------------------------------------------------------------------------------------------------------------------------------
nerit	0:1e09cd7d66b4	861	// MAIN SECTION
nerit	0:1e09cd7d66b4	862	// ------------------------------------------------------------------------------------------------------------------------------------------------------------------
nerit	0:1e09cd7d66b4	863
nerit	0:1e09cd7d66b4	864	int main()
nerit	0:1e09cd7d66b4	865	{
nerit	0:1e09cd7d66b4	866	wd.Configure(2); //watchdog set at xx seconds
nerit	0:1e09cd7d66b4	867
nerit	0:1e09cd7d66b4	868	for (int a=0; a<5;a++){
nerit	0:1e09cd7d66b4	869	mediaSpeed[a]=0;
nerit	0:1e09cd7d66b4	870	}
nerit	0:1e09cd7d66b4	871
nerit	0:1e09cd7d66b4	872	// Stepper driver init and set
nerit	0:1e09cd7d66b4	873	TBmotorRst=0; // reset stepper driver
nerit	0:1e09cd7d66b4	874	TBmotorDirecti=0; // reset stepper direction
nerit	0:1e09cd7d66b4	875	// M1 M2 M3 RESOLUTION
nerit	0:1e09cd7d66b4	876	// 0 0 0 1/2
nerit	0:1e09cd7d66b4	877	// 1 0 0 1/8
nerit	0:1e09cd7d66b4	878	// 0 1 0 1/16
nerit	0:1e09cd7d66b4	879	// 1 1 0 1/32
nerit	0:1e09cd7d66b4	880	// 0 0 1 1/64
nerit	0:1e09cd7d66b4	881	// 1 0 1 1/128
nerit	0:1e09cd7d66b4	882	// 0 1 1 1/10
nerit	0:1e09cd7d66b4	883	// 1 1 1 1/20
nerit	0:1e09cd7d66b4	884	if (TBmotorSteps==400){
nerit	0:1e09cd7d66b4	885	TBmotor_M1=0;
nerit	0:1e09cd7d66b4	886	TBmotor_M2=0;
nerit	0:1e09cd7d66b4	887	TBmotor_M3=0;
nerit	0:1e09cd7d66b4	888	}else if (TBmotorSteps==1600){
nerit	0:1e09cd7d66b4	889	TBmotor_M1=1;
nerit	0:1e09cd7d66b4	890	TBmotor_M2=0;
nerit	0:1e09cd7d66b4	891	TBmotor_M3=0;
nerit	0:1e09cd7d66b4	892	}else if (TBmotorSteps==3200){
nerit	0:1e09cd7d66b4	893	TBmotor_M1=0;
nerit	0:1e09cd7d66b4	894	TBmotor_M2=1;
nerit	0:1e09cd7d66b4	895	TBmotor_M3=0;
nerit	0:1e09cd7d66b4	896	}else if (TBmotorSteps==6400){
nerit	0:1e09cd7d66b4	897	TBmotor_M1=1;
nerit	0:1e09cd7d66b4	898	TBmotor_M2=1;
nerit	0:1e09cd7d66b4	899	TBmotor_M3=0;
nerit	0:1e09cd7d66b4	900	}else if (TBmotorSteps==12800){
nerit	0:1e09cd7d66b4	901	TBmotor_M1=0;
nerit	0:1e09cd7d66b4	902	TBmotor_M2=0;
nerit	0:1e09cd7d66b4	903	TBmotor_M3=1;
nerit	0:1e09cd7d66b4	904	}else if (TBmotorSteps==25600){
nerit	0:1e09cd7d66b4	905	TBmotor_M1=1;
nerit	0:1e09cd7d66b4	906	TBmotor_M2=0;
nerit	0:1e09cd7d66b4	907	TBmotor_M3=1;
nerit	0:1e09cd7d66b4	908	}else if (TBmotorSteps==2000){
nerit	0:1e09cd7d66b4	909	TBmotor_M1=0;
nerit	0:1e09cd7d66b4	910	TBmotor_M2=1;
nerit	0:1e09cd7d66b4	911	TBmotor_M3=1;
nerit	0:1e09cd7d66b4	912	}else if (TBmotorSteps==4000){
nerit	0:1e09cd7d66b4	913	TBmotor_M1=1;
nerit	0:1e09cd7d66b4	914	TBmotor_M2=1;
nerit	0:1e09cd7d66b4	915	TBmotor_M3=1;
nerit	0:1e09cd7d66b4	916	}
nerit	0:1e09cd7d66b4	917	TBmotorRst=1;
nerit	0:1e09cd7d66b4	918
nerit	0:1e09cd7d66b4	919	// DC reset ad set
nerit	0:1e09cd7d66b4	920	int decima = 100;
nerit	0:1e09cd7d66b4	921	wait_ms(200);
nerit	0:1e09cd7d66b4	922	SD_CurrentStart=floor(SDcurrent.read()*decima)/decima;
nerit	0:1e09cd7d66b4	923	wait_ms(2);
nerit	0:1e09cd7d66b4	924	SD_CurrentStart+=floor(SDcurrent.read()*decima)/decima;
nerit	0:1e09cd7d66b4	925	wait_ms(1);
nerit	0:1e09cd7d66b4	926	SD_CurrentStart+=floor(SDcurrent.read()*decima)/decima;
nerit	0:1e09cd7d66b4	927	wait_ms(3);
nerit	0:1e09cd7d66b4	928	SD_CurrentStart+=floor(SDcurrent.read()*decima)/decima;
nerit	0:1e09cd7d66b4	929	SD_CurrentStart=(floor((SD_CurrentStart/4.0f)*decima)/decima)-0.01f;
nerit	0:1e09cd7d66b4	930	wait_ms(100);
nerit	0:1e09cd7d66b4	931	DC_prepare();
nerit	0:1e09cd7d66b4	932
nerit	0:1e09cd7d66b4	933	speedTimer.start(); // speed pulse timer
nerit	0:1e09cd7d66b4	934	intraPickTimer.start();
nerit	0:1e09cd7d66b4	935	speedTimeOut.start();
nerit	0:1e09cd7d66b4	936	speedFilter.start();
nerit	0:1e09cd7d66b4	937	seedFilter.start();
nerit	0:1e09cd7d66b4	938	TBfilter.start();
nerit	0:1e09cd7d66b4	939	sincroTimer.start();
nerit	0:1e09cd7d66b4	940	rotationTimeOut.start();
nerit	0:1e09cd7d66b4	941	metalTimer.start();
nerit	0:1e09cd7d66b4	942	quincTime.start();
nerit	1:e88bf5011af6	943	quincTimeSD.start();
nerit	0:1e09cd7d66b4	944
nerit	0:1e09cd7d66b4	945	//*******************************************************
nerit	0:1e09cd7d66b4	946	// controls for check DC motor current
nerit	0:1e09cd7d66b4	947	//currentCheck.attach(&DC_CheckCurrent, 0.10f);
nerit	0:1e09cd7d66b4	948	pwmCheck.rise(&startDelay);
nerit	0:1e09cd7d66b4	949	//tbCorrection.attach(&correction,0.1f;
nerit	0:1e09cd7d66b4	950
nerit	0:1e09cd7d66b4	951	if (inProva==0){
nerit	0:1e09cd7d66b4	952	tractorSpeedRead.rise(&tractorReadSpeed);
nerit	0:1e09cd7d66b4	953	#if !defined(speedMaster)
nerit	0:1e09cd7d66b4	954	quinconceIn.rise(&quincTrigga);
nerit	0:1e09cd7d66b4	955	quinconceIn.fall(&quincTrigga);
nerit	0:1e09cd7d66b4	956	#endif
nerit	0:1e09cd7d66b4	957	tftUpdate.attach(&videoUpdate,0.50f);
nerit	0:1e09cd7d66b4	958	seedCorrection.attach(&seedCorrect,0.005f); // con 16 becchi a 4,5Kmh ci sono 37mS tra un becco e l'altro, quindi 8 correzioni di tb
nerit	0:1e09cd7d66b4	959	if (speedFromPick==0){
nerit	0:1e09cd7d66b4	960	ElementPosition.rise(&sd25Fall);
nerit	0:1e09cd7d66b4	961	}
nerit	0:1e09cd7d66b4	962	}else{
nerit	0:1e09cd7d66b4	963	tftUpdate.attach(&videoUpdate,0.125f);
nerit	0:1e09cd7d66b4	964	}
nerit	0:1e09cd7d66b4	965
nerit	0:1e09cd7d66b4	966	aggiornaParametri();
nerit	0:1e09cd7d66b4	967
nerit	0:1e09cd7d66b4	968	SDmotorPWM.period_us(periodoSD); // frequency 1KHz pilotaggio motore DC
nerit	0:1e09cd7d66b4	969	SDmotorPWM.write(0.0f); // duty cycle = stop
nerit	0:1e09cd7d66b4	970	TBmotorDirecti=0; // tb motor direction set
nerit	0:1e09cd7d66b4	971
nerit	0:1e09cd7d66b4	972	#if defined(provaStepper)
nerit	0:1e09cd7d66b4	973	TBmotorRst=1;
nerit	0:1e09cd7d66b4	974	TBticker.attach_us(&step_TBPulseOut,3000.0f); // clock time are seconds and attach seed motor stepper controls
nerit	0:1e09cd7d66b4	975	#endif // end prova stepper
nerit	0:1e09cd7d66b4	976
nerit	0:1e09cd7d66b4	977	#if defined(canbusActive)
nerit	0:1e09cd7d66b4	978	can1.attach(&leggiCAN, CAN::RxIrq);
nerit	0:1e09cd7d66b4	979	#endif
nerit	0:1e09cd7d66b4	980
nerit	0:1e09cd7d66b4	981	//**************************************************************************************************************
nerit	0:1e09cd7d66b4	982	// MAIN LOOP
nerit	0:1e09cd7d66b4	983	//**************************************************************************************************************
nerit	0:1e09cd7d66b4	984	while (true){
nerit	0:1e09cd7d66b4	985	// ripetitore segnale di velocità
nerit	0:1e09cd7d66b4	986	if (tractorSpeedRead==0){speedClock=0;}
nerit	0:1e09cd7d66b4	987
nerit	0:1e09cd7d66b4	988	// inversione segnali ingressi
nerit	0:1e09cd7d66b4	989	TBzeroPinInput = !TBzeroPinInputRev;
nerit	0:1e09cd7d66b4	990	seedWheelZeroPinInput = !seedWheelZeroPinInputRev;
nerit	0:1e09cd7d66b4	991
nerit	0:1e09cd7d66b4	992	// se quinconce attivo ed unita' master invia segnale di sincro
nerit	0:1e09cd7d66b4	993	#if defined(speedMaster)
nerit	0:1e09cd7d66b4	994	if (seedWheelZeroPinInput==1){
nerit	0:1e09cd7d66b4	995	quinconceOut=1;
nerit	0:1e09cd7d66b4	996	}
nerit	0:1e09cd7d66b4	997	if ((double)SDactualPosition >= (SDsectorStep/2.0f)){
nerit	0:1e09cd7d66b4	998	quinconceOut=0;
nerit	0:1e09cd7d66b4	999	}
nerit	0:1e09cd7d66b4	1000	#endif
nerit	0:1e09cd7d66b4	1001
nerit	0:1e09cd7d66b4	1002	#if defined(overWriteCanSimulation)
nerit	0:1e09cd7d66b4	1003	leggiCAN();
nerit	0:1e09cd7d66b4	1004	#endif
nerit	0:1e09cd7d66b4	1005
nerit	0:1e09cd7d66b4	1006	// simulazione velocita
nerit	0:1e09cd7d66b4	1007	if (enableSimula==1){
nerit	0:1e09cd7d66b4	1008	double TMT = 0.0f;
nerit	0:1e09cd7d66b4	1009	if (speedSimula > 0){
nerit	0:1e09cd7d66b4	1010	TMT = (double)(speedSimula) * 100.0f /3600.0f;
nerit	0:1e09cd7d66b4	1011	pulseSpeedInterval = pulseDistance / TMT;
nerit	0:1e09cd7d66b4	1012	}else{
nerit	0:1e09cd7d66b4	1013	pulseSpeedInterval = 10000.0f;
nerit	0:1e09cd7d66b4	1014	}
nerit	0:1e09cd7d66b4	1015	if (avviaSimula==1){
nerit	0:1e09cd7d66b4	1016	if(oldSimulaSpeed!=pulseSpeedInterval){
nerit	0:1e09cd7d66b4	1017	spedSimclock.attach_us(&speedSimulationClock,pulseSpeedInterval);
nerit	0:1e09cd7d66b4	1018	oldSimulaSpeed=pulseSpeedInterval;
nerit	0:1e09cd7d66b4	1019	}
nerit	0:1e09cd7d66b4	1020	}else{
nerit	0:1e09cd7d66b4	1021	oldSimulaSpeed=10000.0f;
nerit	0:1e09cd7d66b4	1022	spedSimclock.detach();
nerit	0:1e09cd7d66b4	1023	}
nerit	0:1e09cd7d66b4	1024	}else{
nerit	0:1e09cd7d66b4	1025	spedSimclock.detach();
nerit	0:1e09cd7d66b4	1026	}
nerit	0:1e09cd7d66b4	1027
nerit	0:1e09cd7d66b4	1028	//*******************************************************
nerit	0:1e09cd7d66b4	1029	// determina se sono in bassa velocità per il controllo di TB
nerit	0:1e09cd7d66b4	1030	if (speedOfSeedWheel<=minSeedSpeed){
nerit	0:1e09cd7d66b4	1031	if (lowSpeedRequired==0){
nerit	0:1e09cd7d66b4	1032	ritardaLowSpeed.reset();
nerit	0:1e09cd7d66b4	1033	ritardaLowSpeed.start();
nerit	0:1e09cd7d66b4	1034	}
nerit	0:1e09cd7d66b4	1035	lowSpeedRequired=1;
nerit	0:1e09cd7d66b4	1036	}else{
nerit	0:1e09cd7d66b4	1037	if (lowSpeedRequired==1){
nerit	0:1e09cd7d66b4	1038	lowSpeedRequired=0;
nerit	0:1e09cd7d66b4	1039	ritardaLowSpeed.reset();
nerit	0:1e09cd7d66b4	1040	ritardaLowSpeed.stop();
nerit	0:1e09cd7d66b4	1041	}
nerit	0:1e09cd7d66b4	1042	}
nerit	0:1e09cd7d66b4	1043
nerit	0:1e09cd7d66b4	1044	if (ritardaLowSpeed.read_ms()> 2000){
nerit	0:1e09cd7d66b4	1045	lowSpeed=1;
nerit	0:1e09cd7d66b4	1046	}else{
nerit	0:1e09cd7d66b4	1047	lowSpeed=0;
nerit	0:1e09cd7d66b4	1048	}
nerit	0:1e09cd7d66b4	1049
nerit	0:1e09cd7d66b4	1050
nerit	0:1e09cd7d66b4	1051	//**************************************************************************************************************
nerit	0:1e09cd7d66b4	1052	//**************************************************************************************************************
nerit	0:1e09cd7d66b4	1053	// LOGICAL CONTROLS
nerit	0:1e09cd7d66b4	1054	//**************************************************************************************************************
nerit	0:1e09cd7d66b4	1055	//**************************************************************************************************************
nerit	0:1e09cd7d66b4	1056
nerit	0:1e09cd7d66b4	1057	if (inProva==0){
nerit	0:1e09cd7d66b4	1058	if ((startCycleSimulation==0)&&(enableSimula==0)){zeroRequestBuf=1;runRequestBuf=0;enableCycle=1;
nerit	0:1e09cd7d66b4	1059	}else{zeroRequestBuf=1;runRequestBuf=0;enableCycle=1;}
nerit	0:1e09cd7d66b4	1060	if ((tractorSpeedRead==0)&&(speedFilter.read_ms()>3)){oldTractorSpeedRead=0;}
nerit	0:1e09cd7d66b4	1061	// ----------------------------------------
nerit	0:1e09cd7d66b4	1062	// filtra il segnale dei becchi per misurare il tempo di intervallo tra loro
nerit	0:1e09cd7d66b4	1063	// ----------------------------------------
nerit	0:1e09cd7d66b4	1064	if (seedWheelZeroPinInput==0){
nerit	0:1e09cd7d66b4	1065	//trigSlave=false;
nerit	0:1e09cd7d66b4	1066	if(seedFilter.read_ms()>=4){
nerit	0:1e09cd7d66b4	1067	oldSeedWheelZeroPinInput=0;
nerit	0:1e09cd7d66b4	1068	SDzeroDebounced=0;
nerit	0:1e09cd7d66b4	1069	}
nerit	0:1e09cd7d66b4	1070	}
nerit	0:1e09cd7d66b4	1071	if ((seedWheelZeroPinInput==1)&&(oldSeedWheelZeroPinInput==0)){
nerit	0:1e09cd7d66b4	1072	timeIntraPick = (double)intraPickTimer.read_ms();
nerit	0:1e09cd7d66b4	1073	prePosSD=500; // preposizionamento SD
nerit	0:1e09cd7d66b4	1074	intraPickTimer.reset();
nerit	0:1e09cd7d66b4	1075	rotationTimeOut.reset();
nerit	0:1e09cd7d66b4	1076	seedFilter.reset();
nerit	0:1e09cd7d66b4	1077	sincroTimer.reset();
nerit	0:1e09cd7d66b4	1078	oldSeedWheelZeroPinInput=1;
nerit	1:e88bf5011af6	1079	quincTimeSD.reset();
nerit	0:1e09cd7d66b4	1080	SDzeroDebounced=1;
nerit	0:1e09cd7d66b4	1081	if (trigDaMaster==false){
nerit	0:1e09cd7d66b4	1082	quincInibit=true;
nerit	0:1e09cd7d66b4	1083	trigSlave=true;
nerit	0:1e09cd7d66b4	1084	}else{
nerit	0:1e09cd7d66b4	1085	quincInibit=false;
nerit	0:1e09cd7d66b4	1086	trigSlave=false;
nerit	0:1e09cd7d66b4	1087	trigDaMaster=false;
nerit	0:1e09cd7d66b4	1088	}
nerit	0:1e09cd7d66b4	1089	SDwheelTimer.reset();
nerit	0:1e09cd7d66b4	1090	if (quincCnt<10){
nerit	0:1e09cd7d66b4	1091	quincCnt++;
nerit	0:1e09cd7d66b4	1092	}
nerit	1:e88bf5011af6	1093	if (quincCnt >9){
nerit	0:1e09cd7d66b4	1094	quincEnable=1;
nerit	0:1e09cd7d66b4	1095	}else{
nerit	0:1e09cd7d66b4	1096	quincEnable=0;
nerit	0:1e09cd7d66b4	1097	}
nerit	1:e88bf5011af6	1098	if (cntQuinc<10){
nerit	1:e88bf5011af6	1099	cntQuinc++;
nerit	1:e88bf5011af6	1100	}
nerit	0:1e09cd7d66b4	1101	if ((aspettaStart==0)&&(lowSpeed==1)){
nerit	0:1e09cd7d66b4	1102	beccoPronto=1;
nerit	0:1e09cd7d66b4	1103	}
nerit	0:1e09cd7d66b4	1104	SDzeroCyclePulse=1;
nerit	0:1e09cd7d66b4	1105	lockStart=0;
nerit	0:1e09cd7d66b4	1106	double fase1=0.0f;
nerit	0:1e09cd7d66b4	1107	forzaFase=0;
nerit	0:1e09cd7d66b4	1108	double limite=fixedStepGiroSD/pickNumber;
nerit	0:1e09cd7d66b4	1109	if (pickCounterLow< 0xFF){
nerit	0:1e09cd7d66b4	1110	pickCounterLow++;
nerit	0:1e09cd7d66b4	1111	}else{
nerit	0:1e09cd7d66b4	1112	pickCounterHig++;
nerit	0:1e09cd7d66b4	1113	pickCounterLow=0;
nerit	0:1e09cd7d66b4	1114	}
nerit	0:1e09cd7d66b4	1115	if (tamburoStandard==0){
nerit	0:1e09cd7d66b4	1116	fase1=TBdeltaStep;
nerit	0:1e09cd7d66b4	1117	}else{
nerit	0:1e09cd7d66b4	1118	if(speedForCorrection >= speedOfSeedWheel){
nerit	0:1e09cd7d66b4	1119	fase1=TBdeltaStep;
nerit	0:1e09cd7d66b4	1120	}else{
nerit	0:1e09cd7d66b4	1121	//fase1=(TBdeltaStep)-(((speedOfSeedWheel-speedForCorrection)/1.25f)*(TBfaseStep));
nerit	0:1e09cd7d66b4	1122	fase1=(TBdeltaStep)-(((speedOfSeedWheel)/1.25f)*(TBfaseStep));
nerit	0:1e09cd7d66b4	1123	}
nerit	0:1e09cd7d66b4	1124	if (fase1 > limite){
nerit	0:1e09cd7d66b4	1125	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	0:1e09cd7d66b4	1126	//forzaFase=1;
nerit	0:1e09cd7d66b4	1127	}
nerit	0:1e09cd7d66b4	1128	if ((fase1 > (limite -20.0f))&&(fase1<(limite +5.0f))){
nerit	0:1e09cd7d66b4	1129	fase1 = limite -20.0f; // se la fase è molto vicina al limite interpone uno spazio minimo di lavoro del sincronismo
nerit	0:1e09cd7d66b4	1130	forzaFase=1;
nerit	0:1e09cd7d66b4	1131	}
nerit	0:1e09cd7d66b4	1132	trigRepos=1;
nerit	0:1e09cd7d66b4	1133	}
nerit	0:1e09cd7d66b4	1134	fase = (uint32_t)fase1+500;
nerit	0:1e09cd7d66b4	1135	#if defined(pcSerial)
nerit	0:1e09cd7d66b4	1136	#if defined(inCorre)
nerit	0:1e09cd7d66b4	1137	pc.printf(" limite %f", limite);
nerit	0:1e09cd7d66b4	1138	pc.printf(" delta %f", TBdeltaStep);
nerit	0:1e09cd7d66b4	1139	pc.printf(" faseStep %f", TBfaseStep);
nerit	0:1e09cd7d66b4	1140	pc.printf(" fase %d",fase);
nerit	0:1e09cd7d66b4	1141	pc.printf(" forzaFase %d",forzaFase);
nerit	0:1e09cd7d66b4	1142	pc.printf(" trigRepos %d", trigRepos);
nerit	0:1e09cd7d66b4	1143	pc.printf(" ActualSD: %d",SDactualPosition);
nerit	0:1e09cd7d66b4	1144	pc.printf(" SpeedWheel: %f",speedOfSeedWheel);
nerit	0:1e09cd7d66b4	1145	pc.printf(" SPEED: %f \n",tractorSpeed_MtS_timed);
nerit	0:1e09cd7d66b4	1146	#endif
nerit	0:1e09cd7d66b4	1147	#endif
nerit	0:1e09cd7d66b4	1148	if (timeIntraPick >= (memoIntraPick*2)){
nerit	0:1e09cd7d66b4	1149	if ((aspettaStart==0)&&(zeroRequestBuf==0)){
nerit	0:1e09cd7d66b4	1150	if (firstStart==0){
nerit	0:1e09cd7d66b4	1151	all_pickSignal=1;
nerit	0:1e09cd7d66b4	1152	}
nerit	0:1e09cd7d66b4	1153	}
nerit	0:1e09cd7d66b4	1154	}
nerit	0:1e09cd7d66b4	1155	memoIntraPick = timeIntraPick;
nerit	0:1e09cd7d66b4	1156	if (speedFromPick==1){
nerit	0:1e09cd7d66b4	1157	speedOfSeedWheel=((seedPerimeter / pickNumber)/timeIntraPick)*1000.0f;
nerit	0:1e09cd7d66b4	1158	}
nerit	0:1e09cd7d66b4	1159	if ((tractorSpeed_MtS_timed==0.0f)&&(zeroCycle==0)&&(zeroCycleEnd==1)){
nerit	0:1e09cd7d66b4	1160	if ((firstStart==0)&&(simOk==0)&&(enableSimula==0)&&(zeroCycleEnd==1)){
nerit	0:1e09cd7d66b4	1161	all_noSpeedSen=1;
nerit	0:1e09cd7d66b4	1162	}
nerit	0:1e09cd7d66b4	1163	#if defined(pcSerial)
nerit	0:1e09cd7d66b4	1164	#if defined(canDataReceived)
nerit	0:1e09cd7d66b4	1165	pc.printf("allarme no speed sensor");
nerit	0:1e09cd7d66b4	1166	#endif
nerit	0:1e09cd7d66b4	1167	#endif
nerit	0:1e09cd7d66b4	1168	}
nerit	0:1e09cd7d66b4	1169	pulseRised2=1;
nerit	0:1e09cd7d66b4	1170	#if defined(speedMaster)
nerit	0:1e09cd7d66b4	1171	double oldLastPr = (double)oldLastPulseRead*1.2f;
nerit	0:1e09cd7d66b4	1172	if((double)speedTimeOut.read_us()> (oldLastPr)){
nerit	0:1e09cd7d66b4	1173	if ((firstStart==0)&&(simOk==0)&&(enableSimula==0)&&(zeroCycleEnd==1)){
nerit	0:1e09cd7d66b4	1174	all_speedError =1;
nerit	0:1e09cd7d66b4	1175	}
nerit	0:1e09cd7d66b4	1176	#if defined(pcSerial)
nerit	0:1e09cd7d66b4	1177	#if defined(canDataReceived)
nerit	0:1e09cd7d66b4	1178	pc.printf("allarme errore speed sensor");
nerit	0:1e09cd7d66b4	1179	#endif
nerit	0:1e09cd7d66b4	1180	#endif
nerit	0:1e09cd7d66b4	1181
nerit	0:1e09cd7d66b4	1182	}
nerit	0:1e09cd7d66b4	1183	#endif
nerit	0:1e09cd7d66b4	1184	//*******************************************
nerit	0:1e09cd7d66b4	1185	// esegue calcolo clock per la generazione della posizione teorica
nerit	0:1e09cd7d66b4	1186	// la realtà in base al segnale di presenza del becco
nerit	0:1e09cd7d66b4	1187	realSpeed = speedOfSeedWheel;
nerit	0:1e09cd7d66b4	1188	realGiroSD = seedPerimeter / realSpeed;
nerit	0:1e09cd7d66b4	1189	tempoBecco = (realGiroSD/360.0f)*16000.0f;
nerit	0:1e09cd7d66b4	1190	frequenzaReale = fixedStepGiroSD/realGiroSD;
nerit	0:1e09cd7d66b4	1191	semiPeriodoReale = (1000000.0f/frequenzaReale);
nerit	0:1e09cd7d66b4	1192	tempoTraBecchi_mS = 0.0f;
nerit	0:1e09cd7d66b4	1193	seedWheelRPM = (speedOfSeedWheel)*K_WheelRPM ; // calcola i giri al minuto della ruota di semina 7.37 31,75
nerit	0:1e09cd7d66b4	1194	rapportoRuote = pickNumber/cellsNumber; // 0.67 calcola il rapporto tra il numero di becchi ed il numero di celle 0.8 1,00
nerit	0:1e09cd7d66b4	1195	TBrpm = seedWheelRPM*rapportoRuote; // 5.896 31,75
nerit	0:1e09cd7d66b4	1196	TBgiroStep = TBmotorSteps*TBreductionRatio;
nerit	0:1e09cd7d66b4	1197	K_TBfrequency = TBgiroStep/60.0f; // 1600 * 1.65625f /60 = 44 44,00
nerit	0:1e09cd7d66b4	1198	TBfrequency = (TBrpm*K_TBfrequency); // 130Hz a 0,29Mts 1397,00 a 1,25mt/s con 15 becchi e 15 celle
nerit	0:1e09cd7d66b4	1199	TBperiod=1000000.0f/TBfrequency; // 715uS
nerit	0:1e09cd7d66b4	1200	}
nerit	0:1e09cd7d66b4	1201	// ----------------------------------------
nerit	0:1e09cd7d66b4	1202	// check SD fase
nerit	0:1e09cd7d66b4	1203	if ((prePosSD >= fase)\|\|(forzaFase==1)){//&&(prePosSD < (fase +30))){
nerit	0:1e09cd7d66b4	1204	forzaFase=0;
nerit	0:1e09cd7d66b4	1205	if (trigRepos==1){
nerit	0:1e09cd7d66b4	1206	SDactualPosition=0;
nerit	0:1e09cd7d66b4	1207	if ((countCicli<30)&&(trigCicli==0)){countCicli++;trigCicli=1;}
nerit	0:1e09cd7d66b4	1208	if(countCicli>=cicliAspettaStart){aspettaStart=0;}
nerit	0:1e09cd7d66b4	1209	if ((lowSpeed==0)&&(aspettaStart==0)&&(lockStart==0)){syncroCheck=1;beccoPronto=0;}
nerit	0:1e09cd7d66b4	1210	if (trigTB==0){
nerit	0:1e09cd7d66b4	1211	inhibit=1;
nerit	0:1e09cd7d66b4	1212	trigSD=1;
nerit	0:1e09cd7d66b4	1213	}else{
nerit	0:1e09cd7d66b4	1214	inhibit=0;
nerit	0:1e09cd7d66b4	1215	trigTB=0;
nerit	0:1e09cd7d66b4	1216	trigSD=0;
nerit	0:1e09cd7d66b4	1217	}
nerit	0:1e09cd7d66b4	1218	trigRepos=0;
nerit	0:1e09cd7d66b4	1219	}
nerit	0:1e09cd7d66b4	1220	}else{
nerit	0:1e09cd7d66b4	1221	trigCicli=0;
nerit	0:1e09cd7d66b4	1222	}
nerit	0:1e09cd7d66b4	1223	// ----------------------------------------
nerit	0:1e09cd7d66b4	1224	// filtra il segnale del tamburo per lo stop in fase del tamburo stesso
nerit	0:1e09cd7d66b4	1225	if (TBzeroPinInput==0){if (TBfilter.read_ms()>=5){oldTBzeroPinInput=0;}}
nerit	0:1e09cd7d66b4	1226	if ((TBzeroPinInput==1)&&(oldTBzeroPinInput==0)){
nerit	0:1e09cd7d66b4	1227	oldTBzeroPinInput=1;
nerit	0:1e09cd7d66b4	1228	if (loadDaCanInCorso==0){
nerit	0:1e09cd7d66b4	1229	stopCicloTB=1;
nerit	0:1e09cd7d66b4	1230	startCicloTB=0;
nerit	0:1e09cd7d66b4	1231	}
nerit	0:1e09cd7d66b4	1232	TBfilter.reset();
nerit	0:1e09cd7d66b4	1233	TBzeroCyclePulse=1;
nerit	0:1e09cd7d66b4	1234	TBactualPosition=0;
nerit	0:1e09cd7d66b4	1235	if (cellsCounterLow < 0xFF){
nerit	0:1e09cd7d66b4	1236	cellsCounterLow++;
nerit	0:1e09cd7d66b4	1237	}else{
nerit	0:1e09cd7d66b4	1238	cellsCounterHig++;
nerit	0:1e09cd7d66b4	1239	cellsCounterLow=0;
nerit	0:1e09cd7d66b4	1240	}
nerit	0:1e09cd7d66b4	1241	if (loadDaCanInCorso==1){
nerit	0:1e09cd7d66b4	1242	cntCellsForLoad++;
nerit	0:1e09cd7d66b4	1243	if (cntCellsForLoad >= 5){
nerit	0:1e09cd7d66b4	1244	stopCicloTB=1;
nerit	0:1e09cd7d66b4	1245	cntCellsForLoad=0;
nerit	0:1e09cd7d66b4	1246	}
nerit	0:1e09cd7d66b4	1247	}else{
nerit	0:1e09cd7d66b4	1248	cntCellsForLoad=0;
nerit	0:1e09cd7d66b4	1249	}
nerit	0:1e09cd7d66b4	1250	if (trigSD==0){
nerit	0:1e09cd7d66b4	1251	inhibit=1;
nerit	0:1e09cd7d66b4	1252	trigTB=1;
nerit	0:1e09cd7d66b4	1253	}else{
nerit	0:1e09cd7d66b4	1254	inhibit=0;
nerit	0:1e09cd7d66b4	1255	trigTB=0;
nerit	0:1e09cd7d66b4	1256	trigSD=0;
nerit	0:1e09cd7d66b4	1257	}
nerit	0:1e09cd7d66b4	1258	// torna indietro per sbloccare il tamburo
nerit	0:1e09cd7d66b4	1259	if ((TBmotorDirecti==1)&&(erroreTamburo==1)){
nerit	0:1e09cd7d66b4	1260	cntCellsForReload++;
nerit	0:1e09cd7d66b4	1261	if (cntCellsForReload > 3){
nerit	0:1e09cd7d66b4	1262	TBmotorDirecti=0;
nerit	0:1e09cd7d66b4	1263	erroreTamburo=0;
nerit	0:1e09cd7d66b4	1264	}
nerit	0:1e09cd7d66b4	1265	}
nerit	0:1e09cd7d66b4	1266	}
nerit	0:1e09cd7d66b4	1267	if ((double)TBactualPosition > ((TBgiroStep/cellsNumber)*1.2f)){
nerit	0:1e09cd7d66b4	1268	if (firstStart==0){
nerit	0:1e09cd7d66b4	1269	all_cellSignal=1;
nerit	0:1e09cd7d66b4	1270	}
nerit	0:1e09cd7d66b4	1271	if (erroreTamburo==0){
nerit	0:1e09cd7d66b4	1272	erroreTamburo=1;
nerit	0:1e09cd7d66b4	1273	TBmotorDirecti=1;
nerit	0:1e09cd7d66b4	1274	cntCellsForReload=0;
nerit	0:1e09cd7d66b4	1275	cntTbError++;
nerit	0:1e09cd7d66b4	1276	}
nerit	0:1e09cd7d66b4	1277	#if defined(pcSerial)
nerit	0:1e09cd7d66b4	1278	#if defined(canDataReceived)
nerit	0:1e09cd7d66b4	1279	pc.printf("allarme error cells");
nerit	0:1e09cd7d66b4	1280	#endif
nerit	0:1e09cd7d66b4	1281	#endif
nerit	0:1e09cd7d66b4	1282	}
nerit	0:1e09cd7d66b4	1283	if (((double)TBactualPosition > ((TBgiroStep/cellsNumber)*3.0f))\|\|(cntTbError>2)){
nerit	0:1e09cd7d66b4	1284	if (firstStart==0){
nerit	0:1e09cd7d66b4	1285	all_noStepRota=1;
nerit	0:1e09cd7d66b4	1286	}
nerit	0:1e09cd7d66b4	1287	cntTbError=0;
nerit	0:1e09cd7d66b4	1288	#if defined(pcSerial)
nerit	0:1e09cd7d66b4	1289	#if defined(canDataReceived)
nerit	0:1e09cd7d66b4	1290	pc.printf("allarme no cells sensor");
nerit	0:1e09cd7d66b4	1291	#endif
nerit	0:1e09cd7d66b4	1292	#endif
nerit	0:1e09cd7d66b4	1293	}
nerit	0:1e09cd7d66b4	1294	// ----------------------------------------
nerit	0:1e09cd7d66b4	1295	// read and manage joystick
nerit	0:1e09cd7d66b4	1296	// WARNING - ENABLE CYCLE IS SOFTWARE ALWAYS ACTIVE
nerit	0:1e09cd7d66b4	1297	if (enableCycle==1){
nerit	0:1e09cd7d66b4	1298	if(runRequestBuf==1){
nerit	0:1e09cd7d66b4	1299	if (OldStartCycle!=runRequestBuf){
nerit	0:1e09cd7d66b4	1300	if((startCycle==0)&&(zeroCycleEnd==1)){
nerit	0:1e09cd7d66b4	1301	startCycle=1;
nerit	0:1e09cd7d66b4	1302	OldStartCycle = runRequestBuf;
nerit	0:1e09cd7d66b4	1303	oldZeroCycle=0;
nerit	0:1e09cd7d66b4	1304	}
nerit	0:1e09cd7d66b4	1305	}
nerit	0:1e09cd7d66b4	1306	}else{
nerit	0:1e09cd7d66b4	1307	startCycle=0;
nerit	0:1e09cd7d66b4	1308	pntMedia=0;
nerit	0:1e09cd7d66b4	1309	}
nerit	0:1e09cd7d66b4	1310	if (azzeraDaCan==1){
nerit	0:1e09cd7d66b4	1311	if (tractorSpeed_MtS_timed==0.0f){
nerit	0:1e09cd7d66b4	1312	zeroRequestBuf=1;
nerit	0:1e09cd7d66b4	1313	oldZeroCycle=0;
nerit	0:1e09cd7d66b4	1314	}
nerit	0:1e09cd7d66b4	1315	azzeraDaCan=0;
nerit	0:1e09cd7d66b4	1316	}
nerit	0:1e09cd7d66b4	1317	if (loadDaCan==1){
nerit	0:1e09cd7d66b4	1318	if (tractorSpeed_MtS_timed==0.0f){
nerit	0:1e09cd7d66b4	1319	ciclaTB();
nerit	0:1e09cd7d66b4	1320	}
nerit	0:1e09cd7d66b4	1321	}
nerit	0:1e09cd7d66b4	1322	if ((zeroRequestBuf==1)){
nerit	0:1e09cd7d66b4	1323	if (oldZeroCycle!=zeroRequestBuf){
nerit	0:1e09cd7d66b4	1324	zeroCycle=1;
nerit	0:1e09cd7d66b4	1325	zeroCycleEnd=0;
nerit	0:1e09cd7d66b4	1326	SDzeroed=0;
nerit	0:1e09cd7d66b4	1327	TBzeroed=0;
nerit	0:1e09cd7d66b4	1328	zeroTrigger=0;
nerit	0:1e09cd7d66b4	1329	oldZeroCycle = zeroRequestBuf;
nerit	0:1e09cd7d66b4	1330	}
nerit	0:1e09cd7d66b4	1331	}
nerit	0:1e09cd7d66b4	1332	}else{
nerit	0:1e09cd7d66b4	1333	startCycle=0;
nerit	0:1e09cd7d66b4	1334	zeroCycle=0;
nerit	0:1e09cd7d66b4	1335	}
nerit	0:1e09cd7d66b4	1336
nerit	0:1e09cd7d66b4	1337	//***************************************************************************************************
nerit	0:1e09cd7d66b4	1338	// pulseRised define the event of speed wheel pulse occurs
nerit	0:1e09cd7d66b4	1339	//
nerit	0:1e09cd7d66b4	1340	double maxInterval = pulseDistance/minWorkSpeed;
nerit	0:1e09cd7d66b4	1341	double minIntervalPulse = pulseDistance/maxWorkSpeed;
nerit	0:1e09cd7d66b4	1342	if (pulseRised==1){
nerit	0:1e09cd7d66b4	1343	if (enableSpeed<10){enableSpeed++;}
nerit	0:1e09cd7d66b4	1344	pulseRised=0;pulseRised1=1;speedMediaCalc();
nerit	0:1e09cd7d66b4	1345	// calcola velocità trattore
nerit	0:1e09cd7d66b4	1346	if(enableSpeed>=2){
nerit	0:1e09cd7d66b4	1347	if ((pulseSpeedInterval>=0.0f)){ //minIntervalPulse)&&(pulseSpeedInterval<maxInterval)){
nerit	0:1e09cd7d66b4	1348	tractorSpeed_MtS_timed = ((pulseDistance / pulseSpeedInterval)); // tractor speed (unit= Mt/S) from pulse time interval
nerit	0:1e09cd7d66b4	1349	if (checkSDrotation==0){
nerit	0:1e09cd7d66b4	1350	checkSDrotation=1;
nerit	0:1e09cd7d66b4	1351	SDwheelTimer.start();
nerit	0:1e09cd7d66b4	1352	}
nerit	0:1e09cd7d66b4	1353	}
nerit	0:1e09cd7d66b4	1354	}
nerit	0:1e09cd7d66b4	1355	if ((pulseSpeedInterval>=minIntervalPulse)&&(pulseSpeedInterval<maxInterval)){
nerit	0:1e09cd7d66b4	1356	enableSDcontrol =1;
nerit	0:1e09cd7d66b4	1357	}
nerit	0:1e09cd7d66b4	1358	speedTimeOut.reset();
nerit	0:1e09cd7d66b4	1359	}else{
nerit	0:1e09cd7d66b4	1360	double oldLastPr = (double)oldLastPulseRead*1.7f;
nerit	0:1e09cd7d66b4	1361	if((double)speedTimeOut.read_us()> (oldLastPr)){
nerit	0:1e09cd7d66b4	1362	tractorSpeed_MtS_timed = 0.0f;
nerit	0:1e09cd7d66b4	1363	enableSDcontrol =0;
nerit	0:1e09cd7d66b4	1364	pntMedia=0;
nerit	0:1e09cd7d66b4	1365	speedTimeOut.reset();
nerit	0:1e09cd7d66b4	1366	enableSpeed=0;
nerit	0:1e09cd7d66b4	1367	quincCnt=0;
nerit	0:1e09cd7d66b4	1368	}
nerit	0:1e09cd7d66b4	1369	}
nerit	0:1e09cd7d66b4	1370	// esegue il controllo di velocità minima
nerit	0:1e09cd7d66b4	1371	if ((double)speedTimer.read_ms()>=maxInterval){tractorSpeed_MtS_timed = 0.0f;
nerit	0:1e09cd7d66b4	1372	enableSDcontrol =0;
nerit	0:1e09cd7d66b4	1373	enableSpeed=0;
nerit	0:1e09cd7d66b4	1374	}
nerit	0:1e09cd7d66b4	1375	//***************************************************************************************************************
nerit	0:1e09cd7d66b4	1376	// cycle logic control section
nerit	0:1e09cd7d66b4	1377	//***************************************************************************************************************
nerit	0:1e09cd7d66b4	1378	if (enableSimula==1){if(simOk==0){tractorSpeed_MtS_timed=0.0f;}}
nerit	0:1e09cd7d66b4	1379	if ((tractorSpeed_MtS_timed>0.01f)){
nerit	0:1e09cd7d66b4	1380	cycleStopRequest=1;
nerit	0:1e09cd7d66b4	1381	// calcola il tempo teorico di passaggio becchi sulla base della velocità del trattore
nerit	0:1e09cd7d66b4	1382	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	0:1e09cd7d66b4	1383	if (speedFromPick==1) {
nerit	0:1e09cd7d66b4	1384	tempoTraBecchi_mS = (tempoGiroSD / pickNumber)*1000.0f; // tempo tra due impulsi dai becchi in millisecondi ( circa 157mS a 4,5Km/h)
nerit	0:1e09cd7d66b4	1385	}else{
nerit	0:1e09cd7d66b4	1386	tempoTraBecchi_mS = (tempoGiroSD / 25.0f)*1000.0f; // tempo tra due impulsi dai becchi in millisecondi ( circa 157mS a 4,5Km/h)
nerit	0:1e09cd7d66b4	1387	}
nerit	0:1e09cd7d66b4	1388	//*******************************************
nerit	0:1e09cd7d66b4	1389	// segue calcolo duty cycle comando motore DC per allinearsi con la velocità del trattore
nerit	0:1e09cd7d66b4	1390	double dutyTeorico = 0.00;
nerit	0:1e09cd7d66b4	1391	if ((tractorSpeed_MtS_timed>0.0)&&(tractorSpeed_MtS_timed<tabSpeed[0])){dutyTeorico = tabComan[0];}
nerit	0:1e09cd7d66b4	1392	for (int ii = 0;ii<16;ii++){
nerit	0:1e09cd7d66b4	1393	if ((tractorSpeed_MtS_timed>=tabSpeed[ii])&&(tractorSpeed_MtS_timed<tabSpeed[ii+1])){
nerit	0:1e09cd7d66b4	1394	dutyTeorico = tabComan[ii+1];
nerit	0:1e09cd7d66b4	1395	}
nerit	0:1e09cd7d66b4	1396	}
nerit	0:1e09cd7d66b4	1397	if (tractorSpeed_MtS_timed > tabSpeed[16]){dutyTeorico=tractorSpeed_MtS_timed/1.25f;}
nerit	1:e88bf5011af6	1398	// if ((pulseRised1==1)&&(enableSpeed>3)&&(pulseRised2==1)&&(quincMemoEnable==false)){
nerit	1:e88bf5011af6	1399	#if !defined(speedMaster)
nerit	1:e88bf5011af6	1400	#if defined(mezzo)
nerit	1:e88bf5011af6	1401	if (quincEnable==1){
nerit	1:e88bf5011af6	1402	if (quinconceActive==1){
nerit	1:e88bf5011af6	1403	quinCalc(2.0f);
nerit	1:e88bf5011af6	1404	}else{
nerit	1:e88bf5011af6	1405	quinCalc(1.0f);
nerit	1:e88bf5011af6	1406	}
nerit	1:e88bf5011af6	1407	}else{
nerit	1:e88bf5011af6	1408	quincMemoEnable=false;
nerit	1:e88bf5011af6	1409	}
nerit	1:e88bf5011af6	1410	#else
nerit	1:e88bf5011af6	1411	quinCalc(1.0f);
nerit	1:e88bf5011af6	1412	#endif
nerit	1:e88bf5011af6	1413	#endif
nerit	0:1e09cd7d66b4	1414	if ((pulseRised1==1)&&(enableSpeed>3)&&(pulseRised2==1)){
nerit	0:1e09cd7d66b4	1415	double erroreTempo = 0.0f;
nerit	0:1e09cd7d66b4	1416	if(speedFromPick==1){
nerit	1:e88bf5011af6	1417	double giorgio = timeIntraPick/percento;
nerit	1:e88bf5011af6	1418	//erroreTempo = (double)timeIntraPick - tempoTraBecchi_mS; // errore tra il tempo previsto ed il tempo reale ( >0 se sto andando più piano del previsto)
nerit	1:e88bf5011af6	1419	erroreTempo = giorgio - tempoTraBecchi_mS; // errore tra il tempo previsto ed il tempo reale ( >0 se sto andando più piano del previsto)
nerit	0:1e09cd7d66b4	1420	}else{
nerit	0:1e09cd7d66b4	1421	erroreTempo = (double)memoTimeHole - tempoTraBecchi_mS; // errore tra il tempo previsto ed il tempo reale ( >0 se sto andando più piano del previsto)
nerit	0:1e09cd7d66b4	1422	}
nerit	0:1e09cd7d66b4	1423	double errorePercentuale = erroreTempo / tempoTraBecchi_mS;
nerit	1:e88bf5011af6	1424	double k1=errorePercentuale*0.265f;
nerit	1:e88bf5011af6	1425	double k3=0.0f;
nerit	1:e88bf5011af6	1426	double k4=0.0f;
nerit	1:e88bf5011af6	1427	double k5=0.0f;
nerit	1:e88bf5011af6	1428	if (tractorSpeed_MtS_timed <= minSeedSpeed){
nerit	1:e88bf5011af6	1429	k3=0.105;
nerit	1:e88bf5011af6	1430	k4=0.207;
nerit	1:e88bf5011af6	1431	k5=0.310;
nerit	1:e88bf5011af6	1432	}else{
nerit	1:e88bf5011af6	1433	k3=0.1075f;
nerit	1:e88bf5011af6	1434	k4=0.211f;
nerit	1:e88bf5011af6	1435	k5=0.315f;
nerit	1:e88bf5011af6	1436	}
nerit	1:e88bf5011af6	1437	if ((errorePercentuale > 25.0f)\|\|(errorePercentuale < -25.0f)){
nerit	1:e88bf5011af6	1438	k1=errorePercentuale*k5;
nerit	1:e88bf5011af6	1439	}
nerit	1:e88bf5011af6	1440	if (((errorePercentuale >= 10.0f)&&(errorePercentuale<=25.0f))\|\|((errorePercentuale <= -10.0f)&&(errorePercentuale>=-25.0f))){
nerit	1:e88bf5011af6	1441	k1=errorePercentuale*k4;
nerit	1:e88bf5011af6	1442	}
nerit	1:e88bf5011af6	1443	if ((errorePercentuale < 10.0f)\|\|(errorePercentuale > -10.0f)){
nerit	1:e88bf5011af6	1444	k1=errorePercentuale*k3;
nerit	1:e88bf5011af6	1445	}
nerit	0:1e09cd7d66b4	1446	double memoCorrezione = (dutyTeorico *k1);
nerit	1:e88bf5011af6	1447	#if defined(pcSerial)
nerit	1:e88bf5011af6	1448	#if defined(Qnc)
nerit	1:e88bf5011af6	1449	pc.printf("IntraPick: %f TempoBecchi: %f ErrPerc: %f\n",timeIntraPick, tempoTraBecchi_mS,errorePercentuale);
nerit	1:e88bf5011af6	1450	#endif
nerit	1:e88bf5011af6	1451	#endif
nerit	0:1e09cd7d66b4	1452	correzione = correzione + memoCorrezione;
nerit	0:1e09cd7d66b4	1453	pulseRised1=0;
nerit	0:1e09cd7d66b4	1454	pulseRised2=0;
nerit	0:1e09cd7d66b4	1455	}
nerit	0:1e09cd7d66b4	1456	// introduce il controllo di corrente
nerit	0:1e09cd7d66b4	1457	if (incrementCurrent){
nerit	0:1e09cd7d66b4	1458	boostDcOut +=0.005f;
nerit	0:1e09cd7d66b4	1459	}
nerit	0:1e09cd7d66b4	1460	if (reduceCurrent){
nerit	0:1e09cd7d66b4	1461	boostDcOut -=0.005f;
nerit	0:1e09cd7d66b4	1462	}
nerit	0:1e09cd7d66b4	1463	if (boostDcOut >= 0.2f){
nerit	0:1e09cd7d66b4	1464	boostDcOut=0.2f;
nerit	0:1e09cd7d66b4	1465	all_genericals=1;
nerit	0:1e09cd7d66b4	1466	}
nerit	0:1e09cd7d66b4	1467	if (boostDcOut <=-0.2f){
nerit	0:1e09cd7d66b4	1468	boostDcOut=-0.2f;
nerit	0:1e09cd7d66b4	1469	all_genericals=1;
nerit	0:1e09cd7d66b4	1470	}
nerit	1:e88bf5011af6	1471	//correzione += boostDcOut;
nerit	0:1e09cd7d66b4	1472	#if defined(pcSerial)
nerit	0:1e09cd7d66b4	1473	#if defined(Qnca)
nerit	0:1e09cd7d66b4	1474	pc.printf(" DUTY PRE FINALE: %f",dcActualDuty);
nerit	0:1e09cd7d66b4	1475	#endif
nerit	0:1e09cd7d66b4	1476	#endif
nerit	0:1e09cd7d66b4	1477	// prova a mantenere il quinconce quando attivo
nerit	1:e88bf5011af6	1478	//dcActualDuty=dcActualDuty;
nerit	1:e88bf5011af6	1479	/*
nerit	1:e88bf5011af6	1480	#if !defined(speedMaster)
nerit	0:1e09cd7d66b4	1481	#if defined(mezzo)
nerit	0:1e09cd7d66b4	1482	if (quincEnable==1){
nerit	0:1e09cd7d66b4	1483	if (quinconceActive==1){
nerit	0:1e09cd7d66b4	1484	quinCalc(2.0f);
nerit	0:1e09cd7d66b4	1485	}else{
nerit	0:1e09cd7d66b4	1486	quinCalc(1.0f);
nerit	0:1e09cd7d66b4	1487	}
nerit	0:1e09cd7d66b4	1488	}else{
nerit	1:e88bf5011af6	1489	quincMemoEnable=false;
nerit	0:1e09cd7d66b4	1490	}
nerit	0:1e09cd7d66b4	1491	#else
nerit	0:1e09cd7d66b4	1492	quinCalc(1.0f);
nerit	0:1e09cd7d66b4	1493	#endif
nerit	0:1e09cd7d66b4	1494	#endif
nerit	1:e88bf5011af6	1495	*/
nerit	1:e88bf5011af6	1496	DC_brake=0;
nerit	1:e88bf5011af6	1497	DC_forward=1;
nerit	1:e88bf5011af6	1498	DC_prepare();
nerit	0:1e09cd7d66b4	1499
nerit	0:1e09cd7d66b4	1500	// il semiperiodoreale è calcolato sulla lettura del passaggio becchi reale
nerit	0:1e09cd7d66b4	1501	if (correggiSD == true){
nerit	0:1e09cd7d66b4	1502	seedWheelPeriod = semiPeriodoReale / abs(percento);
nerit	0:1e09cd7d66b4	1503	}else{
nerit	0:1e09cd7d66b4	1504	seedWheelPeriod = semiPeriodoReale;
nerit	0:1e09cd7d66b4	1505	}
nerit	0:1e09cd7d66b4	1506	if (seedWheelPeriod < 180.0f){seedWheelPeriod = 180.0f;}
nerit	0:1e09cd7d66b4	1507	if ((oldSeedWheelPeriod!=seedWheelPeriod)&&(seedWheelPeriod >=180.0f )){
nerit	0:1e09cd7d66b4	1508	SDticker.attach_us(&step_SDPulseOut,seedWheelPeriod); // clock time are milliseconds and attach seed motor stepper controls
nerit	0:1e09cd7d66b4	1509	oldSeedWheelPeriod=seedWheelPeriod;
nerit	0:1e09cd7d66b4	1510	}
nerit	0:1e09cd7d66b4	1511
nerit	0:1e09cd7d66b4	1512	//if (correzione > (dutyTeorico0.20)){correzione = dutyTeorico0.20;}
nerit	1:e88bf5011af6	1513	if (correzione > 0.30f){correzione = 0.30f;}
nerit	1:e88bf5011af6	1514	if (correzione < -0.30f){correzione = -0.30f;}
nerit	0:1e09cd7d66b4	1515	if((enableSDcontrol==1)){
nerit	0:1e09cd7d66b4	1516	if (correzioneAttiva==1){
nerit	0:1e09cd7d66b4	1517	dcActualDuty = dutyTeorico + correzione;
nerit	1:e88bf5011af6	1518	//if (dcActualDuty<dcStarting){dcActualDuty=dcStarting;}
nerit	0:1e09cd7d66b4	1519	}
nerit	0:1e09cd7d66b4	1520	}else{
nerit	0:1e09cd7d66b4	1521	dcActualDuty = dutyTeorico;
nerit	0:1e09cd7d66b4	1522	}
nerit	0:1e09cd7d66b4	1523	if (dcActualDuty <0.0f){dcActualDuty=0.0f;}
nerit	0:1e09cd7d66b4	1524	if (dcActualDuty > 1.0f){dcActualDuty = dcMaxSpeed;}
nerit	0:1e09cd7d66b4	1525	#if defined(pcSerial)
nerit	0:1e09cd7d66b4	1526	#if defined(Qnca)
nerit	0:1e09cd7d66b4	1527	pc.printf(" DUTY FINALE: %f \n",dcActualDuty);
nerit	0:1e09cd7d66b4	1528	#endif
nerit	0:1e09cd7d66b4	1529	#endif
nerit	0:1e09cd7d66b4	1530	SDmotorPWM.write(dcActualDuty);
nerit	0:1e09cd7d66b4	1531	// allarme
nerit	0:1e09cd7d66b4	1532	if (SDwheelTimer.read_ms()>4000){
nerit	0:1e09cd7d66b4	1533	if (firstStart==0){
nerit	0:1e09cd7d66b4	1534	all_noDcRotati=1;
nerit	0:1e09cd7d66b4	1535	}
nerit	0:1e09cd7d66b4	1536	#if defined(pcSerial)
nerit	0:1e09cd7d66b4	1537	#if defined(VediAllarmi)
nerit	0:1e09cd7d66b4	1538	pc.printf("allarme no DC rotation");
nerit	0:1e09cd7d66b4	1539	#endif
nerit	0:1e09cd7d66b4	1540	#endif
nerit	0:1e09cd7d66b4	1541
nerit	0:1e09cd7d66b4	1542	}
nerit	0:1e09cd7d66b4	1543
nerit	0:1e09cd7d66b4	1544	//***************************************************************************************************************
nerit	0:1e09cd7d66b4	1545	// CONTROLLA TAMBURO
nerit	0:1e09cd7d66b4	1546	//***************************************************************************************************************
nerit	0:1e09cd7d66b4	1547	if(lowSpeed==0){
nerit	0:1e09cd7d66b4	1548	if (syncroCheck==1){
nerit	0:1e09cd7d66b4	1549	syncroCheck=0;
nerit	0:1e09cd7d66b4	1550	lockStart=1;
nerit	0:1e09cd7d66b4	1551	periodo = TBperiod;
nerit	0:1e09cd7d66b4	1552	if (aspettaStart==0){cambiaTB(periodo);}
nerit	0:1e09cd7d66b4	1553	}
nerit	0:1e09cd7d66b4	1554	// controllo di stop
nerit	0:1e09cd7d66b4	1555	double memoIntraP = (double)memoIntraPick*1.8f;
nerit	0:1e09cd7d66b4	1556	if ((double)rotationTimeOut.read_ms()> (memoIntraP)){
nerit	0:1e09cd7d66b4	1557	syncroCheck=0;
nerit	0:1e09cd7d66b4	1558	aspettaStart=1;
nerit	0:1e09cd7d66b4	1559	countCicli=0;
nerit	0:1e09cd7d66b4	1560	if (TBzeroCyclePulse==1){TBticker.detach();}
nerit	0:1e09cd7d66b4	1561	}
nerit	0:1e09cd7d66b4	1562	}else{ // fine ciclo fuori da low speed
nerit	0:1e09cd7d66b4	1563	syncroCheck=0;
nerit	0:1e09cd7d66b4	1564	lockStart=0;
nerit	0:1e09cd7d66b4	1565	if (beccoPronto==1){
nerit	0:1e09cd7d66b4	1566	if (tamburoStandard==1){
nerit	0:1e09cd7d66b4	1567	double ritardoMassimo = 0.0f;
nerit	0:1e09cd7d66b4	1568	if(speedFromPick==1){
nerit	0:1e09cd7d66b4	1569	ritardoMassimo = (double)timeIntraPick;
nerit	0:1e09cd7d66b4	1570	}else{
nerit	0:1e09cd7d66b4	1571	ritardoMassimo = (double)memoTimeHole;
nerit	0:1e09cd7d66b4	1572	}
nerit	0:1e09cd7d66b4	1573	int tempoDiSincro = (int)((double)(ritardoMassimo - ((tempoBecco/2.0f)+((speedOfSeedWheel/1.25f)*ritardoMassimo)))); //
nerit	0:1e09cd7d66b4	1574	if (tempoDiSincro <= 1){tempoDiSincro=1;}
nerit	0:1e09cd7d66b4	1575	if ((sincroTimer.read_ms()>= tempoDiSincro)){
nerit	0:1e09cd7d66b4	1576	if (tractorSpeed_MtS_timed >= minWorkSpeed){startCicloTB=1;}
nerit	0:1e09cd7d66b4	1577	beccoPronto=0;
nerit	0:1e09cd7d66b4	1578	}
nerit	0:1e09cd7d66b4	1579	}else{
nerit	0:1e09cd7d66b4	1580	// tamburo per zucca
nerit	0:1e09cd7d66b4	1581	if (speedOfSeedWheel >= minWorkSpeed){startCicloTB=1;}
nerit	0:1e09cd7d66b4	1582	beccoPronto=0;
nerit	0:1e09cd7d66b4	1583	}
nerit	0:1e09cd7d66b4	1584	}
nerit	0:1e09cd7d66b4	1585	ciclaTB();
nerit	0:1e09cd7d66b4	1586	}
nerit	0:1e09cd7d66b4	1587	//*************************************************************
nerit	0:1e09cd7d66b4	1588	}else{ // fine ciclo con velocita maggiore di 0
nerit	0:1e09cd7d66b4	1589	SDwheelTimer.stop();
nerit	0:1e09cd7d66b4	1590	SDwheelTimer.reset();
nerit	0:1e09cd7d66b4	1591	checkSDrotation=0;
nerit	0:1e09cd7d66b4	1592	oldFaseLavoro=0;
nerit	0:1e09cd7d66b4	1593	aspettaStart=1;
nerit	0:1e09cd7d66b4	1594	countCicli=0;
nerit	0:1e09cd7d66b4	1595	startCycle=0;
nerit	0:1e09cd7d66b4	1596	oldSeedWheelPeriod=0.0f;
nerit	0:1e09cd7d66b4	1597	oldPeriodoTB=0.0f;
nerit	0:1e09cd7d66b4	1598	correzione=0.0f;
nerit	0:1e09cd7d66b4	1599	OLDpulseSpeedInterval=1000.01f;
nerit	0:1e09cd7d66b4	1600	cicloTbinCorso=0;
nerit	0:1e09cd7d66b4	1601	cntTbError=0;
nerit	0:1e09cd7d66b4	1602	if (cycleStopRequest==1){
nerit	0:1e09cd7d66b4	1603	zeroDelay.reset();
nerit	0:1e09cd7d66b4	1604	zeroDelay.start();
nerit	0:1e09cd7d66b4	1605	runRequestBuf=0;
nerit	0:1e09cd7d66b4	1606	zeroRequestBuf=1;
nerit	0:1e09cd7d66b4	1607	cycleStopRequest=0;
nerit	0:1e09cd7d66b4	1608	SDzeroCyclePulse=0;
nerit	0:1e09cd7d66b4	1609	TBzeroCyclePulse=0;
nerit	0:1e09cd7d66b4	1610	zeroCycleEnd=0;
nerit	0:1e09cd7d66b4	1611	zeroCycle=1;
nerit	0:1e09cd7d66b4	1612	zeroTrigger=0;
nerit	0:1e09cd7d66b4	1613	}
nerit	0:1e09cd7d66b4	1614	}
nerit	0:1e09cd7d66b4	1615	//*************************************************************************************************
nerit	0:1e09cd7d66b4	1616	// ciclo di azzeramento motori
nerit	0:1e09cd7d66b4	1617	if ((zeroCycleEnd==0)&&(zeroCycle==1)){//&&(zeroDelay.read_ms()>10000)){
nerit	0:1e09cd7d66b4	1618	if (zeroTrigger==0){
nerit	0:1e09cd7d66b4	1619	TBticker.attach_us(&step_TBPulseOut,1000.0f); // clock time are seconds and attach seed motor stepper controls
nerit	0:1e09cd7d66b4	1620	DC_forward=1;
nerit	0:1e09cd7d66b4	1621	DC_brake=1;
nerit	0:1e09cd7d66b4	1622	DC_prepare();
nerit	0:1e09cd7d66b4	1623	frenata=0;
nerit	0:1e09cd7d66b4	1624	zeroTrigger=1;
nerit	0:1e09cd7d66b4	1625	ritardoStop.reset();
nerit	0:1e09cd7d66b4	1626	ritardoComandoStop.reset();
nerit	0:1e09cd7d66b4	1627	ritardoComandoStop.start();
nerit	0:1e09cd7d66b4	1628	timeOutZeroSD.start();
nerit	0:1e09cd7d66b4	1629	quincTimeSet.reset();
nerit	0:1e09cd7d66b4	1630	}
nerit	0:1e09cd7d66b4	1631	int tempoFrenata=300;
nerit	0:1e09cd7d66b4	1632	if (((ritardoStop.read_ms()>tempoFrenata)&&(SDzeroDebounced==0))\|\|(accensione==1)\|\|(ritardoComandoStop.read_ms()>400)){
nerit	0:1e09cd7d66b4	1633	accensione=0;
nerit	0:1e09cd7d66b4	1634	avvicinamento=1;
nerit	0:1e09cd7d66b4	1635	avvicinamentoOn.reset();
nerit	0:1e09cd7d66b4	1636	avvicinamentoOn.start();
nerit	0:1e09cd7d66b4	1637	SDmotorPWM.write(0.32f); // duty cycle = 60% of power
nerit	0:1e09cd7d66b4	1638	DC_forward=1;
nerit	0:1e09cd7d66b4	1639	DC_brake=0;
nerit	0:1e09cd7d66b4	1640	DC_prepare();
nerit	0:1e09cd7d66b4	1641	ritardoComandoStop.reset();
nerit	0:1e09cd7d66b4	1642	ritardoComandoStop.stop();
nerit	0:1e09cd7d66b4	1643	}
nerit	0:1e09cd7d66b4	1644	if (avvicinamento==1){
nerit	0:1e09cd7d66b4	1645	if(avvicinamentoOn.read_ms()>300){
nerit	0:1e09cd7d66b4	1646	SDmotorPWM.write(0.7f);
nerit	0:1e09cd7d66b4	1647	avvicinamentoOn.reset();
nerit	0:1e09cd7d66b4	1648	avvicinamentoOff.reset();
nerit	0:1e09cd7d66b4	1649	avvicinamentoOff.start();
nerit	0:1e09cd7d66b4	1650	}
nerit	0:1e09cd7d66b4	1651	if(avvicinamentoOff.read_ms()>100){
nerit	0:1e09cd7d66b4	1652	SDmotorPWM.write(0.32f);
nerit	0:1e09cd7d66b4	1653	avvicinamentoOff.reset();
nerit	0:1e09cd7d66b4	1654	avvicinamentoOff.stop();
nerit	0:1e09cd7d66b4	1655	avvicinamentoOn.start();
nerit	0:1e09cd7d66b4	1656	}
nerit	0:1e09cd7d66b4	1657	}else{
nerit	0:1e09cd7d66b4	1658	avvicinamentoOn.stop();
nerit	0:1e09cd7d66b4	1659	avvicinamentoOff.stop();
nerit	0:1e09cd7d66b4	1660	avvicinamentoOn.reset();
nerit	0:1e09cd7d66b4	1661	avvicinamentoOff.reset();
nerit	0:1e09cd7d66b4	1662	}
nerit	0:1e09cd7d66b4	1663	if (frenata==0){
nerit	0:1e09cd7d66b4	1664	if (SDzeroCyclePulse==1){
nerit	0:1e09cd7d66b4	1665	SDticker.detach();
nerit	0:1e09cd7d66b4	1666	frenata=1;
nerit	0:1e09cd7d66b4	1667	quincTimeSet.reset();
nerit	0:1e09cd7d66b4	1668	quincTimeSet.start();
nerit	0:1e09cd7d66b4	1669	ritardoStop.start();
nerit	0:1e09cd7d66b4	1670	//ritardoComandoStop.reset();
nerit	0:1e09cd7d66b4	1671	//ritardoComandoStop.stop();
nerit	0:1e09cd7d66b4	1672	}
nerit	0:1e09cd7d66b4	1673	}else{
nerit	0:1e09cd7d66b4	1674	if (quinconceActive==0){
nerit	0:1e09cd7d66b4	1675	if (SDzeroCyclePulse==1){
nerit	0:1e09cd7d66b4	1676	avvicinamento=0;
nerit	0:1e09cd7d66b4	1677	DC_brake=1;
nerit	0:1e09cd7d66b4	1678	DC_prepare();
nerit	0:1e09cd7d66b4	1679	SDzeroed=1;
nerit	0:1e09cd7d66b4	1680	ritardoStop.reset();
nerit	0:1e09cd7d66b4	1681	ritardoStop.stop();
nerit	0:1e09cd7d66b4	1682	}
nerit	0:1e09cd7d66b4	1683	}else{
nerit	0:1e09cd7d66b4	1684	if (quincTimeSet.read_ms()>500){
nerit	0:1e09cd7d66b4	1685	avvicinamento=0;
nerit	0:1e09cd7d66b4	1686	DC_brake=1;
nerit	0:1e09cd7d66b4	1687	DC_prepare();
nerit	0:1e09cd7d66b4	1688	SDzeroed=1;
nerit	0:1e09cd7d66b4	1689	ritardoStop.reset();
nerit	0:1e09cd7d66b4	1690	ritardoStop.stop();
nerit	0:1e09cd7d66b4	1691	quincTimeSet.stop();
nerit	0:1e09cd7d66b4	1692	}
nerit	0:1e09cd7d66b4	1693	}
nerit	0:1e09cd7d66b4	1694	}
nerit	0:1e09cd7d66b4	1695	// azzera tutto in time out
nerit	0:1e09cd7d66b4	1696	if (timeOutZeroSD.read_ms()>=10000){
nerit	0:1e09cd7d66b4	1697	#if defined(pcSerial)
nerit	0:1e09cd7d66b4	1698	#if defined(canDataReceived)
nerit	0:1e09cd7d66b4	1699	pc.printf("allarme no zero");
nerit	0:1e09cd7d66b4	1700	#endif
nerit	0:1e09cd7d66b4	1701	#endif
nerit	0:1e09cd7d66b4	1702	if (firstStart==0){
nerit	0:1e09cd7d66b4	1703	all_no_Zeroing=1;
nerit	0:1e09cd7d66b4	1704	}
nerit	0:1e09cd7d66b4	1705	avvicinamento=0;
nerit	0:1e09cd7d66b4	1706	DC_brake=1;
nerit	0:1e09cd7d66b4	1707	DC_prepare();
nerit	0:1e09cd7d66b4	1708	SDzeroed=1;
nerit	0:1e09cd7d66b4	1709	ritardoStop.reset();
nerit	0:1e09cd7d66b4	1710	ritardoStop.stop();
nerit	0:1e09cd7d66b4	1711	avvicinamentoOn.stop();
nerit	0:1e09cd7d66b4	1712	avvicinamentoOff.stop();
nerit	0:1e09cd7d66b4	1713	avvicinamentoOn.reset();
nerit	0:1e09cd7d66b4	1714	avvicinamentoOff.reset();
nerit	0:1e09cd7d66b4	1715	ritardoComandoStop.reset();
nerit	0:1e09cd7d66b4	1716	ritardoComandoStop.stop();
nerit	0:1e09cd7d66b4	1717	timeOutZeroSD.stop();
nerit	0:1e09cd7d66b4	1718	timeOutZeroSD.reset();
nerit	0:1e09cd7d66b4	1719	}
nerit	0:1e09cd7d66b4	1720	if (TBzeroCyclePulse==1){
nerit	0:1e09cd7d66b4	1721	TBticker.detach();
nerit	0:1e09cd7d66b4	1722	TBzeroed=1;
nerit	0:1e09cd7d66b4	1723	}
nerit	0:1e09cd7d66b4	1724	if ((SDzeroed==1)&&(TBzeroed==1)){
nerit	0:1e09cd7d66b4	1725	avvicinamentoOn.stop();
nerit	0:1e09cd7d66b4	1726	avvicinamentoOff.stop();
nerit	0:1e09cd7d66b4	1727	ritardoComandoStop.stop();
nerit	0:1e09cd7d66b4	1728	ritardoStop.stop();
nerit	0:1e09cd7d66b4	1729	zeroCycleEnd=1;
nerit	0:1e09cd7d66b4	1730	zeroCycle=0;
nerit	0:1e09cd7d66b4	1731	zeroTrigger=0;
nerit	0:1e09cd7d66b4	1732	runRequestBuf=1;
nerit	0:1e09cd7d66b4	1733	zeroRequestBuf=0;
nerit	0:1e09cd7d66b4	1734	cycleStopRequest=0;
nerit	0:1e09cd7d66b4	1735	SDzeroed=0;
nerit	0:1e09cd7d66b4	1736	TBzeroed=0;
nerit	0:1e09cd7d66b4	1737	timeOutZeroSD.stop();
nerit	0:1e09cd7d66b4	1738	timeOutZeroSD.reset();
nerit	0:1e09cd7d66b4	1739	}
nerit	0:1e09cd7d66b4	1740	}
nerit	0:1e09cd7d66b4	1741
nerit	0:1e09cd7d66b4	1742	//*************************************************************************************************
nerit	0:1e09cd7d66b4	1743	if (enableCycle==0){
nerit	0:1e09cd7d66b4	1744	zeroTrigger=0;
nerit	0:1e09cd7d66b4	1745	SDmotorPWM=0;
nerit	0:1e09cd7d66b4	1746	SDmotorInA=0;
nerit	0:1e09cd7d66b4	1747	SDmotorInB=0;
nerit	0:1e09cd7d66b4	1748	}
nerit	0:1e09cd7d66b4	1749	SDzeroCyclePulse=0;
nerit	0:1e09cd7d66b4	1750	TBzeroCyclePulse=0;
nerit	0:1e09cd7d66b4	1751	//*************************************************************************************************
nerit	0:1e09cd7d66b4	1752	}else{//end ciclo normale
nerit	0:1e09cd7d66b4	1753	//*************************************************************************************************
nerit	0:1e09cd7d66b4	1754	// task di prova della scheda
nerit	0:1e09cd7d66b4	1755	//*************************************************************************************************
nerit	0:1e09cd7d66b4	1756	#if defined(provaScheda)
nerit	0:1e09cd7d66b4	1757	clocca++;
nerit	0:1e09cd7d66b4	1758	//led = !led;
nerit	0:1e09cd7d66b4	1759	//txMsg.clear();
nerit	0:1e09cd7d66b4	1760	//txMsg << clocca;
nerit	0:1e09cd7d66b4	1761	//test.printf("aogs \n");
nerit	0:1e09cd7d66b4	1762	//if(can1.write(txMsg)){
nerit	0:1e09cd7d66b4	1763	#if defined(pcSerial)
nerit	0:1e09cd7d66b4	1764	pc.printf("Can write OK \n");
nerit	0:1e09cd7d66b4	1765	#endif
nerit	0:1e09cd7d66b4	1766	//}
nerit	0:1e09cd7d66b4	1767	switch (clocca){
nerit	0:1e09cd7d66b4	1768	case 1:
nerit	0:1e09cd7d66b4	1769	TBmotorStepOut=1; // define step command for up down motor driver
nerit	0:1e09cd7d66b4	1770	break;
nerit	0:1e09cd7d66b4	1771	case 2:
nerit	0:1e09cd7d66b4	1772	SDmotorPWM=1; // define step command for seeding whell motor driver
nerit	0:1e09cd7d66b4	1773	break;
nerit	0:1e09cd7d66b4	1774	case 3:
nerit	0:1e09cd7d66b4	1775	speedClock=1; // define input of
nerit	0:1e09cd7d66b4	1776	break;
nerit	0:1e09cd7d66b4	1777	case 4:
nerit	0:1e09cd7d66b4	1778	break;
nerit	0:1e09cd7d66b4	1779	case 5:
nerit	0:1e09cd7d66b4	1780	SDmotorInA=1;
nerit	0:1e09cd7d66b4	1781	SDmotorInB=0;
nerit	0:1e09cd7d66b4	1782	break;
nerit	0:1e09cd7d66b4	1783	case 6:
nerit	0:1e09cd7d66b4	1784	break;
nerit	0:1e09cd7d66b4	1785	case 7:
nerit	0:1e09cd7d66b4	1786	break;
nerit	0:1e09cd7d66b4	1787	case 8:
nerit	0:1e09cd7d66b4	1788	break;
nerit	0:1e09cd7d66b4	1789	case 9:
nerit	0:1e09cd7d66b4	1790	break;
nerit	0:1e09cd7d66b4	1791	case 10:
nerit	0:1e09cd7d66b4	1792	break;
nerit	0:1e09cd7d66b4	1793	case 11:
nerit	0:1e09cd7d66b4	1794	break;
nerit	0:1e09cd7d66b4	1795	case 12:
nerit	0:1e09cd7d66b4	1796	break;
nerit	0:1e09cd7d66b4	1797	case 13:
nerit	0:1e09cd7d66b4	1798	break;
nerit	0:1e09cd7d66b4	1799	case 14:
nerit	0:1e09cd7d66b4	1800	SDmotorPWM=1; // power mosfet 2 command out
nerit	0:1e09cd7d66b4	1801	break;
nerit	0:1e09cd7d66b4	1802	case 15:
nerit	0:1e09cd7d66b4	1803	break;
nerit	0:1e09cd7d66b4	1804	case 16:
nerit	0:1e09cd7d66b4	1805	case 17:
nerit	0:1e09cd7d66b4	1806	break;
nerit	0:1e09cd7d66b4	1807	case 18:
nerit	0:1e09cd7d66b4	1808	TBmotorStepOut=0; // define step command for up down motor driver
nerit	0:1e09cd7d66b4	1809	SDmotorPWM=0; // define step command for seeding whell motor driver
nerit	0:1e09cd7d66b4	1810	speedClock=0; // define input of
nerit	0:1e09cd7d66b4	1811	SDmotorInA=0;
nerit	0:1e09cd7d66b4	1812	SDmotorInB=0;
nerit	0:1e09cd7d66b4	1813	SDmotorPWM=0; // power mosfet 2 command out
nerit	0:1e09cd7d66b4	1814	break;
nerit	0:1e09cd7d66b4	1815	default:
nerit	0:1e09cd7d66b4	1816	clocca=0;
nerit	0:1e09cd7d66b4	1817	break;
nerit	0:1e09cd7d66b4	1818	}
nerit	0:1e09cd7d66b4	1819	wait_ms(100);
nerit	0:1e09cd7d66b4	1820	#endif // end prova scheda
nerit	0:1e09cd7d66b4	1821
nerit	0:1e09cd7d66b4	1822	#if defined(provaDC)
nerit	0:1e09cd7d66b4	1823	int rampa=1000;
nerit	0:1e09cd7d66b4	1824	int pausa=3000;
nerit	0:1e09cd7d66b4	1825	switch (clocca){
nerit	0:1e09cd7d66b4	1826	case 0:
nerit	0:1e09cd7d66b4	1827	DC_brake=0;
nerit	0:1e09cd7d66b4	1828	DC_forward=1;
nerit	0:1e09cd7d66b4	1829	duty_DC+=0.01f;
nerit	0:1e09cd7d66b4	1830	if (duty_DC>=0.2f){
nerit	0:1e09cd7d66b4	1831	duty_DC=0.2f;
nerit	0:1e09cd7d66b4	1832	clocca=1;
nerit	0:1e09cd7d66b4	1833	}
nerit	0:1e09cd7d66b4	1834	wait_ms(rampa);
nerit	0:1e09cd7d66b4	1835	break;
nerit	0:1e09cd7d66b4	1836	case 1:
nerit	0:1e09cd7d66b4	1837	wait_ms(pausa*4);
nerit	0:1e09cd7d66b4	1838	clocca=2;
nerit	0:1e09cd7d66b4	1839	break;
nerit	0:1e09cd7d66b4	1840	case 2:
nerit	0:1e09cd7d66b4	1841	DC_brake=0;
nerit	0:1e09cd7d66b4	1842	DC_forward=1;
nerit	0:1e09cd7d66b4	1843	duty_DC-=0.01f;
nerit	0:1e09cd7d66b4	1844	if (duty_DC<=0.0f){
nerit	0:1e09cd7d66b4	1845	duty_DC=0.0f;
nerit	0:1e09cd7d66b4	1846	clocca=3;
nerit	0:1e09cd7d66b4	1847	}
nerit	0:1e09cd7d66b4	1848	wait_ms(rampa);
nerit	0:1e09cd7d66b4	1849	break;
nerit	0:1e09cd7d66b4	1850	case 3:
nerit	0:1e09cd7d66b4	1851	wait_ms(pausa);
nerit	0:1e09cd7d66b4	1852	clocca=4;
nerit	0:1e09cd7d66b4	1853	break;
nerit	0:1e09cd7d66b4	1854	case 4:
nerit	0:1e09cd7d66b4	1855	DC_brake=0;
nerit	0:1e09cd7d66b4	1856	DC_forward=1;
nerit	0:1e09cd7d66b4	1857	duty_DC+=0.01f;
nerit	0:1e09cd7d66b4	1858	if (duty_DC>=1.0f){
nerit	0:1e09cd7d66b4	1859	duty_DC=1.0f;
nerit	0:1e09cd7d66b4	1860	clocca=5;
nerit	0:1e09cd7d66b4	1861	}
nerit	0:1e09cd7d66b4	1862	wait_ms(rampa);
nerit	0:1e09cd7d66b4	1863	break;
nerit	0:1e09cd7d66b4	1864	case 5:
nerit	0:1e09cd7d66b4	1865	wait_ms(pausa);
nerit	0:1e09cd7d66b4	1866	clocca=6;
nerit	0:1e09cd7d66b4	1867	break;
nerit	0:1e09cd7d66b4	1868	case 6:
nerit	0:1e09cd7d66b4	1869	DC_brake=0;
nerit	0:1e09cd7d66b4	1870	DC_forward=1;
nerit	0:1e09cd7d66b4	1871	duty_DC-=0.01f;
nerit	0:1e09cd7d66b4	1872	if (duty_DC<=0.0f){
nerit	0:1e09cd7d66b4	1873	duty_DC=0.0f;
nerit	0:1e09cd7d66b4	1874	clocca=7;
nerit	0:1e09cd7d66b4	1875	}
nerit	0:1e09cd7d66b4	1876	wait_ms(rampa);
nerit	0:1e09cd7d66b4	1877	break;
nerit	0:1e09cd7d66b4	1878	case 7:
nerit	0:1e09cd7d66b4	1879	wait_ms(pausa);
nerit	0:1e09cd7d66b4	1880	clocca=0;
nerit	0:1e09cd7d66b4	1881	break;
nerit	0:1e09cd7d66b4	1882	default:
nerit	0:1e09cd7d66b4	1883	break;
nerit	0:1e09cd7d66b4	1884	}
nerit	0:1e09cd7d66b4	1885	if (oldDuty_DC != duty_DC){
nerit	0:1e09cd7d66b4	1886	SDmotorPWM.write(duty_DC); // duty cycle = stop
nerit	0:1e09cd7d66b4	1887	oldDuty_DC=duty_DC;
nerit	0:1e09cd7d66b4	1888	DC_prepare();
nerit	0:1e09cd7d66b4	1889	}
nerit	0:1e09cd7d66b4	1890	#endif
nerit	0:1e09cd7d66b4	1891	}//end in prova
nerit	0:1e09cd7d66b4	1892	wd.Service(); // kick the dog before the timeout
nerit	0:1e09cd7d66b4	1893	} // end while
nerit	0:1e09cd7d66b4	1894	}// end main
nerit	0:1e09cd7d66b4	1895

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	31 Jan 2019
Imports:	0
Forks:	0
Commits:	17
Dependents:	0
Dependencies:	2
Followers:	1

main.cpp@1:e88bf5011af6, 2018-06-15 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning