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@12:5bfbccfb3cf4, 2018-12-29 (annotated)

Committer:: nerit
Date:: Sat Dec 29 08:07:25 2018 +0000
Revision:: 12:5bfbccfb3cf4
Parent:: 11:21e990eb31d0
Child:: 13:0ae23132a2b6

Versione corretta ed inviata via mail alle 09:07 del 29/12/18

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

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@12:5bfbccfb3cf4, 2018-12-29 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning