FORIGO_Modula_V6_R2C_DE_HwV5_10gen2019_2019 - new

Users » francescopistone » Code » FORIGO_Modula_V6_R2C_DE_HwV5_10gen2019_2019

Francesco Pistone / Mbed 2 deprecated FORIGO_Modula_V6_R2C_DE_HwV5_10gen2019_2019

new

main.cpp@4:d32258ec411f, 2018-06-27 (annotated)

Committer:: nerit
Date:: Wed Jun 27 15:33:42 2018 +0000
Revision:: 4:d32258ec411f
Parent:: 3:c0f11ca4df02
Child:: 5:3b95bbfe2dc9

Modifiche parametri dopo prova in campo del quinconce

Who changed what in which revision?

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

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:	16
Dependents:	0
Dependencies:	2
Followers:	1

main.cpp@4:d32258ec411f, 2018-06-27 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning