Forigo
Messa in campo 4 file - 26/06/2020 Francia
Dependencies: mbed X_NUCLEO_IHM03A1_for
Fork of
FORIGO_Modula_V7_3_VdcStep_maggio2020
by 
Francesco Pistone
main.cpp
- Committer:
- nerit
- Date:
- 2020-04-27
- Revision:
- 38:79af1c65dd6f
- Parent:
- 37:0a225902cf48
- Child:
- 39:6814c75dfa5c
File content as of revision 38:79af1c65dd6f:
//********************************************************************************************************************
//********************************************************************************************************************
// FIRMWARE SEMINATRICE MODULA
// VERSIONE PER SCHEDA DI CONTROLLO CON DRIVER INTEGRATI
// V7 - ATTENZIONE - LA VERSIONE V7 HA IL DRIVER STEPPER POWERSTEP01 DA 10A
// IL PROCESSORE UTILIZZATO E' IL STM32L476RG A 80MHz
// IL MOTORE DC E' GESTITO CON IL DRIVER VNH3SP30-E E CON LA LETTURA
// DELLA CORRENTE ASSORBITA TRAMITE IL CONVERTITORE MLX91210-CAS102 CON 50A FONDOSCALA
// CHE FORNISCE UNA TENSIONE DI USCITA PARI A 40mV/A
// FIRST RELEASE OF BOARD DEC 2017
// FIRST RELEASE OF FIRMWARE JAN 2018
//
// THIS RELEASE: 10 february 2019
//
// APPLICATION: MODULA CON DISTRIBUTORE RISO ED ENCODER MOTORE
//
// 29 05 2018 - INSERITO SECONDO ENCODER VIRTUALE PER LA GESTIONE DEL SINCRONISMO TRA TAMBURO E RUOTA DI SEMINA
// IN PRATICA IL PRIMO ENCODER è SINCRONO CON IL SEGNALE DEI BECCHI E VIENE AZZERATO DA QUESTI, MENTRE
// IL SECONDO E' INCREMENTATO IN SINCRONO CON IL PRIMO MA AZZERATO DALLA FASE. IL SUO VALORE E' POI DIVISO
// PER IL RAPPORTO RUOTE E LA CORREZIONE AGISCE SULLA VELOCITA' DEL TAMBURO PER MANTENERE LA FASE DEL SECONDO
// ENCODER
// 05 06 2018 - INSERITO IL CONTROLLO DI GESTIONE DEL QUINCONCE SENZA ENCODER
// 09 06 2018 - INSERITO CONTROLLO DI FASE CON ENCODER MASTER PER QUINCONCE - DATO SCAMBIATO IN CAN
// 03 01 2019 - INSERITA GESTIONE IN RTOS PER IL DRIVER POWERSTEP01
// 10 02 2019 - INSERITO FUNZIONAMENTO STEPPER IN MODALITA' CONTROLLO DI TENSIONE E STEP DA CLOCKPIN
// 16 02 2019 - SOSTITUITA LIBRERIA MBED PER PROBLEMI DI COMPILAZIONE DEL FIRMWARE
/********************
IL FIRMWARE SI COMPONE DI 10 FILES:
- main.cpp
- main.hpp
- iodefinition.hpp
- canbus.hpp
- parameters.hpp
- powerstep.hpp
- timeandtick.hpp
- variables.hpp
- powerstep.hpp
- watchdog.cpp
- watchdog.h
ED UTILIZZA LE LIBRERIE STANDARD MBED PIU'
UNA LIBRERIA MODIFICATA E DEDICATA PER IL CAN
UNA LIBRERIA DEDICATA PER IL DRIVER STEPPER
*********************
LA MACCHINA UTILIZZA SEMPRE 2 SOLI SENSORI; UNO PER SENTIRE LE CELLE DI CARICO SEME ED UNO PER SENTIRE I BECCHI DI SEMINA.
GLI AZIONAMENTI SONO COMPOSTI DA DUE MOTORI; UN DC PER IL CONTROLLO DELLA RUOTA DI SEMINA ED UNO STEPPER PER IL CONTROLLO DEL TAMBURO
UN SENSORE AGGIUNTIVO SULL'ELEMENTO MASTER RILEVA LA VELOCITA' DI AVANZAMENTO
UN SENSORE AGGIUNTIVO SULLA RUOTA DI SEMINA RILEVA LA ROTAZIONE DELLA RUOTA STESSA ATTRAVERSO FORI PRESENTI SUL DISCO DI SEMINA
*********************
LA LOGICA GENERALE PREVEDE CHE IL DC DELLA RUOTA DI SEMINA VENGA COMANDATO IN FUNZIONE DELLA VELOCITA' LETTA DAL SENSORE DI AVANZAMAENTO DEL MASTER
IL PROBLEMA PRINCIPALE E' CHE QUANDO I BECCHI SONO INSERITI NEL TERRENO NON VI E' RETROAZIONE REALE SULLA VELOCITA' DI ROTAZIONE DELLA RUOTA STESSA
PROPRIO PERCHE' L'AVANZAMANETO NEL TERRENO IMPRIME UNA VELOCITA' PROPRIA AL BECCO E QUINDI ANCHE ALLA RUOTA.
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
IL VALORE DI RIFERIMENTO DELL'ANALOGICA DI INGRESSO VIENE AGGIORNATO OGNI VOLTA CHE LA RUOTA DI SEMINA E' FERMA
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
LA FASE VIENE DETERMINATA DAL PASSAGGIO DEI BECCHI SUL SENSORE RELATIVO.
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
SOPRATUTTO QUANDO I BECCHI SONO MOLTO DISTANZIATI.
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
TARATO SULLA REALE VELOCITA' DI ROTAZIONE DELLA RUOTA DI SEMINA.
IL PRIMO ENCODER VIRTUALE SI OCCUPA DI DETERMINARE LA POSIZIONE FISICA DELLA RUOTA DI SEMINA E SI AZZERA AL PASSAGGIO DI OGNI BECCO.
IL SECONDO VIENE AZZERATO DALL'IMPULSO DI FASE DEL PRIMO ENCODER DETERMINATO DAI VALORI IMPOSTI SUL TERMINALE TRITECNICA
IL SECONDO ENCODER VIENE CONFRONTATO CON LA POSIZIONE ASSOLUTA DEL TAMBURO (DETERMINATA DAL NUMERO DI STEP EMESSI DAL CONTROLLO), RAPPORTATA TRA CELLE E BECCHI.
IL CONFRONTO DETERMINA LA POSIZIONE RELATIVA DELLA SINGOLA CELLA RISPETTO AL SINGOLO BECCO. IL MANTENIMENTO DELLA SINCRONIZZAZIONE DI FASE, DETERMINA IL SINCRO CELLA/BECCO.
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
DI CAMBIARE L'ANGOLO DI ANTICIPO DI RILASCIO DEL SEME IN FUNZIONE DELLA VELOCITA' E RECUPERARE COSI' IL TEMPO DI VOLO DEL SEME.
IL TAMBURO HA DUE TIPI DI FUNZIONAMENTO: CONTINUO E AD IMPULSI. E' SELEZIONABILE IN FUNZIONE DELLA VELOCITA' E DEL TIPO DI DISTRIBUTORE MONTATO.
**********************
TUTTI I VALORI, CELLE, BECCHI, IMPULSI VELOCITA', ANCGOLO DI AVVIO, FASE DI SEMINA, ECC.. SONO IMPOSTABILI DA PANNELLO OPERATORE
I DATI SONO SCAMBIATI CON IL PANNELLO OPERATORE E CON GLI ALTRI MODULI ATTRAVERSO RETE CAN CON PROTOCOLLO FREESTYLE ATTRAVERSO INDIRIZZAMENTI DEDICATI
AL MOMENTO NON E' POSSIBILE ATTRIBUIRE L'INIDIRIZZO BASE DELL'ELEMENTO DA TERMINALE OPERATORE MA SOLO IN FASE DI COMPILAZIONE DEL FIRMWARE.
**********************
ALTRE SEZIONI RIGUARDANO LA GENERAZIONE DEGLI ALLARMI, LA COMUNICAZIONE CAN, LA SIMULAZIONE DI LAVORO, LA GESTIONE DELLA DIAGNOSI ECC..
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.
LO STEPPER SEGUE IL DC.
**********************
IN FASE DI ACCENSIONE ED OGNI QUALVOLTA SI ARRIVA A VELOCITA' ZERO, LA MACCHINA ESEGUE UN CICLO DI AZZERAMENTO
NON ESISTE PULSANTE DI MARCIA/STOP; E' SEMPRE ATTIVA.
**********************
NEL PROGRAMMA E' PRESENTE UNA SEZIONE DI TEST FISICO DELLA SCHEDA ATTIVABILE SOLO IN FASE DI COMPILAZIONE
**********************
ALTRE FUNZIONI: PRECARICAMENTO DEL TAMBURO
AZZERAMENTO MANUALE
STATISTICA DI SEMINA (CONTA LE CELLE)
*/
//********************************************************************************************************************
//********************************************************************************************************************
#include "main.hpp"
#if defined(runner)
/* Helper header files. */
#include "DevSPI.h"
/* Component specific header files. */
#include "PowerStep01.h"
#endif
#include "timeandtick.hpp"
#include "canbus.hpp"
#include "watchdog.h"
#include "iodefinition.hpp"
#include "parameters.hpp"
#include "variables.hpp"
#if defined(runner)
#include "powerstep.hpp"
#endif
//********************************************************************************************************************
//********************************************************************************************************************
#if defined(runnerTos)
Thread thread;
#endif
/* Variables -----------------------------------------------------------------*/
/* Functions -----------------------------------------------------------------*/
/**
* @brief This is an example of user handler for the flag interrupt.
* @param None
* @retval None
* @note If needed, implement it, and then attach and enable it:
* + motor->attach_flag_irq(&my_flag_irq_handler);
* + motor->enable_flag_irq();
* To disable it:
* + motor->DisbleFlagIRQ();
*/
#if defined(runner)
void my_flag_irq_handler(void)
{
/* Set ISR flag. */
motor->isrFlag = TRUE;
/* Get the value of the status register. */
unsigned int statusRegister = motor->get_status();
#if defined(pcSerial)
pc.printf(" WARNING: \"FLAG\" interrupt triggered.\r\n");
#endif
/* Check SW_F flag: if not set, the SW input is opened */
if ((statusRegister & POWERSTEP01_STATUS_SW_F ) != 0) {
#if defined(pcSerial)
pc.printf(" SW closed (connected to ground).\r\n");
#endif
}
/* Check SW_EN bit */
if ((statusRegister & POWERSTEP01_STATUS_SW_EVN) == POWERSTEP01_STATUS_SW_EVN) {
#if defined(pcSerial)
pc.printf(" SW turn_on event.\r\n");
#endif
}
/* Check Command Error flag: if set, the command received by SPI can't be */
/* performed. This occurs for instance when a move command is sent to the */
/* Powerstep01 while it is already running */
if ((statusRegister & POWERSTEP01_STATUS_CMD_ERROR) == POWERSTEP01_STATUS_CMD_ERROR) {
#if defined(pcSerial)
pc.printf(" Non-performable command detected.\r\n");
#endif
}
/* Check UVLO flag: if not set, there is an undervoltage lock-out */
if ((statusRegister & POWERSTEP01_STATUS_UVLO)==0) {
#if defined(pcSerial)
pc.printf(" undervoltage lock-out.\r\n");
#endif
}
/* Check thermal STATUS flags: if set, the thermal status is not normal */
if ((statusRegister & POWERSTEP01_STATUS_TH_STATUS)!=0) {
//thermal status: 1: Warning, 2: Bridge shutdown, 3: Device shutdown
#if defined(pcSerial)
pc.printf(" Thermal status: %d.\r\n", (statusRegister & POWERSTEP01_STATUS_TH_STATUS)>>11);
#endif
}
/* Check OCD flag: if not set, there is an overcurrent detection */
if ((statusRegister & POWERSTEP01_STATUS_OCD)==0) {
#if defined(pcSerial)
pc.printf(" Overcurrent detection.\r\n");
#endif
}
/* Reset ISR flag. */
motor->isrFlag = FALSE;
}
#endif
/**
* @brief This is an example of error handler.
* @param[in] error Number of the error
* @retval None
* @note If needed, implement it, and then attach it:
* + motor->attach_error_handler(&my_error_handler);
*/
void my_error_handler(uint16_t error)
{
/* Printing to the console. */
#if defined(pcSerial)
pc.printf("Error %d detected\r\n\n", error);
#endif
/* Infinite loop */
//while (true) {
//}
}
//*******************************************************************************
// FREE RUNNING RTOS THREAD FOR DRUM STEPPER POSITION READING
//*******************************************************************************
#if defined(runner)
void step_Reading(){
//while(true){
/* Get current position of device and print to the console */
TBpassPosition= (uint32_t) motor->get_position();
ildato=TBpassPosition;
if (TBpassPosition >= TBoldPosition){
TBactualPosition= ((TBpassPosition-TBoldPosition)*TBreductionRatio);//*10;
}else{
TBperiod=5.2f*TBrpm;
}
//wait_us(50); // 50 mS di intervallo lettura
//}
}
#endif
//*******************************************************************************
//********************************************************************************************************************
// ------------------------------------------------------------------------------------------------------------------------------------------------------------------
// TASK SECTION
// ------------------------------------------------------------------------------------------------------------------------------------------------------------------
//************************************************************************
void aggioVelocita()
{
realGiroSD = seedPerimeter / speedOfSeedWheel;
tempoBecco = realGiroSD*44.4444f; //(realGiroSD/360.0f)*16000.0f;
frequenzaReale = fixedStepGiroSD/realGiroSD;
semiPeriodoReale = (1000000.0f/frequenzaReale);
seedWheelRPM = (speedOfSeedWheel)*K_WheelRPM ; // calcola i giri al minuto della ruota di semina 7.37 31,75
#if defined(runner)
TBrpm = seedWheelRPM*rapportoRuote; // 5.896 31,75
#if defined(Zucca)
TBperiod=5.2f*TBrpm*2.0f; //prova dopo test con contagiri
#else
if (cellsNumber<8.0f){
TBperiod=6.2f*TBrpm;//4.8f*TBrpm; //prova dopo test con contagiri
}else{
TBperiod=5.2f*TBrpm; //prova dopo test con contagiri
}
#endif
#endif
#if defined(oldStepperDriver)
TBfrequency = (TBrpm*K_TBfrequency); // 130Hz a 0,29Mts 1397,00 a 1,25mt/s con 15 becchi e 15 celle
TBperiod=1000000.0f/TBfrequency; // 715uS
#endif
}
//************************************************************************
// rise of seed speed 25 pulse sensor
void sd25Fall(){
timeHole=metalTimer.read_ms();
double memo_TimeHole= (memoTimeHole + (double)timeHole)/ 2.0f;
memoTimeHole = (double)timeHole;
metalTimer.reset();
if (speedFromPick==0) {
speedOfSeedWheel=((seedPerimeter/25.0f)/memo_TimeHole)*1000.0f; //mtS
}
#if defined(pcSerial)
#if defined(checkLoop)
pc.printf("1\n");
#endif
#endif
}
//************************************************************************
// rise of seed speed motor encoder
void encoRise(){
timeHole=metalTimer.read_us();
double memo_TimeHole= (memoTimeHole + (double)timeHole)/ 2.0f;
memoTimeHole = (double)timeHole;
metalTimer.reset();
if (encoder==true) {
speedOfSeedWheel=((seedPerimeter/seedWheelDcPulse)/memo_TimeHole)*1000000.0f; //mtS
pulseRised2=1;
dcEncoderCnt++;
double sincronizza = frazioneImpulsi * dcEncoderCnt;
prePosSD=500+(uint32_t)sincronizza; // preposizionamento SD
#if defined(speedMaster)
if (quinconceActive==0) {
posForQuinc=500+(uint32_t)sincronizza;
}
#endif
}
#if defined(pcSerial)
#if defined(checkLoop)
pc.printf("2\n");
#endif
#endif
aggioVelocita();
}
//**************************************************
// rise of seed presence sensor
void seedSensorTask(){
seedSee=1;
#if defined(pcSerial)
#if defined(checkLoop)
pc.printf("3\n");
#endif
#endif
}
//**************************************************
// generate speed clock when speed is simulated from Tritecnica display
void speedSimulationClock(){
lastPulseRead=speedTimer.read_us();
oldLastPulseRead=lastPulseRead;
speedTimer.reset();
pulseRised=1;
speedFilter.reset();
#if defined(pcSerial)
#if defined(checkLoop)
pc.printf("4\n");
#endif
#endif
}
//*******************************************************
// interrupt task for tractor speed reading
//*******************************************************
void tractorReadSpeed(){
if ((oldTractorSpeedRead==0)) {
lastPulseRead=speedTimer.read_us();
oldLastPulseRead=lastPulseRead;
speedTimer.reset();
pulseRised=1;
oldTractorSpeedRead=1;
}
speedClock=1;
speedFilter.reset();
cntSpeedError=0;
#if defined(pcSerial)
#if defined(checkLoop)
pc.printf("5\n");
#endif
#endif
}
//*******************************************************
void speedMediaCalc()
{
double lastPd=(double) lastPulseRead/1000.0f;
pulseSpeedInterval = (mediaSpeed[0]+lastPd)/2.0f;
if (enableSimula==1) {
double TMT = (double)(speedSimula) * 100.0f /3600.0f;
pulseSpeedInterval = pulseDistance / TMT;
}
mediaSpeed[0]=lastPd;
OLDpulseSpeedInterval=pulseSpeedInterval;
#if defined(pcSerial)
#if defined(checkLoop)
pc.printf("6\n");
#endif
#endif
}
//*******************************************************
// clocked task for manage virtual encoder of seed wheel i/o
//*******************************************************
//*******************************************************
void step_SDPulseOut()
{
SDactualPosition++;
prePosSD++;
#if defined(speedMaster)
posForQuinc++;
#endif
#if defined(pcSerial)
#if defined(checkLoop)
pc.printf("7\n");
#endif
#endif
}
//*******************************************************
void step_TBPulseOut(){
TBmotorStepOut=!TBmotorStepOut;
if (TBmotorStepOut==0) {
if (TBmotorDirecti==TBforward) {
TBactualPosition++;
}
}
#if defined(pcSerial)
#if defined(stepTamb)
pc.printf("step\n");
#endif
#endif
/*
#if defined(pcSerial)
#if defined(checkLoop)
pc.printf("8\n");
#endif
#endif
*/
}
//*******************************************************
void invertiLo()
{
if (TBmotorDirecti==TBreverse) {
TBmotorDirecti=TBforward;
#if !defined(oldStepperDriver)
#if !defined(runner)
#if defined(Zucca)
motor->step_clock_mode_enable(StepperMotor::BWD);
#else
motor->step_clock_mode_enable(StepperMotor::FWD);
#endif
#endif
#if defined(provaStepper)
motor->run(StepperMotor::FWD,150.0f);
#endif
#endif
} else {
TBmotorDirecti=TBreverse;
#if !defined(oldStepperDriver)
#if !defined(runner)
#if defined(Zucca)
motor->step_clock_mode_enable(StepperMotor::FWD);
#else
motor->step_clock_mode_enable(StepperMotor::BWD);
#endif
#endif
#if defined(provaStepper)
motor->run(StepperMotor::BWD,100.0f);
#endif
#endif
}
#if defined(pcSerial)
#if defined(inversione)
pc.printf("cambio M %d\n",cambiaStep);
pc.printf("posizione %d \n",TBactualPosition);
#endif
#endif
#if defined(pcSerial)
#if defined(checkLoop)
pc.printf("9\n");
#endif
#endif
}
//*******************************************************
// aggiornamento parametri di lavoro fissi e da Tritecnica
void aggiornaParametri()
{
speedPerimeter = Pi * speedWheelDiameter ; // perimeter of speed wheel
pulseDistance = (speedPerimeter / speedWheelPulse)*1000.0f; // linear space between speed wheel pulse
seedPerimeter = Pi * (seedWheelDiameter-(deepOfSeed*2.0f)); // perimeter of seed wheel
intraPickDistance = seedPerimeter/pickNumber;
K_WheelRPM = 60.0f/seedPerimeter; // calcola il K per i giri al minuto della ruota di semina
//K_WhellFrequency = (seedWheelMotorSteps*SDreductionRatio)/60.0f; // calcola il K per la frequenza di comando del motore di semina
rapportoRuote = pickNumber/cellsNumber; // calcola il rapporto tra il numero di becchi ed il numero di celle
SDsectorStep = fixedStepGiroSD / pickNumber;
TBsectorStep = (TBmotorSteps*TBreductionRatio)/cellsNumber;
seedWheelDcPulse=SDreductionRatio*dcPulseTurn;
frazioneImpulsi= (SDsectorStep/(seedWheelDcPulse/pickNumber));
#if defined(runner)
#if defined(Zucca)
KcorT = (SDsectorStep/TBsectorStep)*2.0f;
#else
KcorT = (SDsectorStep/TBsectorStep);///2.0f;
#endif
#else
KcorT = (SDsectorStep/TBsectorStep);///2.0f;
#endif
angoloFase=angoloPh;
avvioGradi=angoloAv;
stepGrado=fixedStepGiroSD/360.0f;
TBdeltaStep=(fixedStepGiroSD/pickNumber)+(stepGrado*avvioGradi);
TBfaseStep = (stepGrado*angoloFase);
TBgiroStep = TBmotorSteps*TBreductionRatio;
K_TBfrequency = TBgiroStep/60.0f; // 1600 * 1.65625f /60 = 44 44,00
if (speedFromPick==1) {
intraPickDistance = seedPerimeter/pickNumber;
} else {
intraPickDistance = seedPerimeter/25.0f; // 25 è il numero di fori presenti nel disco di semina
}
#if defined(pcSerial)
#if defined(checkLoop)
pc.printf("10\n");
#endif
#endif
}
//*******************************************************
void cambiaTB(double perio)
{
#if defined(runner)
// update TB frequency
double TBper=0.0f;
if (aspettaStart==0){
TBper=perio;
if (oldPeriodoTB!=TBper){
#if defined(pcSerial)
#if defined(TBperSo)
pc.printf("TBper: %f MtS: %f\n",TBper,tractorSpeed_MtS_timed);
#endif
#endif
#if defined(Zucca)
motor->run(StepperMotor::BWD,TBper);
#else
motor->run(StepperMotor::FWD,TBper);
#endif
oldPeriodoTB=TBper;
}
}
#else
// update TB frequency
double limite=500.0f;
double TBper=0.0f;
double scala =2.0f;
if (aspettaStart==0) {
if (perio<limite) {
perio=limite;
}
TBper=perio/scala;
if (oldPeriodoTB!=TBper) {
if (TBper >= (limite/2.0f)) {
#if defined(pcSerial)
#if defined(checkLoop)
pc.printf("11a\n");
pc.printf("11a TBper: %f \n",TBper);
#endif
#endif
if (TBper != NULL) {
#if !defined(oldStepperDriver)
#if defined(runner)
#if defined(Zucca)
motor->step_clock_mode_enable(StepperMotor::BWD);
#else
motor->step_clock_mode_enable(StepperMotor::FWD);
#endif
#endif
#endif
TBticker.attach_us(&step_TBPulseOut,TBper); // clock time are milliseconds and attach seed motor stepper controls
}
} else {
#if defined(pcSerial)
#if defined(checkLoop)
pc.printf("11b\n");
#endif
#endif
TBticker.detach();
#if defined(pcSerial)
#if defined(loStop)
pc.printf("A1\n");
#endif
#endif
#if !defined(oldStepperDriver)
#if defined(runner)
motor->soft_hiz();
#endif
#endif
}
oldPeriodoTB=TBper;
}
}
#endif
}
//*******************************************************
void seedCorrect()
{
/*
posError determina la posizione relativa di TB rispetto ad SD
la reale posizione di SD viene modificata in funzione della velocità per
traslare la posizione relativa di TB. All'aumentare della velocità la posizione
di SD viene incrementata così che TB acceleri per raggiungerla in modo da rilasciare il seme prima
La taratura del sistema avviene determinando prima il valore di angoloFase alla minima velocità,
poi, alla massima velocità, dovrebbe spostarsi la posizione relativa con una variabile proporzionale alla velocità, ma c'è un però.
Il problema è che il momento di avvio determina una correzione dell'angolo di partenza del tamburo
angolo che viene rideterminato ogni volta che il sensore becchi legge un transito.
Di fatto c'è una concorrenza tra l'angolo di avvio determinato e la correzione di posizione relativa
del tamburo. E' molto probabile che convenga modificare solo la posizione relativa e non anche l'angolo di avvio
Ancora di più se viene eliminata la parte gestita da ciclata.
In questo modo dovrebbe esserci solo un andamento in accelerazione di TB che viene poi eventualmente decelerato
dal passaggio sul sensore di TB. Funzione corretta perchè il sincronismo tra i sensori genera l'inibizione della correzione
di fase di TB. In pratica il ciclo viene resettato al passaggio sul sensore di SD che riporta a 0 la posizione di SD.
Appena il sensore di TB viene impegnato allora viene abilitato il controllo di fase del tamburo.
Questo si traduce nel fatto che il controllo di posizione viene gestito solo all'interno di uno slot di semina in modo che
il tamburo non risenta della condizione di reset della posizione di SD mentre lui è ancora nella fase precedente. Si fermerebbe.
// La considerazione finale è che mantenendo l'angolo di avvio fisso e regolato sulla bassa velocità, intervenendo solo sulla correzione
// di posizione in questa routine, dovrebbe essere possibile seminare correttamente a tutte le velocità regolando solo 2 parametri.
*/
/*
SDsectorStep = (double)fixedStepGiroSD / (double)pickNumber;
TBsectorStep = (TBmotorSteps*TBreductionRatio)/cellsNumber;
KcorT = (SDsectorStep/TBsectorStep);
angoloFase=angoloPh;
stepGrado=fixedStepGiroSD/360.0f;
avvioGradi = costante da terminale tritecnica
TBdeltaStep=(fixedStepGiroSD/pickNumber)-(stepGrado*avvioGradi);
TBfaseStep = (stepGrado*angoloFase);
*/
if ((tractorSpeed_MtS_timed>0.01f)) {
if (inhibit==0) {
double posError =0.0f;
double posSD=((double)SDactualPosition)/KcorT;
posError = posSD - (double)TBactualPosition;
// interviene sulla velocità di TB per raggiungere la corretta posizione relativa
if((lowSpeed==0)&&(aspettaStart==0)) {
double lowLim=-50.0f;
double higLim = 50.0f;
double divide= 100.0f;
if (drumSelect==false){
if (pickNumber <= 5) {
lowLim=-500.0f;
higLim= 500.0f;
divide= 25.0f;
} else {
lowLim=-10.0f;
higLim= 130.0f;
divide= 100.0f;
}
}else{
if (pickNumber <= 5) {
lowLim=-100.0f;
higLim= 100.0f;
divide= 75.0f;
}else{
lowLim=-500.0f; //pneumatico Alessandria
higLim= 130.0f; //pneumatico Alessandria
divide= 20.0f; //pneumatico Alessandria
}
}
if (posError>higLim) {
posError=higLim;
//posError=0.0f;
//motor->soft_hiz();
//aspettareSincro=1;
//stopCicloTB=1;
}
if (posError<lowLim) {
posError=lowLim;
//aspettareSincro=1;
//stopCicloTB=1;
}
if (((posError >=1.0f)||(posError<=-1.0f))) {
#if defined(runner)
double variante = posError/divide;
if (variante < -0.399f){variante=-0.3999;}
/*if ((posError<=15.0f)&&(posError>=-15.0f)){
ePpos = periodo;// *(1.0f+ variante);
}else{*/
ePpos = periodo *(1.0f+ variante);
//}
#if defined(pcSerial)
#if defined(TBperS)
pc.printf("TBpos: %f SDpos: %f SDact: %f Err: %f Correggi: %f periodo: %f KcorT: %f \n",(double)TBactualPosition,posSD,(double)SDactualPosition,posError,ePpos,periodo, KcorT);
#endif
#endif
#else
ePpos = periodo /(1.0f+ ((posError/divide)));
#endif
if (ePpos>0.0f) {
cambiaTB(ePpos);
} else {
cambiaTB(periodo);///2.0f);
}
}
}
}
}
#if defined(pcSerial)
#if defined(checkLoopa)
pc.printf("12\n");
#endif
#endif
}
//*******************************************************
void videoUpdate()
{
for(int aa=0; aa<4; aa++) {
speedForDisplay[aa]=speedForDisplay[aa+1];
}
speedForDisplay[4]=tractorSpeed_MtS_timed;
totalSpeed=0.0f;
for (int aa=0; aa<5; aa++) {
totalSpeed += speedForDisplay[aa];
}
totalSpeed = totalSpeed / 5.0f;
#if defined(pcSerial)
#if defined(SDreset)
pc.printf("Fase: %d",fase);
pc.printf(" PrePosSD: %d",prePosSD);
pc.printf(" PosSD: %d",SDactualPosition);
pc.printf(" speed: %f",tractorSpeed_MtS_timed);
pc.printf(" Trigger: %d \n", trigRepos);
#endif
#endif
#if defined(pcSerial)
#if defined(checkLoop)
pc.printf("13\n");
#endif
#endif
}
//*******************************************************
void ciclaTB()
{
if ((startCicloTB==1)&&(cicloTbinCorso==0)) {
#if defined(pcSerial)
#if defined(checkLoop)
pc.printf("14a TBperiod: %f\n",TBperiod);
#endif
#endif
#if defined(runner)
#if defined(Zucca)
motor->run(StepperMotor::BWD,TBperiod);
#else
motor->run(StepperMotor::FWD,TBperiod);
#endif
#else
if (TBperiod >= (250.0f*2.0f)) {
if (TBperiod != NULL) {
#if !defined(oldstepperDriver)
#if defined(runner)
#if defined(Zucca)
motor->step_clock_mode_enable(StepperMotor::BWD);
#else
motor->step_clock_mode_enable(StepperMotor::FWD);
#endif
#endif
#endif
TBticker.attach_us(&step_TBPulseOut,TBperiod/2.0f); // clock time are milliseconds and attach seed motor stepper controls
}
}
#endif
cicloTbinCorso = 1;
startCicloTB=0;
}
if ((loadDaCan==1)&&(loadDaCanInCorso==0)) {
#if defined(pcSerial)
#if defined(checkLoop)
pc.printf("14b\n");
#endif
#endif
#if defined(runner)
#if defined(Zucca)
motor->run(StepperMotor::BWD,50.0f);
#else
motor->run(StepperMotor::FWD,50.0f);
#endif
#else
#if !defined(oldStepperDriver)
#if defined(Zucca)
motor->step_clock_mode_enable(StepperMotor::BWD);
#else
motor->step_clock_mode_enable(StepperMotor::FWD);
#endif
#endif
TBticker.attach_us(&step_TBPulseOut,1000.0f); // clock time are milliseconds and attach seed motor stepper controls
#endif
loadDaCanInCorso=1;
stopCicloTB=0;
}
if ((stopCicloTB==1)&&(TBactualPosition>5)&&(TBactualPosition<50)) {
#if defined(pcSerial)
#if defined(checkLoop)
pc.printf("14c\n");
#endif
#endif
#if !defined(runner)
TBticker.detach();
#endif
#if defined(pcSerial)
#if defined(loStop)
pc.printf("A2\n");
#endif
#endif
#if !defined(oldStepperDriver)
motor->soft_hiz();
#endif
cicloTbinCorso = 0;
stopCicloTB=0;
loadDaCanInCorso=0;
loadDaCan=0;
}
}
// ------------------------------------------------------------------------------------------------------------------------------------------------------------------
void stepSetting()
{
// Stepper driver init and set
TBmotorRst=1; // reset stepper driver
// M1 M2 M3 RESOLUTION
// 0 0 0 1/2
// 1 0 0 1/8
// 0 1 0 1/16
// 1 1 0 1/32
// 0 0 1 1/64
// 1 0 1 1/128
// 0 1 1 1/10
// 1 1 1 1/20
if (TBmotorSteps==400.0f){
TBmotor_M1=1;//0;
TBmotor_M2=1;//0;
TBmotor_M3=1;//0;
#if defined(pcSerial)
pc.printf("Steps 400\n");
#endif
}else if (TBmotorSteps==1600.0f){
TBmotor_M1=0;//0;
TBmotor_M2=1;//1;
TBmotor_M3=1;//1;
#if defined(pcSerial)
pc.printf("Steps 1600\n");
#endif
}else if (TBmotorSteps==3200.0f){
TBmotor_M1=1;//0;
TBmotor_M2=0;//1;
TBmotor_M3=1;//0;
#if defined(pcSerial)
pc.printf("Steps 3200\n");
#endif
}else if (TBmotorSteps==6400.0f){
TBmotor_M1=0;//1;
TBmotor_M2=0;//1;
TBmotor_M3=1;//0;
#if defined(pcSerial)
pc.printf("Steps 6400\n");
#endif
}else if (TBmotorSteps==12800.0f){
TBmotor_M1=1;//0;
TBmotor_M2=1;//0;
TBmotor_M3=0;//1;
#if defined(pcSerial)
pc.printf("Steps 12800\n");
#endif
}else if (TBmotorSteps==25600.0f){
TBmotor_M1=0;//1;
TBmotor_M2=1;//0;
TBmotor_M3=0;//1;
#if defined(pcSerial)
pc.printf("Steps 25600\n");
#endif
}else if (TBmotorSteps==2000.0f){
TBmotor_M1=1;//0;
TBmotor_M2=0;//1;
TBmotor_M3=0;//1;
#if defined(pcSerial)
pc.printf("Steps 2000\n");
#endif
}else if (TBmotorSteps==4000.0f){
TBmotor_M1=0;//1;
TBmotor_M2=0;//1;
TBmotor_M3=0;//1;
#if defined(pcSerial)
pc.printf("Steps 4000\n");
#endif
}else{
// set dei 1600 passi
TBmotor_M1=0;
TBmotor_M2=1;
TBmotor_M3=1;
#if defined(pcSerial)
pc.printf("Step standard\n");
#endif
}
TBmotorRst=0; // reset stepper driver
TBmotorDirecti=TBforward; // reset stepper direction
#if defined(pcSerial)
#if defined(checkLoop)
pc.printf("15\n");
#endif
#endif
}
//****************************************
void controllaCorrente(){
correggiCorrente=1;
}
//****************************************
void dcSetting(){
if ((speedFromPick==0)&&(encoder==false)) {
DcEncoder.rise(&sd25Fall);
}
if (encoder==true) {
DcEncoder.rise(&encoRise);
}
#if defined(pcSerial)
#if defined(checkLoop)
pc.printf("16\n");
#endif
#endif
}
//*******************************************************
void allarmi()
{
uint8_t alarmLowRegister1=0x00;
alarmLowRegister=0x00;
alarmHighRegister=0x80;
//alarmLowRegister=alarmLowRegister+(all_semiFiniti*0x01); // manca il sensore
alarmLowRegister=alarmLowRegister+(all_pickSignal*0x02); // fatto
alarmLowRegister=alarmLowRegister+(all_cellSignal*0x04); // fatto
alarmLowRegister=alarmLowRegister+(all_lowBattery*0x08); // fatto
alarmLowRegister=alarmLowRegister+(all_overCurrDC*0x10); // fatto
alarmLowRegister=alarmLowRegister+(all_stopSistem*0x20); // verificarne la necessità
//alarmLowRegister=alarmLowRegister+(all_upElements*0x40); // manca il sensore
if (seedSensorEnable==true) {
alarmLowRegister=alarmLowRegister+(all_noSeedOnCe*0x80); // manca il sensore
}
//alarmLowRegister1=alarmLowRegister1+(all_cfgnErrors*0x01); // da scrivere
alarmLowRegister1=alarmLowRegister1+(all_noDcRotati*0x02); // fatto
alarmLowRegister1=alarmLowRegister1+(all_noStepRota*0x04); // fatto
alarmLowRegister1=alarmLowRegister1+(all_speedError*0x08); // fatto
alarmLowRegister1=alarmLowRegister1+(all_noSpeedSen*0x10); // fatto
alarmLowRegister1=alarmLowRegister1+(all_no_Zeroing*0x20); // fatto
alarmLowRegister1=alarmLowRegister1+(all_genericals*0x40);
if (alarmLowRegister1 > 0) {
alarmHighRegister = 0x81;
alarmLowRegister = alarmLowRegister1;
}
#if defined(pcSerial)
#if defined(VediAllarmi)
if (all_pickSignal==1) {
pc.printf("AllarmeBecchi\n");
}
if (all_cellSignal==1) {
pc.printf("AllarmeCelle\n");
}
if (all_lowBattery==1) {
pc.printf("AllarmeBassaCorrente\n");
}
if (all_overCurrDC==1) {
pc.printf("AllarmeAltaCorrente\n");
}
if (all_stopSistem==1) {
pc.printf("AllarmeStop\n");
}
if (all_noDcRotati==1) {
pc.printf("AllarmeDCnoRotation\n");
}
if (all_noStepRota==1) {
pc.printf("AllarmeNoStepRotation\n");
}
if (all_speedError==1) {
pc.printf("AllarmeSpeedError\n");
}
if (all_noSpeedSen==1) {
pc.printf("AllarmeNoSpeedSensor\n");
}
if (all_no_Zeroing==1) {
pc.printf("AllarmeNoZero\n");
}
if (all_genericals==1) {
pc.printf("AllarmeGenerico\n");
}
pc.printf("Code: 0x%x%x\n",alarmHighRegister,alarmLowRegister);
#endif
#endif
all_semiFiniti=0;
all_pickSignal=0;
all_cellSignal=0;
all_lowBattery=0;
all_overCurrDC=0;
all_stopSistem=0;
all_upElements=0;
all_noSeedOnCe=0;
all_cfgnErrors=0;
all_noDcRotati=0;
all_noStepRota=0;
all_speedError=0;
all_noSpeedSen=0;
all_no_Zeroing=0;
all_genericals=0;
#if defined(pcSerial)
#if defined(checkLoop)
pc.printf("17\n");
#endif
#endif
}
//*******************************************************
#if defined(speedMaster)
void upDateSincro(){
char val1[8]= {0,0,0,0,0,0,0,0};
val1[3]=(posForQuinc /0x01000000)&0x000000FF;
val1[2]=(posForQuinc /0x00010000)&0x000000FF;
val1[1]=(posForQuinc /0x00000100)&0x000000FF;
val1[0]=posForQuinc & 0x000000FF;
//double pass = tractorSpeed_MtS_timed*100.0f;
double pass = speedOfSeedWheel*100.0f;
val1[4]=(uint8_t)(pass)&0x000000FF;
val1[5]=(prePosSD /0x00010000)&0x000000FF;
val1[6]=(prePosSD /0x00000100)&0x000000FF;
val1[7]=prePosSD & 0x000000FF;
#if defined(canbusActive)
#if defined(speedMaster)
if(can1.write(CANMessage(TX_SI, *&val1,8))) {
checkState=0;
}
#endif
#endif
#if defined(pcSerial)
#if defined(checkLoop)
pc.printf("18\n");
#endif
#endif
}
#endif
//*******************************************************
void upDateSpeed()
{
/*
aggiorna dati OPUSA3
val1[0] contiene il dato di velocità
val1[1] contiene il byte basso della tabella allarmi
uint8_t all_semiFiniti = 0; // semi finiti (generato dal relativo sensore)
uint8_t all_pickSignal = 0; // errore segnale becchi (generato dal tempo tra un becco ed il successivo)
uint8_t all_cellSignal = 0; // errore segnale celle (generato dal sensore tamburo )
uint8_t all_lowBattery = 0; // allarme batteria (valore interno di tritecnica)
uint8_t all_overCurrDC = 0; // sovracorrente motore DC (generato dal sensore di corrente)
uint8_t all_stopSistem = 0; // sistema in stop (a tempo con ruota ferma)
uint8_t all_upElements = 0; // elementi sollevati (generato dal relativo sensore)
uint8_t all_noSeedOnCe = 0; // fallanza di semina (manca il seme nella cella)
uint8_t all_cfgnErrors = 0; // errore di configurazione (incongruenza dei parametri impostati)
uint8_t all_noDcRotati = 0; // ruota di semina bloccata (arriva la velocità ma non i becchi)
uint8_t all_noStepRota = 0; // tamburo di semina bloccato (comando il tamburo ma non ricevo il sensore)
uint8_t all_speedError = 0; // errore sensore velocità (tempo impulsi non costante)
uint8_t all_noSpeedSen = 0; // mancanza segnale di velocità (sento i becchi ma non la velocita)
uint8_t all_no_Zeroing = 0; // mancato azzeramento sistema (generato dal timeout del comando motore DC)
uint8_t all_genericals = 0; // allarme generico
val1[2] contiene il byte alto della tabella di allarmi
val1[3] contiene i segnali per la diagnostica
bit 0= sensore ruota fonica
bit 1= sensore celle
bit 2= sensore posizione
bit 3= sensore becchi
bit 4= motore DC
bit 5= controller
bit 6= motore stepper
*/
char val1[8]= {0,0,0,0,0,0,0,0};
val1[3]=0;
val1[3]=val1[3]+(tractorSpeedRead*0x01);
val1[3]=val1[3]+(TBzeroPinInput*0x02);
val1[3]=val1[3]+(DcEncoder*0x04);
val1[3]=val1[3]+(seedWheelZeroPinInput*0x08);
#if defined(simulaPerCan)
if (buttonUser==0) {
val1[1]=0x02;
} else {
val1[1]=0x00;
}
val1[3]=valore;
valore+=1;
if(valore>50) {
valore=0;
}
tractorSpeed_MtS_timed=valore;
oldLocalTractorSpeed=valore;
#endif
allarmi();
valore = totalSpeed*36.0f; // tractorSpeed_MtS_timed*36.0f;
val1[0]=valore;
val1[1]=alarmHighRegister; // registro alto allarmi. Parte sempre da 0x80
val1[2]=alarmLowRegister; // registro basso allarmi
//val1[3]=val1[3];// registro di stato degli ingressi
val1[4]=resetComandi;
val1[5]=cellsCounterLow;
val1[6]=cellsCounterHig;
#if defined(canbusActive)
if(can1.write(CANMessage(TX_ID, *&val1,8))) {
checkState=0;
}
#endif
#if defined(pcSerial)
#if defined(checkLoop)
pc.printf("19\n");
#endif
#endif
}
//*******************************************************
void leggiCAN()
{
#if defined(canbusActive)
if(can1.read(rxMsg)) {
if (firstStart==1) {
#if defined(speedMaster)
sincUpdate.attach(&upDateSincro,0.012f);//0.009f
canUpdate.attach(&upDateSpeed,0.21f);
#else
canUpdate.attach(&upDateSpeed,0.407f);
#endif
firstStart=0;
}
if (rxMsg.id==RX_ID) {
#if defined(pcSerial)
#if defined(canDataReceiveda)
pc.printf("Messaggio ricevuto\n");
#endif
#endif
}
if (rxMsg.id==RX_Broadcast) {
#if defined(pcSerial)
#if defined(canDataReceivedb)
pc.printf("BroadCast ricevuto\n");
#endif
#endif
enableSimula= rxMsg.data[0];
speedSimula = rxMsg.data[1];
avviaSimula = rxMsg.data[2];
selezionato = rxMsg.data[3];
comandiDaCan = rxMsg.data[4];
speedStepp = rxMsg.data[5];
if (speedStepp==0){
simStepper=0;
}else{
simStepper=1;
}
#if defined(pcSerial)
#if defined(canDataReceived)
pc.printf("Speed simula %d \n",speedSimula);
#endif
#endif
switch (comandiDaCan) {
case 1:
case 3:
azzeraDaCan=1;
resetComandi=0x01;
comandiDaCan=0;
break;
case 2:
loadDaCan=1;
resetComandi=0x02;
comandiDaCan=0;
break;
default:
comandiDaCan=0;
resetComandi=0xFF;
break;
}
#if defined(pcSerial)
#if defined(canDataReceivedR)
pc.printf("Comandi: %x\n",resetComandi);
#endif
#endif
if (speedSimula>45) {
speedSimula=45;
}
// modulo 1
if (RX_ID==0x100) {
if ((selezionato&0x01)==0x01) {
simOk=1;
} else {
simOk=0;
}
}
// modulo 2
if (RX_ID==0x102) {
if ((selezionato&0x02)==0x02) {
simOk=1;
} else {
simOk=0;
}
}
// modulo 3
if (RX_ID==0x104) {
if ((selezionato&0x04)==0x04) {
simOk=1;
} else {
simOk=0;
}
}
// modulo 4
if (RX_ID==0x106) {
if ((selezionato&0x08)==0x08) {
simOk=1;
} else {
simOk=0;
}
}
// modulo 5
if (RX_ID==0x108) {
if ((selezionato&0x10)==0x10) {
simOk=1;
} else {
simOk=0;
}
}
}
//if (tractorSpeed_MtS_timed <= 0.01f){
if (rxMsg.id==RX_Settings) {
//if (tractorSpeed_MtS_timed==0.0f) {
pickNumber = rxMsg.data[0]; // numero di becchi installati sulla ruota di semina
cellsNumber = rxMsg.data[1]; // numero di celle del tamburo
deepOfSeed=((double)rxMsg.data[2]/1000.0f); // deep of seeding
seedWheelDiameter = ((rxMsg.data[4]*0x100)+rxMsg.data[3])/10000.0f; // seed wheel diameter setting
speedWheelDiameter = ((rxMsg.data[6]*0x100)+rxMsg.data[5])/10000.0f; // variable for tractor speed calculation (need to be set from UI) ( Unit= meters )
speedWheelPulse = rxMsg.data[7]; // variable which define the number of pulse each turn of tractor speed wheel (need to be set from UI)
aggiornaParametri();
//}
}
if (tractorSpeed_MtS_timed <= 0.01f){
if (rxMsg.id==RX_AngoloPh) {
if (tractorSpeed_MtS_timed==0.0f) {
#if defined(M1)
angoloPh = (double) rxMsg.data[0] ;
aggiornaParametri();
#endif
#if defined(M2)
angoloPh = (double) rxMsg.data[1] ;
aggiornaParametri();
#endif
#if defined(M3)
angoloPh = (double) rxMsg.data[2] ;
aggiornaParametri();
#endif
#if defined(M4)
angoloPh = (double) rxMsg.data[3] ;
aggiornaParametri();
#endif
#if defined(M5)
angoloPh = (double) rxMsg.data[4] ;
aggiornaParametri();
#endif
#if defined(M6)
angoloPh = (double) rxMsg.data[5] ;
aggiornaParametri();
#endif
}
}
if (rxMsg.id==RX_AngoloAv) {
if (tractorSpeed_MtS_timed==0.0f) {
#if defined(M1)
angoloAv = (double) rxMsg.data[0] ;
aggiornaParametri();
#endif
#if defined(M2)
angoloAv = (double) rxMsg.data[1] ;
aggiornaParametri();
#endif
#if defined(M3)
angoloAv = (double) rxMsg.data[2] ;
aggiornaParametri();
#endif
#if defined(M4)
angoloAv = (double) rxMsg.data[3] ;
aggiornaParametri();
#endif
#if defined(M5)
angoloAv = (double) rxMsg.data[4] ;
aggiornaParametri();
#endif
#if defined(M6)
angoloAv = (double) rxMsg.data[5] ;
aggiornaParametri();
#endif
}
}
if (rxMsg.id==RX_Quinconce) {
if (tractorSpeed_MtS_timed==0.0f) {
quinconceActive = (uint8_t) rxMsg.data[0];
quincPIDminus = (uint8_t) rxMsg.data[1];
quincPIDplus = (uint8_t) rxMsg.data[2];
quincLIMminus = (uint8_t) rxMsg.data[3];
quincLIMplus = (uint8_t) rxMsg.data[4];
quincSector = (uint8_t) rxMsg.data[5];
aggiornaParametri();
}
}
}
if (tractorSpeed_MtS_timed > 0.0f){
if (rxMsg.id==RX_QuincSinc) {
masterSinc = (uint32_t) rxMsg.data[3] * 0x01000000;
masterSinc = masterSinc + ((uint32_t) rxMsg.data[2] * 0x00010000);
masterSinc = masterSinc + ((uint32_t) rxMsg.data[1] * 0x00000100);
masterSinc = masterSinc + ((uint32_t) rxMsg.data[0]);
speedFromMaster = (double)rxMsg.data[4]/100.0f;
mast2_Sinc = ((uint32_t) rxMsg.data[5] * 0x00010000);
mast2_Sinc = mast2_Sinc + ((uint32_t) rxMsg.data[6] * 0x00000100);
mast2_Sinc = mast2_Sinc + ((uint32_t) rxMsg.data[7]);
canDataCheck=1;
}
}
//if (tractorSpeed_MtS_timed <= 0.01f){
if (rxMsg.id==RX_Configure) {
uint8_t flags = rxMsg.data[0];
if ((flags&0x80)==0x80) { // comando taglianylon
if (oldTagliaNylon==false){
tagliaNylonAttivo=true;
oldTagliaNylon=true;
}
} else {
if (oldTagliaNylon==true){
tagliaNylonAttivo=false;
oldTagliaNylon=false;
}
if ((tagliaNylonAttivo==false)&&(tractorSpeed_MtS_timed<=0.01f)){
uint16_t steps = (uint32_t) rxMsg.data[2]*0x00000100;
steps = steps + ((uint32_t)rxMsg.data[1]);
TBmotorSteps =steps;
#if defined(oldStepperDriver)
stepSetting();
#endif
cellsCountSet = rxMsg.data[3];
if ((flags&0x01)==0x01) {
if (encoder==false) {
encoder=true;
speedFromPick=0;
DcEncoder.rise(NULL);
dcSetting();
}
} else {
if (encoder==true) {
encoder=false;
speedFromPick=1;
DcEncoder.rise(NULL);
dcSetting();
}
}
if ((flags&0x02)==0x02) {
tankLevelEnable=true;
} else {
tankLevelEnable=false;
}
if ((flags&0x04)==0x04) {
seedSensorEnable=true;
} else {
seedSensorEnable=false;
}
if ((flags&0x08)==0x08) {
drumSelect=true; // usare per selezione del tamburo =0 meccanico =1 PNEUMATICO
} else {
drumSelect=false; // forzato a true per Germania
}
if ((flags&0x10)==0x10) {
canDataCheckEnable=true;
} else {
canDataCheckEnable=false;
}
if ((flags&0x20)==0x20) {
tamburoStandard=1;
} else {
tamburoStandard=0;
}
if ((flags&0x40)==0x40) {
currentCheckEnable=true;
} else {
currentCheckEnable=false;
}
aggiornaParametri();
}
}
}
//}
}
#endif
#if defined(overWriteCanSimulation)
enableSimula=1;
speedSimula=25;
avviaSimula=1;
simOk=1;
#endif
#if defined(pcSerial)
#if defined(checkLoop)
pc.printf("20\n");
#endif
#endif
}
//*******************************************************
void DC_CheckCurrent()
{
// TODO: tabella di riferimento assorbimenti alle varie velocità al fine di gestire
// gli allarmi e le correzioni di velocità
//float SD_analogMatrix[10]={0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f};
//int SD_analogIndex[10]={0,0,0,0,0,0,0,0,0,0};
// Analog reading
//number = floor(number * 100) / 100;
timeout.detach();
SD_CurrentAnalog = floor(SDcurrent.read()*100)/100; // valore in ingresso compreso tra 0.00 e 1.00
SD_CurrentScaled = floor(( (SD_CurrentStart-SD_CurrentAnalog)*3.3f) / (SD_CurrentFactor/1000.0f)*10)/10;
}
//*******************************************************
void DC_prepare()
{
// direction or brake preparation
if (DC_brake==1) {
SDmotorInA=1;
SDmotorInB=1;
} else {
if (DC_forward==0) {
SDmotorInA=1;
SDmotorInB=0;
} else {
SDmotorInA=0;
SDmotorInB=1;
}
}
// fault reading
if (SDmotorInA==1) {
SD_faultA=1;
} else {
SD_faultA=0;
}
if (SDmotorInB==1) {
SD_faultB=1;
} else {
SD_faultB=0;
}
#if defined(pcSerial)
#if defined(checkLoopa)
pc.printf("22\n");
#endif
#endif
}
//*******************************************************
void startDelay(){
SD_CurrentStart = floor(SDcurrent.read()*100)/100; // valore in ingresso compreso tra 0.00 e 1.00
int ritardo =0;
ritardo = (int)((float)(dcActualDuty*800.0f));
timeout.attach_us(&DC_CheckCurrent,ritardo);
}
//*******************************************************
void quincTrigon(){
quincClock=true;
#if defined(pcSerial)
#if defined(checkLoop)
pc.printf("24\n");
#endif
#endif
}
void quincTrigof(){
quincClock=false;
#if defined(pcSerial)
#if defined(checkLoop)
pc.printf("25\n");
#endif
#endif
}
//*******************************************************
void quinCalc()
{
// riceve l'impulso di sincro dal master e fa partire il timer di verifica dell'errore
#if !defined(mezzo)
if ((quincClock==true)&&(oldQuincIn==0)) {
oldQuincIn=1;
if (quincStart==0) {
oldQuincTimeSD = (double) quincTimeSD.read_ms();
quincTime.reset();
quincTimeSD.reset();
quincStart=1;
}
}
if(quincClock==false) {
oldQuincIn=0;
}
#else
if ((((quinconceActive==0)&&(quincClock==true))||((quinconceActive==1)&&(quincClock==false)))&&(oldQuincIn==0)) {
oldQuincIn=1;
if (quincStart==0) {
oldQuincTimeSD = (double) quincTimeSD.read_ms();
quincTime.reset();
quincStart=1;
}
}
if (quinconceActive==0) {
if (quincClock==false) {
oldQuincIn=0;
}
} else {
if (quincClock==true) {
oldQuincIn=0;
}
}
#endif
//****************************************************************************************
if (quincCnt>=4) {
if (countPicks==0) {
if ((sincroQui==1)&&(quincStart==0)) {
// decelera
countPicks=1;
}
if ((sincroQui==0)&&(quincStart==1)) {
// accelera
countPicks=2;
}
}
if ((sincroQui==1)&&(quincStart==1)) {
if (countPicks==1) { //decelera
scostamento = oldQuincTimeSD;
if ((scostamento < (tempoBecchiPerQuinc*0.85f))) {
double scostPerc = (scostamento/tempoBecchiPerQuinc);
percento -= ((double)quincPIDminus/100.0f)*(scostPerc);
#if defined(pcSerial)
#if defined(laq)
pc.printf("RALL scos2: %f tbpq: %f percento: %f \n",scostamento,tempoBecchiPerQuinc,percento);
#endif
#endif
}
//if (scostamento <15.0f){percento=0.0f;}
}
if (countPicks==2) { //accelera
scostamento = (double)quincTime.read_ms();
if (scostamento < (tempoBecchiPerQuinc*0.85f)) {
double scostPerc = (scostamento/tempoBecchiPerQuinc);
percento += ((double)quincPIDplus/100.0f)*(scostPerc);
#if defined(pcSerial)
#if defined(laq)
pc.printf("ACCE scos1: %f tbpq: %f percento: %f \n",scostamento,tempoBecchiPerQuinc,percento);
#endif
#endif
}
//if (scostamento <15.0f){percento=0.0f;}
}
sincroQui=0;
quincStart=0;
countPicks=0;
// questo e il primo
#if !defined(speedMaster)
if (quincCnt>=3) {
if (speedFromMaster>0.0f) {
if (enableSimula==0) {
tractorSpeed_MtS_timed = speedFromMaster + percento;
/*if (tractorSpeed_MtS_timed > (oldLocalTractorSpeed*1.15f)){
tractorSpeed_MtS_timed = oldLocalTractorSpeed*1.15f;
}
if (tractorSpeed_MtS_timed < (oldLocalTractorSpeed/1.15f)){
tractorSpeed_MtS_timed = oldLocalTractorSpeed/1.15f;
}*/
}
}
}
#endif
}
//*******************************************************************
if (canDataCheckEnable==true) {
if (canDataCheck==1) { // sincro da comunicazione can del valore di posizione del tamburo master
canDataCheck=0;
double parametro = SDsectorStep/(double)quincSector;
double differenza=0.0f;
#if defined(mezzo)
if (quinconceActive==1) {
differenza = (double)masterSinc - (double)prePosSD;
} else {
differenza = (double)mast2_Sinc - (double)prePosSD;
}
#else
differenza = (double)mast2_Sinc - (double)prePosSD;
#endif
if (abs(differenza)<=50.0f){differenza=0.0f;}
if ((differenza > 0.0f)&&(differenza < parametro)) {
double diffPerc = differenza / parametro;
percento += ((double)quincPIDplus/100.0f)*abs(diffPerc);
}
if ((differenza < 0.0f)&&(abs(differenza) < parametro)) {
double diffPerc = (double)differenza / parametro;
percento -= ((double)quincPIDminus/100.0f)*abs(diffPerc);
}
// questo e il secondo
#if !defined(speedMaster)
if (quincCnt>=3) {
if (speedFromMaster>0.0f) {
if (enableSimula==0) {
tractorSpeed_MtS_timed = speedFromMaster + percento;
/*if (tractorSpeed_MtS_timed > (oldLocalTractorSpeed*1.15f)){
tractorSpeed_MtS_timed = oldLocalTractorSpeed*1.15f;
}
if (tractorSpeed_MtS_timed < (oldLocalTractorSpeed/1.15f)){
tractorSpeed_MtS_timed = oldLocalTractorSpeed/1.15f;
}*/
}
}
}
#endif
}
}
}
if ((percento) > ((double) quincLIMplus/100.0f)) {
percento= (double)quincLIMplus/100.0f;
}
if ((percento) < (((double)quincLIMminus*-1.0f)/100.0f)) {
percento=((double)quincLIMminus*-1.0f)/100.0f;
}
#if defined(pcSerial)
#if defined(checkLoop)
pc.printf("26\n");
#endif
#endif
}
// ----------------------------------------
#if defined(seedSensor)
void resetDelay(){
delaySeedCheck.reset();
delaySeedCheck.stop();
#if defined(pcSerial)
#if defined(checkLoop)
pc.printf("27\n");
#endif
#endif
}
#endif
// ----------------------------------------
void aggiornati(){
#if defined(runner)
legPos.detach();
TBoldPosition= (uint32_t) motor->get_position();
legPos.attach(&step_Reading,0.002f);
#if defined(pcSerial)
#if defined(TBperS)
pc.printf("TBoldPos: %d\n",TBoldPosition);
#endif
#endif
#else
TBactualPosition=0;
#endif
}
// ------------------------------------------------------------------------------------------------------------------------------------------------------------------
// MAIN SECTION
// ---------------------------------------------------------------------------------------------------------------------------------------------------------------
//*******************************************************************************
int main()
{
#if defined(pcSerial)
#if defined(resetCpu)
pc.printf("RESET\n");
#endif
#endif
#if defined(canbusActive)
can1.attach(&leggiCAN, CAN::RxIrq);
#endif
wait(1.0f);
wait(1.0f);
wait(ritAvv);
#if defined(oldStepperDriver)
stepSetting();
#endif
#if defined(runner)
TBmotor_SW=1;
#endif
TBmotorDirecti=TBforward; // reset stepper direction
//----- Initialization
/* Initializing SPI bus. */
// dev_spi(mosi,miso,sclk)
// D11= PA7; D12= PA6; D13= PA5
#if defined(runner)
DevSPI dev_spi(D11, D12, D13);
dev_spi.frequency(5000000);
/* Initializing Motor Control Component. */
// powerstep01( flag, busy,stby, stck, cs, dev_spi)
// motor = new PowerStep01(D2, D4, D8, D9, D10, dev_spi); // linea standard per IHM03A1
motor = new PowerStep01(PA_8, PC_7, PC_4, PB_3, PB_6, dev_spi); // linea per scheda seminatrice V7
if (motor->init(&init) != COMPONENT_OK) {
exit(EXIT_FAILURE);
}
/* Attaching and enabling an interrupt handler. */
motor->attach_flag_irq(&my_flag_irq_handler);
motor->enable_flag_irq();
//motor->disable_flag_irq();
/* Attaching an error handler */
//motor->attach_error_handler(&my_error_handler);
#endif
wait(1.0f);
for (int a=0; a<5; a++) {
mediaSpeed[a]=0;
}
// DC reset ad set
int decima = 100;
wait_ms(200);
SD_CurrentStart=floor(SDcurrent.read()*decima)/decima;
wait_ms(2);
SD_CurrentStart+=floor(SDcurrent.read()*decima)/decima;
wait_ms(1);
SD_CurrentStart+=floor(SDcurrent.read()*decima)/decima;
wait_ms(3);
SD_CurrentStart+=floor(SDcurrent.read()*decima)/decima;
SD_CurrentStart=(floor((SD_CurrentStart/4.0f)*decima)/decima)-0.01f;
wait_ms(100);
DC_prepare();
speedTimer.start(); // speed pulse timer
intraPickTimer.start();
speedTimeOut.start();
speedFilter.start();
seedFilter.start();
TBfilter.start();
sincroTimer.start();
rotationTimeOut.start();
metalTimer.start();
quincTime.start();
quincTimeSD.start();
#if defined(runner)
legPos.attach(&step_Reading,0.002f);
#endif
//*******************************************************
// controls for check DC motor current
pwmCheck.rise(&startDelay);
wait_ms(500);
dcVeri.attach(&controllaCorrente,2.0f);
delaySpeedCheck.start();
#if defined(runnerTos)
thread.start(step_Reading);
#endif
if (inProva==0) {
tractorSpeedRead.rise(&tractorReadSpeed);
#if !defined(speedMaster)
quinconceIn.rise(&quincTrigon);
quinconceIn.fall(&quincTrigof);
#endif
#if defined(speedMaster)
tftUpdate.attach(&videoUpdate,0.50f);
#endif
seedCorrection.attach(&seedCorrect,0.010f); // con 16 becchi a 4,5Kmh ci sono 37mS tra un becco e l'altro, quindi 8 correzioni di tb
dcSetting();
#if defined(seedSensor)
seedCheck.fall(&seedSensorTask);
#endif
} else {
tftUpdate.attach(&videoUpdate,0.125f);
}
aggiornaParametri();
SDmotorPWM.period_us(periodoSD); // frequency 1KHz pilotaggio motore DC
SDmotorPWM.write(1.0f); // duty cycle = stop
TBmotorDirecti=TBforward; // tb motor direction set
#if defined(provaStepper)
TBmotorRst=1;
TBmotorDirecti=TBforward;
// definire il pin di clock che è PB_3
#if defined(Zucca)
motor->run(StepperMotor::BWD,100.0f);
//motor->step_clock_mode_enable(StepperMotor::FWD);
#else
motor->run(StepperMotor::BWD,100.0f);
//motor->step_clock_mode_enable(StepperMotor::BWD);
#endif
// attiva l'out per il controllo dello stepper in stepClockMode
//DigitalOut TBmotorStepOut(PB_3); // PowerStep01 Step Input
#if defined(pcSerial)
#if defined(checkLoop)
pc.printf("11f\n");
#endif
#endif
//TBticker.attach(&step_TBPulseOut,0.0005f); // clock time are seconds and attach seed motor stepper controls
TATicker.attach(&invertiLo,3.0f);
#else
#if !defined(oldStepperDriver)
// definire il pin di clock che è PB_3
motor->set_home();
motor->go_to(50);
motor->wait_while_active();
#if !defined(runner)
#if defined(Zucca)
motor->step_clock_mode_enable(StepperMotor::BWD);
#else
motor->step_clock_mode_enable(StepperMotor::FWD);
#endif
#endif
#if defined(pcSerial)
#if defined(loStop)
pc.printf("A3\n");
#endif
#endif
motor->soft_hiz();
// attiva l'out per il controllo dello stepper in stepClockMode
DigitalOut TBmotorStepOut(PB_3); // PowerStep01 Step Input
#endif
#endif // end prova stepper
wd.Configure(2.0); //watchdog set at xx seconds
//**************************************************************************************************************
// MAIN LOOP
//**************************************************************************************************************
while (true) {
// ripetitore segnale di velocità. Il set a 1 è nella task ad interrupt
if (tractorSpeedRead==0) {
speedClock=0;
}
if ((tractorSpeed_MtS_timed>0.0f)&&(oldTractStop==1)){
alarmDelay.start();
oldTractStop=0;
}
if (alarmDelay.read()>5){
alarmEnable=true;
}
if ((tractorSpeed_MtS_timed<=0.05f)){
oldTractStop=1;
alarmDelay.stop();
alarmDelay.reset();
alarmEnable=false;
}
// inversione segnali ingressi
#if !defined(simulaBanco)
seedWheelZeroPinInput = !seedWheelZeroPinInputRev;
#else
if ((prePosSD-500) >= SDsectorStep) {
seedWheelZeroPinInput=1;
}
if ((prePosSD > 500)&&(prePosSD<580)) {
seedWheelZeroPinInput=0;
}
#endif
if (drumSelect==true){
TBzeroPinInput = TBzeroPinInputRev;
}else{
TBzeroPinInput = !TBzeroPinInputRev;
}
// se quinconce attivo ed unita' master invia segnale di sincro
#if defined(speedMaster)
if (seedWheelZeroPinInput==1) {
quinconceOut=0;
}
if (((double)(prePosSD-500) >= (SDsectorStep/((double)quincSector)))&&(quinconceOut=1)) {
quinconceOut=1;
}
if (quinconceActive==1) {
if ((quinconceOut==1)&&(oldQuinconceOut==1)) {
posForQuinc=500;
oldQuinconceOut=0;
}
if (((double)posForQuinc-500.0f)> (SDsectorStep-200)) {
oldQuinconceOut=1;
}
}
#endif
// simulazione velocita
if (enableSimula==1) {
double TMT = 0.0f;
if (speedSimula > 0) {
TMT = (double)(speedSimula) * 100.0f /3600.0f;
pulseSpeedInterval = pulseDistance / TMT;
} else {
pulseSpeedInterval = 10000.0f;
}
if (avviaSimula==1) {
if(oldSimulaSpeed!=pulseSpeedInterval) {
spedSimclock.attach_us(&speedSimulationClock,pulseSpeedInterval);
oldSimulaSpeed=pulseSpeedInterval;
}
} else {
oldSimulaSpeed=10000.0f;
spedSimclock.detach();
}
} else {
spedSimclock.detach();
}
//*******************************************************
// determina se sono in bassa velocità per il controllo di TB
if (speedOfSeedWheel<=minSeedSpeed) {
if (lowSpeedRequired==0) {
ritardaLowSpeed.reset();
ritardaLowSpeed.start();
}
lowSpeedRequired=1;
} else {
if (lowSpeedRequired==1) {
lowSpeedRequired=0;
ritardaLowSpeed.reset();
ritardaLowSpeed.stop();
}
}
if (ritardaLowSpeed.read_ms()> 2000) {
lowSpeed=1;
} else {
lowSpeed=0;
}
//**************************************************************************************************************
//**************************************************************************************************************
// LOGICAL CONTROLS
//**************************************************************************************************************
//**************************************************************************************************************
if (inProva==0) {
// simulazione stepper
if (simOk==1){
if (simStepper==1){
oldSimStepper=true;
simStepSpeed= ((double)speedStepp*180.45f)/50.0f;
if (oldSimStepSpeed!=simStepSpeed){
#if defined(runner)
motor->run(StepperMotor::FWD,simStepSpeed);
#endif
#if defined(oldStepperDriver)
TBticker.attach_us(&step_TBPulseOut,simStepSpeed/2.0f); // clock time are milliseconds and attach seed motor stepper controls
#endif
oldSimStepSpeed=simStepSpeed;
}
}else{
if (oldSimStepper==true){
oldSimStepper=false;
#if defined(runner)
motor->soft_hiz();
#endif
#if defined(oldStepperDriver)
TBticker.detach();
#endif
oldSimStepSpeed=0.0f;
}
}
}else{
if (oldSimStepper==true){
oldSimStepper=false;
#if defined(runner)
motor->soft_hiz();
#endif
#if defined(oldStepperDriver)
TBticker.detach();
#endif
oldSimStepSpeed=0.0f;
}
}
if ((startCycleSimulation==0)&&(enableSimula==0)) {
runRequestBuf=1;//0;
} else {
runRequestBuf=1;//0;
}
if ((tractorSpeedRead==0)&&(speedFilter.read_ms()>3)) {
oldTractorSpeedRead=0;
}
// ----------------------------------------
// filtra il segnale dei becchi per misurare il tempo di intervallo tra loro
// ----------------------------------------
if ((seedWheelZeroPinInput==0)&&(oldSeedWheelZeroPinInput==1)) {
if(seedFilter.read_ms()>=4) {
oldSeedWheelZeroPinInput=0;
SDzeroDebounced=0;
}
}
if ((seedWheelZeroPinInput==1)&&(oldSeedWheelZeroPinInput==0)) {
timeIntraPick = (double)intraPickTimer.read_ms();
prePosSD=500; // preposizionamento SD
intraPickTimer.reset();
rotationTimeOut.reset();
seedFilter.reset();
sincroTimer.reset();
oldSeedWheelZeroPinInput=1;
quincTime.reset();
quincTimeSD.reset();
SDzeroDebounced=1;
sincroQui=1;
SDwheelTimer.reset();
dcEncoderCnt=0;
#if defined(speedMaster)
if (quinconceActive==0) {
posForQuinc=500;
}
if (checkSDrotation==0) {
checkSDrotation=1;
SDwheelTimer.start();
}
#endif
if (quincCnt<10) {
quincCnt++;
}
if ((aspettaStart==0)&&(lowSpeed==1)) {
beccoPronto=1;
}
lockStart=0;
double fase1=0.0f;
forzaFase=0;
double limite=fixedStepGiroSD/pickNumber;
if (tamburoStandard==0) {
fase1=TBdeltaStep;
} else {
if(speedForCorrection >= speedOfSeedWheel) {
fase1=TBdeltaStep;
} else {
fase1=(TBdeltaStep)-(((speedOfSeedWheel)/maxWorkSpeed)*(TBfaseStep));
}
if (fase1 > limite) {
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)
}
if ((fase1 > (limite -20.0f))&&(fase1<(limite +5.0f))) {
fase1 = limite -20.0f; // se la fase è molto vicina al limite interpone uno spazio minimo di lavoro del sincronismo
forzaFase=1;
}
trigRepos=1;
}
fase = (uint32_t)fase1+500;
#if defined(pcSerial)
#if defined(inCorre)
pc.printf(" limite %f", limite);
pc.printf(" delta %f", TBdeltaStep);
pc.printf(" faseStep %f", TBfaseStep);
pc.printf(" fase %d",fase);
pc.printf(" forzaFase %d",forzaFase);
pc.printf(" trigRepos %d", trigRepos);
pc.printf(" ActualSD: %d",SDactualPosition);
pc.printf(" SpeedWheel: %f",speedOfSeedWheel);
pc.printf(" SPEED: %f \n",tractorSpeed_MtS_timed);
#endif
#endif
if (tractorSpeed_MtS_timed>0.2f){
if (timeIntraPick >= (memoIntraPick*2)) {
if ((aspettaStart==0)&&(alarmEnable==true)) {
if (firstStart==0) {
all_pickSignal=1;
}
}
}
}
memoIntraPick = timeIntraPick;
if ((speedFromPick==1)&&(encoder==false)) {
speedOfSeedWheel=((seedPerimeter / pickNumber)/timeIntraPick)*1000.0f;
#if defined(pcSerial)
#if defined(Qnca)
pc.printf("perim: %f pickN: %f sped: %f\n", seedPerimeter, pickNumber,speedOfSeedWheel);
#endif
#endif
}
if (encoder==false) {
pulseRised2=1;
}
#if defined(speedMaster)
if ((tractorSpeed_MtS_timed==0.0f)) {
if ((firstStart==0)&&(simOk==0)&&(enableSimula==0)) {
cntSpeedError++;
if (cntSpeedError >= 5){
all_noSpeedSen=1;
}
}
}
if ((tractorSpeed_MtS_timed>0.2f)&&(alarmEnable==true)){
double oldLastPr = (double)oldLastPulseRead*2.8f;
if((double)speedTimeOut.read_us()> (oldLastPr)) {
if ((firstStart==0)&&(simOk==0)&&(enableSimula==0)) {
all_speedError =1;
}
}
}
#endif
//*******************************************
// esegue calcolo clock per la generazione della posizione teorica
// la realtà in base al segnale di presenza del becco
if (speedOfSeedWheel < 0.002f) {
#if defined(pcSerial)
#if defined(checkLoopb)
pc.printf("forza\n");
#endif
#endif
speedOfSeedWheel=0.001f;
}
aggioVelocita();
}
// ----------------------------------------
// check SD fase
if ((prePosSD >= fase)||(forzaFase==1)) { //&&(prePosSD < (fase +30))){
forzaFase=0;
if (trigRepos==1) {
SDactualPosition=0;
if ((countCicli<30)&&(trigCicli==0)) {
countCicli++;
trigCicli=1;
}
if(countCicli>=cicliAspettaStart) {
aspettaStart=0;
}
if ((lowSpeed==0)&&(aspettaStart==0)&&(lockStart==0)) {
syncroCheck=1;
beccoPronto=0;
}
if (trigTB==0) {
inhibit=1;
trigSD=1;
} else {
inhibit=0;
trigTB=0;
trigSD=0;
}
trigRepos=0;
}
} else {
trigCicli=0;
}
// ----------------------------------------
// filtra il segnale del tamburo per lo stop in fase del tamburo stesso
if (TBzeroPinInput==0) {
if (TBfilter.read_ms()>=2) {
oldTBzeroPinInput=0;
}
}
if (((TBzeroPinInput==1)&&(oldTBzeroPinInput==0))&&(simStepper==0)) {
oldTBzeroPinInput=1;
if (loadDaCanInCorso==0) {
stopCicloTB=1;
startCicloTB=0;
}
TBfilter.reset();
TBzeroCyclePulse=1;
#if defined(runner)
legPos.detach();
TBoldPosition= (uint32_t) motor->get_position();
legPos.attach(&step_Reading,0.002f);
#if defined(pcSerial)
#if defined(TBperS)
pc.printf("TBoldPos: %d\n",TBoldPosition);
#endif
#endif
#if defined(perProva)
pc.printf(" ildato %d Actual %d\n", ildato, TBactualPosition);
#endif
#else
TBactualPosition=0;
#endif
if (cntTbError>0) {
cntCellsCorrect++;
}
if (cntCellsCorrect>0) {
cntTbError=0;
cntCellsCorrect=0;
}
// conteggio celle erogate
if (cellsCounterLow < 0xFF) {
cellsCounterLow++;
} else {
cellsCounterHig++;
cellsCounterLow=0;
}
// ciclo conteggio celle per carico manuale
if (loadDaCanInCorso==1) {
cntCellsForLoad++;
double cellsToCount = ceil(((cellsNumber/360.0)*180.0f)+1.0f);
if (cntCellsForLoad >= cellsToCount) {
stopCicloTB=1;
cntCellsForLoad=0;
}
} else {
cntCellsForLoad=0;
}
// inibizione controllo di sincro per fuori fase
if (trigSD==0) {
inhibit=1;
trigTB=1;
} else {
inhibit=0;
trigTB=0;
trigSD=0;
}
// conta le celle indietro per sbloccare il tamburo
if ((TBmotorDirecti==TBreverse)&&(erroreTamburo==1)) {
cntCellsForReload++;
if (cntCellsForReload >= cellsCountSet) {
TBmotorDirecti=TBforward; // rotazione normale
if (tractorSpeed_MtS_timed>0.0f){
#if defined(runner)
#if defined(Zucca)
motor->run(StepperMotor::BWD);
#else
motor->run(StepperMotor::FWD);
#endif
#else
#if !defined(oldStepperDriver)
#if defined(Zucca)
motor->step_clock_mode_enable(StepperMotor::FWD);
#else
motor->step_clock_mode_enable(StepperMotor::FWD);
#endif
#endif
#endif
}
erroreTamburo=0;
cntCellsCorrect=0;
}
}
#if defined(seedSensor)
resetDelay();
delaySeedCheck.start();
#endif
}
if (((double)TBactualPosition > ((TBgiroStep/cellsNumber)*1.8f)&&(erroreTamburo==0))&&(simStepper==0)) {
if ((firstStart==0)&&(alarmEnable==true)) {
if (tractorSpeed_MtS_timed>0.4f){
if (cntTbError>5) {
all_cellSignal=1;
#if defined(seedSensor)
resetDelay();
#endif
}
}
}
if (erroreTamburo==0) {
if (cellsCountSet>1){
erroreTamburo=1;
TBmotorDirecti=TBreverse; // rotazione inversa
if (tractorSpeed_MtS_timed>0.0f){
#if defined(runner)
#if defined(Zucca)
motor->run(StepperMotor::FWD);
#else
motor->run(StepperMotor::BWD);
#endif
#else
#if !defined(oldStepperDriver)
#if defined(Zucca)
motor->step_clock_mode_enable(StepperMotor::FWD);
#else
motor->step_clock_mode_enable(StepperMotor::BWD);
#endif
#endif
#endif
}
}
cntCellsForReload=0;
if (tractorSpeed_MtS_timed>0.0f){
cntTbError++;
//aggiornati();
}else{
cntTbError=0;
}
#if defined(seedSensor)
resetDelay();
#endif
}
}
if ((tractorSpeed_MtS_timed>0.0f)&&(alarmEnable==true)){
if (((double)TBactualPosition > ((TBgiroStep/cellsNumber)*3.0f))||(cntTbError>5)) {
if (firstStart==0) {
all_noStepRota=1;
#if defined(seedSensor)
resetDelay();
#endif
}
cntTbError=0;
}
}
// ----------------------------------------
// read and manage joystick
if ((loadDaCan==1)&(simStepper==0)) {
if (tractorSpeed_MtS_timed==0.0f) {
#if defined(pcSerial)
#if defined(checkLoop)
pc.printf("daCAN\n");
#endif
#endif
ciclaTB();
}
}
//***************************************************************************************************
// pulseRised define the event of speed wheel pulse occurs
//
//double maxInterval = pulseDistance/minWorkSpeed;
//double minIntervalPulse = pulseDistance/maxWorkSpeed;
if (pulseRised==1) {
if (enableSpeed<10) {enableSpeed++;}
pulseRised=0;
pulseRised1=1;
speedMediaCalc();
// calcola velocità trattore
if(enableSpeed>=2) {
if ((pulseSpeedInterval>=0.0f)) { //minIntervalPulse)&&(pulseSpeedInterval<maxInterval)){
// todo: separare il controllo su speedfrommaster per attivarlo solo sullo slave
if((quincCnt<3)||(speedFromMaster==0.0f)||(enableSimula==1)) {
tractorSpeed_MtS_timed = ((pulseDistance / pulseSpeedInterval)); // tractor speed (unit= Mt/S) from pulse time interval
if (delaySpeedCheck.read()>=5.0f){
if ((tractorSpeed_MtS_timed<=(oldLocalTractorSpeed+hiLimitSpeed))&&(tractorSpeed_MtS_timed>=(oldLocalTractorSpeed-loLimitSpeed))){
tractorSpeed_MtS_timed=oldLocalTractorSpeed;
}
}
oldLocalTractorSpeed = tractorSpeed_MtS_timed;
}
/*if (checkSDrotation==0) {
checkSDrotation=1;
SDwheelTimer.start();
}*/
}
}
speedTimeOut.reset();
} else {
double oldLastPr = (double)oldLastPulseRead*1.7f;
if((double)speedTimeOut.read_us()> (oldLastPr)) {
tractorSpeed_MtS_timed = 0.0f;
oldLocalTractorSpeed=0.0f;
#if defined(seedSensor)
resetDelay();
#endif
pntMedia=0;
speedTimeOut.reset();
enableSpeed=0;
quincCnt=0;
}
}
#if defined(seedSensor)
if (seedSensorEnable==true) {
if (delaySeedCheck.read_ms()>100) {
if (seedSee==0) {
all_noSeedOnCe=1;
}
resetDelay();
}
}
#endif
// esegue il controllo di velocità minima
/*if ((double)speedTimer.read_ms()>=maxInterval){
tractorSpeed_MtS_timed = 0.0f;
enableSpeed=0;
}*/
// esegue il controllo di velocità massima
/*if ((double)speedTimer.read_ms()<=minIntervalPulse){
tractorSpeed_MtS_timed = 4.5f;
}*/
//***************************************************************************************************************
// cycle logic control section
//***************************************************************************************************************
if (enableSimula==1) {
if(simOk==0) {
tractorSpeed_MtS_timed=0.0f;
oldLocalTractorSpeed=0.0f;
}
}
if ((tractorSpeed_MtS_timed>0.01f)&&(simStepper==0)) {
#if defined(pcSerial)
#if defined(Qncc)
pc.printf("TsP: %f SpW: %f InPic: %f EPerc: %f Duty:%f \n",tractorSpeed_MtS_timed,speedOfSeedWheel,timeIntraPick, errorePercentuale, dcActualDuty);
#endif
#endif
cycleStopRequest=1;
// calcola il tempo teorico di passaggio becchi sulla base della velocità del trattore
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)
if (encoder==false) {
if (speedFromPick==1) {
tempoTraBecchi_mS = (tempoGiroSD / pickNumber)*1000.0f; // tempo tra due impulsi dai becchi in millisecondi ( circa 157mS a 4,5Km/h)
} else {
tempoTraBecchi_mS = (tempoGiroSD / 25.0f)*1000.0f; // tempo tra due impulsi dai becchi in millisecondi ( circa 157mS a 4,5Km/h)
}
} else {
tempoTraBecchi_mS = (tempoGiroSD / (SDreductionRatio*dcPulseTurn))*1000.0f; // tempo tra due impulsi dai becchi in millisecondi ( circa 157mS a 4,5Km/h)
#if defined(pcSerial)
#if defined(Qnce)
pc.printf("tempoGiroSD: %f SDreductionRatio: %f tempoBecchi:%f\n",tempoGiroSD,SDreductionRatio,tempoTraBecchi_mS);
#endif
#endif
#if !defined(speedMaster)
double tempoGiroSDfomMaster = seedPerimeter / speedFromMaster;
tempoBecchiPerQuinc = (tempoGiroSDfomMaster / pickNumber)*1000.0f;
#endif
}
//*******************************************
// segue calcolo duty cycle comando motore DC per allinearsi con la velocità del trattore
double dutyTeorico = 0.00;
double deltaV=0.0f;
double deltaD=0.0f;
double setV=0.0f;
double teoriaC=0.0f;
double speedCorrectionMachine=0.0f;
//#if defined(speedMaster)
speedCorrectionMachine=(seedWheelDiameter/(seedWheelDiameter-(deepOfSeed*2.0f)))*tractorSpeed_MtS_timed;
//#else
// speedCorrectionMachine=tractorSpeed_MtS_timed;
//#endif
//if ((tractorSpeed_MtS_timed>0.0)&&(tractorSpeed_MtS_timed<tabSpeed[0])) {
if ((speedCorrectionMachine>0.0f)&&(speedCorrectionMachine<tabSpeed[0])) {
dutyTeorico = tabComan[0];
}
#if defined(oldStepperDriver)
for (int ii = 0; ii<16; ii++) { // era ii<16
#endif
#if defined(runner)
for (int ii = 0; ii<tabeling; ii++) { // era ii<16
#endif
if ((speedCorrectionMachine>=tabSpeed[ii])&&(speedCorrectionMachine<tabSpeed[ii+1])) {
// esegue l'interpolazione dei valori stimati di duty in base alla velocità
deltaV=tabSpeed[ii+1]-tabSpeed[ii];
deltaD=tabComan[ii+1]-tabComan[ii];
//setV = tractorSpeed_MtS_timed-tabSpeed[ii];
setV = speedCorrectionMachine-tabSpeed[ii];
teoriaC=(setV/deltaV)*deltaD;
dutyTeorico = tabComan[ii]+teoriaC; // era ii+1 al 23/03/19
#if defined(pcSerial)
#if defined(boost1)
pc.printf("tractor: %f tabspeed1: %f tabspeed2: %f\n",tractorSpeed_MtS_timed, tabSpeed[ii],tabSpeed[ii+1]);
pc.printf("deltaV: %f deltaD: %f setV: %f teoriaC %f \n",deltaV,deltaD,setV,teoriaC);
#endif
#endif
}
}
#if defined(oldStepperDriver)
if (tractorSpeed_MtS_timed > tabSpeed[16]) {
dutyTeorico=tractorSpeed_MtS_timed/maxWorkSpeed;
}
#endif
#if defined(runner)
if (speedCorrectionMachine > tabSpeed[tabeling]) {
dutyTeorico=speedCorrectionMachine/maxWorkSpeed;
}
#endif
#if !defined(speedMaster)
quinCalc();
#endif
#if defined(pcSerial)
#if defined(Qncd)
pc.printf("enableSpeed: %d pulseRised2: %d quincCnt: %d\n",enableSpeed,pulseRised2,quincCnt);
#endif
#endif
if ((enableSpeed>3)&&(pulseRised2==1)&&(quincCnt>=2)) {
double erroreTempo = 0.0f;
if(encoder==false) {
if(speedFromPick==1) {
erroreTempo = (double)timeIntraPick - tempoTraBecchi_mS;
} else {
erroreTempo = (double)memoTimeHole - tempoTraBecchi_mS; // errore tra il tempo previsto ed il tempo reale ( >0 se sto andando più piano del previsto)
}
} else {
erroreTempo = ((double)memoTimeHole/1000.0f) - tempoTraBecchi_mS; // errore tra il tempo previsto ed il tempo reale ( >0 se sto andando più piano del previsto)
#if defined(pcSerial)
#if defined(Qnce)
pc.printf("timeHole: %d TempoBecchi: %f erroreTempo: %f\n",memoTimeHole,tempoTraBecchi_mS,erroreTempo);
#endif
#endif
}
double errorePercentuale = erroreTempo / tempoTraBecchi_mS;
double k3=0.0f;
double k4=0.0f;
double k5=0.0f;
double k6=0.0f;
#if defined(speedMaster)
k3=0.010f;
#else
k3=0.050f;
#endif
k4=1.103f;
k5=10.00f;
k6=20.50f;
double L1 = 0.045f;
double L_1=-0.045f;
double L2 = 0.150f;
double L_2=-0.150f;
double L3 = 0.301f;
double L_3=-0.301f;
double k1=0.0f;
if ((errorePercentuale > L3)||(errorePercentuale < L_3)) {
k1=errorePercentuale*k6;
}
if (((errorePercentuale >= L2)&&(errorePercentuale<=L3))||((errorePercentuale <= L_2)&&(errorePercentuale>=L_3))) {
k1=errorePercentuale*k5;
}
if (((errorePercentuale < L2)&&(errorePercentuale>L1))||((errorePercentuale > L_2)&&(errorePercentuale<L_1))) {
k1=errorePercentuale*k4;
}
if ((errorePercentuale < L1)||(errorePercentuale > L_1)) {
k1=errorePercentuale*k3;
}
double memoCorrezione = k1;
if (quincCnt >= 2) {
correzione = correzione + memoCorrezione;
if (correzione > (1.0f - dutyTeorico)) {
correzione = (1.0f - dutyTeorico);
//#if defined(pcSerial)
// pc.printf("limite\n");
//#endif
}
if ((correzione < 0.0f)&&(dutyTeorico+correzione<0.0f)) {
correzione = -1.0f*dutyTeorico;
//#if defined(pcSerial)
// pc.printf("limite\n");
//#endif
}
}
pulseRised1=0;
pulseRised2=0;
#if defined(pcSerial)
#if defined(Qnca)
pc.printf("ErTem: %f K1: %f Corr: %f MemoCorr:%f DutyTeo: %f \n",erroreTempo, k1,correzione, memoCorrezione, dutyTeorico);
pc.printf("TsP: %f SpW: %f InPic: %f TBec: %f EPerc: %f Duty:%f \n",tractorSpeed_MtS_timed,speedOfSeedWheel,timeIntraPick, tempoTraBecchi_mS,errorePercentuale, dcActualDuty);
#endif
#endif
#if defined(pcSerial)
#if defined(Qncb)
pc.printf("TsP: %f SpW: %f InPic: %f EPerc: %f Duty:%f \n",tractorSpeed_MtS_timed,speedOfSeedWheel,timeIntraPick, errorePercentuale, dcActualDuty);
#endif
#endif
#if defined(pcSerial)
#if defined(boost)
pc.printf("boost: %f teory: %f real: %f curr: %f cc:%d check: %d\n",boostDcOut,dutyTeorico, dcActualDuty,SD_CurrentScaled,correggiCorrente,currentCheckEnable);
#endif
#endif
}
// introduce il controllo di corrente
double variazione=0.0f;
if (correggiCorrente==1){
if (SD_CurrentScaled < 1.6f){
boostDcOut +=0.01f;
}
if (SD_CurrentScaled > 2.6f){
boostDcOut -=0.01f;
}
if (boostDcOut >= 0.2f){
boostDcOut=0.2f;
//all_genericals=1;
}
if (boostDcOut <=-0.2f){
boostDcOut=-0.2f;
//all_genericals=1;
}
variazione=boostDcOut;
correggiCorrente=0;
}
if (currentCheckEnable==true){
correzione += variazione;
variazione=0.0f;
}
DC_brake=0;
DC_forward=1;
DC_prepare();
// il semiperiodoreale è calcolato sulla lettura del passaggio becchi reale
seedWheelPeriod = semiPeriodoReale;
if (seedWheelPeriod < 180.0f) {
seedWheelPeriod = 180.0f;
}
if ((oldSeedWheelPeriod!=seedWheelPeriod)&&(seedWheelPeriod >=180.0f )) {
SDticker.attach_us(&step_SDPulseOut,seedWheelPeriod); // clock time are microseconds and attach seed motor stepper controls
oldSeedWheelPeriod=seedWheelPeriod;
}
if((quincCnt>=3)) {
if (correzioneAttiva==1) {
dcActualDuty = dutyTeorico + correzione;
} else {
dcActualDuty = dutyTeorico;
}
} else {
dcActualDuty = dutyTeorico;
}
if (dcActualDuty <=0.0f) {
dcActualDuty=0.05f;
}
if (dcActualDuty > 0.95f) {
dcActualDuty = 0.95f;
}
if (dcActualDuty > (dutyTeorico *1.15f)){dcActualDuty=dutyTeorico*1.15f;}
if (dcActualDuty < (dutyTeorico *0.85f)){dcActualDuty=dutyTeorico*0.85f;}
if (olddcActualDuty!=dcActualDuty) {
SDmotorPWM.write(1.0f-dcActualDuty);
olddcActualDuty=dcActualDuty;
}
// allarme
if ((tractorSpeed_MtS_timed>0.0f)&&(alarmEnable==true)){
if (SDwheelTimer.read_ms()>4000) {
if (firstStart==0) {
all_noDcRotati=1;
}
#if defined(pcSerial)
#if defined(VediAllarmi)
pc.printf("allarme no DC rotation");
#endif
#endif
}
}
//***************************************************************************************************************
// CONTROLLA TAMBURO
//***************************************************************************************************************
if(lowSpeed==0) {
if (syncroCheck==1) {
syncroCheck=0;
lockStart=1;
periodo = TBperiod;
#if !defined(oldStepperDriver)
#if !defined(runner)
#if defined(Zucca)
motor->step_clock_mode_enable(StepperMotor::BWD);
#else
motor->step_clock_mode_enable(StepperMotor::FWD);
#endif
#endif
#endif
if (aspettaStart==0) {
#if defined(pcSerial)
#if defined(checkLoop)
pc.printf("da sincro\n");
#endif
#endif
cambiaTB(periodo);
}
}
// controllo di stop
double memoIntraP = (double)memoIntraPick*1.8f;
if ((double)rotationTimeOut.read_ms()> (memoIntraP)) {
syncroCheck=0;
aspettaStart=1;
countCicli=0;
#if defined(pcSerial)
#if defined(checkLoop)
pc.printf("AspettaSI\n");
#endif
#endif
if (TBzeroCyclePulse==1) {
#if !defined(runner)
TBticker.detach();
#endif
#if !defined(oldStepperDriver)
#if defined(runner)
motor->soft_hiz();
#endif
#endif
}
}
} else { // fine ciclo fuori da low speed
syncroCheck=0;
lockStart=0;
if (beccoPronto==1) {
if (tamburoStandard==1) {
double ritardoMassimo = 0.0f;
if (encoder==false) {
if(speedFromPick==1) {
ritardoMassimo = (double)timeIntraPick;
} else {
ritardoMassimo = (double)memoTimeHole;
}
} else {
ritardoMassimo = (double)timeIntraPick;
}
int tempoDiSincro = (int)((double)(ritardoMassimo - ((tempoBecco/1.8f)+((speedOfSeedWheel/maxWorkSpeed)*ritardoMassimo)))); //
if (tempoDiSincro <= 1) {
tempoDiSincro=1;
}
if ((sincroTimer.read_ms()>= tempoDiSincro)) {
if (tractorSpeed_MtS_timed >= minWorkSpeed) {
startCicloTB=1;
}
beccoPronto=0;
}
} else {
// tamburo per zucca
if (speedOfSeedWheel >= minWorkSpeed) {
startCicloTB=1;
}
beccoPronto=0;
}
}
ciclaTB();
}
//*************************************************************
} else { // fine ciclo con velocita maggiore di 0
delaySpeedCheck.reset();
if (cycleStopRequest==1) {
SDwheelTimer.stop();
SDwheelTimer.reset();
#if defined(seedSensor)
resetDelay();
#endif
checkSDrotation=0;
oldFaseLavoro=0;
aspettaStart=1;
countCicli=0;
oldSeedWheelPeriod=0.0f;
oldPeriodoTB=0.0f;
correzione=0.0f;
OLDpulseSpeedInterval=1000.01f;
cicloTbinCorso=0;
cntTbError=0;
olddcActualDuty=0.0f;
#if defined(pcSerial)
#if defined(checkLoopb)
pc.printf("forza\n");
#endif
#endif
speedOfSeedWheel=0.0f;
cycleStopRequest=0;
DC_brake=1;
DC_prepare();
metalTimer.reset();
#if defined(pcSerial)
#if defined(checkLoop)
pc.printf("17h\n");
#endif
#endif
#if !defined(runner)
TBticker.detach();
#endif
#if defined(pcSerial)
#if defined(loStop)
pc.printf("A5\n");
#endif
#endif
if(simStepper==0){
#if defined(runner)
motor->soft_hiz();
motor->set_home();
#endif
#if defined(oldStepperDriver)
TBticker.detach();
#endif
}
cntSpeedError=0;
timeIntraPick=0.0f;
memoIntraPick=0.001f;
intraPickTimer.reset();
pntMedia=0;
#if defined(pcSerial)
#if defined(stopSignal)
pc.printf("stop\n");
#endif
#endif
}
}
//*************************************************************************************************
TBzeroCyclePulse=0;
//*************************************************************************************************
} //end inProva==0
wd.Service(); // kick the dog before the timeout
} // end while
} // end main