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@0:1e09cd7d66b4, 2018-06-13 (annotated)

Committer:: nerit
Date:: Wed Jun 13 08:52:42 2018 +0000
Revision:: 0:1e09cd7d66b4
Child:: 1:e88bf5011af6

Modula V6 double encoder with quinconce

Who changed what in which revision?

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

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@0:1e09cd7d66b4, 2018-06-13 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning