Forigo
/
FORIGO_Modula_V7_3_VdcStep_DICEMBRE2020
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed X_NUCLEO_IHM03A1_for
Diff: main.cpp
- Revision:
- 8:310f9e4eac7b
- Parent:
- 6:e8c18f0f399a
- Child:
- 9:7f02256f6e8f
--- a/main.cpp Tue Feb 19 17:03:53 2019 +0000
+++ b/main.cpp Mon Mar 11 06:44:59 2019 +0000
@@ -13,7 +13,7 @@
//
// THIS RELEASE: 10 february 2019
//
-// APPLICATION: MODULA CON DISTRIBUTORE RISO ED ENCODER MOTORE
+// 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
@@ -38,7 +38,7 @@
- powerstep.hpp
- watchdog.cpp
- watchdog.h
-ED UTILIZZA LE LIBRERIE STANDARD MBED PIU'
+ED UTILIZZA LE LIBRERIE STANDARD MBED PIU'
UNA LIBRERIA MODIFICATA E DEDICATA PER IL CAN
UNA LIBRERIA DEDICATA PER IL DRIVER STEPPER
*********************
@@ -48,7 +48,7 @@
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
+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
@@ -86,15 +86,22 @@
//********************************************************************************************************************
//********************************************************************************************************************
#include "main.hpp"
+/* Helper header files. */
+#include "DevSPI.h"
+/* Component specific header files. */
+#include "PowerStep01.h"
#include "timeandtick.hpp"
#include "canbus.hpp"
#include "watchdog.h"
#include "iodefinition.hpp"
#include "parameters.hpp"
#include "variables.hpp"
+#include "powerstep.hpp"
//********************************************************************************************************************
//********************************************************************************************************************
-
+#if defined(runnerTos)
+ Thread thread;
+#endif
/* Variables -----------------------------------------------------------------*/
@@ -116,47 +123,47 @@
motor->isrFlag = TRUE;
/* Get the value of the status register. */
unsigned int statusRegister = motor->get_status();
- #if defined(pcSerial)
- printf(" WARNING: \"FLAG\" interrupt triggered.\r\n");
- #endif
+#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)
- printf(" SW closed (connected to ground).\r\n");
- #endif
+#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)
- printf(" SW turn_on event.\r\n");
- #endif
+#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)
- printf(" Non-performable command detected.\r\n");
- #endif
- }
+#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)
- printf(" undervoltage lock-out.\r\n");
- #endif
- }
+#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)
- printf(" Thermal status: %d.\r\n", (statusRegister & POWERSTEP01_STATUS_TH_STATUS)>>11);
- #endif
- }
+ //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)
- printf(" Overcurrent detection.\r\n");
- #endif
+#if defined(pcSerial)
+ pc.printf(" Overcurrent detection.\r\n");
+#endif
}
/* Reset ISR flag. */
motor->isrFlag = FALSE;
@@ -172,29 +179,74 @@
void my_error_handler(uint16_t error)
{
/* Printing to the console. */
- #if defined(pcSerial)
- printf("Error %d detected\r\n\n", error);
- #endif
-
+#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();
+ if (TBpassPosition >= TBoldPosition){
+ TBactualPosition= ((TBpassPosition-TBoldPosition)*TBreductionRatio);//*10;
+ #if defined(pcSerial)
+ #if defined(rtosData)
+ printf(" 1 Position: %d TBpass: %d Tbold: %d \r\n", TBactualPosition, TBpassPosition, TBoldPosition);
+ #endif
+ #endif
+ }else{
+ TBactualPosition=((((2097152-TBoldPosition)+TBpassPosition))*TBreductionRatio);//*10;
+ #if defined(pcSerial)
+ #if defined(rtosData)
+ printf(" 2 Position: %d TBpass: %d Tbold: %d \r\n", TBactualPosition, TBpassPosition, TBoldPosition);
+ #endif
+ #endif
+ }
+ //wait_us(50); // 50 mS di intervallo lettura
+ //}
+ }
+#endif
//*******************************************************************************
//********************************************************************************************************************
// ------------------------------------------------------------------------------------------------------------------------------------------------------------------
// TASK SECTION
// ------------------------------------------------------------------------------------------------------------------------------------------------------------------
//************************************************************************
+void aggioVelocita()
+{
+ realGiroSD = seedPerimeter / speedOfSeedWheel;
+ tempoBecco = (realGiroSD/360.0f)*16000.0f;
+ frequenzaReale = fixedStepGiroSD/realGiroSD;
+ semiPeriodoReale = (1000000.0f/frequenzaReale);
+ //tempoTraBecchi_mS = 0.0f;
+ seedWheelRPM = (speedOfSeedWheel)*K_WheelRPM ; // calcola i giri al minuto della ruota di semina 7.37 31,75
+ TBrpm = seedWheelRPM*rapportoRuote; // 5.896 31,75
+ TBfrequency = (TBrpm*K_TBfrequency); // 130Hz a 0,29Mts 1397,00 a 1,25mt/s con 15 becchi e 15 celle
+ #if defined(runner)
+ TBperiod=5.2f*TBrpm; //prova dopo test con contagiri
+ //5.681818f
+ #else
+ TBperiod=1000000.0f/TBfrequency; // 715uS
+ #endif
+
+}
//************************************************************************
// rise of seed speed 25 pulse sensor
-void sd25Fall(){
+void sd25Fall()
+{
timeHole=metalTimer.read_ms();
int memo_TimeHole= (memoTimeHole + timeHole)/ (int)2;
memoTimeHole = timeHole;
metalTimer.reset();
- if (speedFromPick==0){
+ if (speedFromPick==0) {
speedOfSeedWheel=((seedPerimeter/25.0f)/(double)memo_TimeHole)*1000.0f; //mtS
}
#if defined(pcSerial)
@@ -209,7 +261,7 @@
int memo_TimeHole= (memoTimeHole + timeHole)/ (int)2;
memoTimeHole = timeHole;
metalTimer.reset();
- if (encoder==true){
+ if (encoder==true) {
speedOfSeedWheel=((seedPerimeter/((SDreductionRatio*25.5f)))/(double)memo_TimeHole)*1000000.0f; //mtS
pulseRised2=1;
}
@@ -218,6 +270,7 @@
pc.printf("2\n");
#endif
#endif
+ aggioVelocita();
}
//**************************************************
// rise of seed presence sensor
@@ -231,7 +284,7 @@
}
//**************************************************
// generate speed clock when speed is simulated from Tritecnica display
-void speedSimulationClock(){
+void speedSimulationClock(){
lastPulseRead=speedTimer.read_us();
oldLastPulseRead=lastPulseRead;
speedTimer.reset();
@@ -247,14 +300,14 @@
// interrupt task for tractor speed reading
//*******************************************************
void tractorReadSpeed(){
- if ((oldTractorSpeedRead==0)){
+ if ((oldTractorSpeedRead==0)) {
lastPulseRead=speedTimer.read_us();
oldLastPulseRead=lastPulseRead;
speedTimer.reset();
pulseRised=1;
oldTractorSpeedRead=1;
- speedClock=1;
}
+ speedClock=1;
speedFilter.reset();
#if defined(pcSerial)
#if defined(checkLoop)
@@ -263,51 +316,54 @@
#endif
}
//*******************************************************
-void speedMediaCalc(){
+void speedMediaCalc()
+{
double lastPd=(double) lastPulseRead/1000.0f;
pulseSpeedInterval = (mediaSpeed[0]+lastPd)/2.0f;
- if (enableSimula==1){
+ 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
-}
+#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(){
+void step_SDPulseOut()
+{
SDactualPosition++;
prePosSD++;
- #if defined(speedMaster)
- posForQuinc++;
- #endif
- #if defined(pcSerial)
- #if defined(checkLoop)
- pc.printf("7\n");
- #endif
- #endif
+#if defined(speedMaster)
+ posForQuinc++;
+#endif
+#if defined(pcSerial)
+#if defined(checkLoop)
+ pc.printf("7\n");
+#endif
+#endif
}
//*******************************************************
-void step_TBPulseOut(){
+void step_TBPulseOut()
+{
TBmotorStepOut=!TBmotorStepOut;
- if (TBmotorStepOut==0){
- if (TBmotorDirecti==TBforward){
+ if (TBmotorStepOut==0) {
+ if (TBmotorDirecti==TBforward) {
TBactualPosition++;
}
}
- #if defined(pcSerial)
- #if defined(stepTamb)
- pc.printf("step\n");
- #endif
- #endif
+#if defined(pcSerial)
+#if defined(stepTamb)
+ pc.printf("step\n");
+#endif
+#endif
/*
#if defined(pcSerial)
#if defined(checkLoop)
@@ -317,13 +373,18 @@
*/
}
//*******************************************************
-void invertiLo(){
- if (TBmotorDirecti==TBreverse){
+void invertiLo()
+{
+ if (TBmotorDirecti==TBreverse) {
TBmotorDirecti=TBforward;
- motor->step_clock_mode_enable(StepperMotor::FWD);
- }else{
+ #if !defined(runner)
+ motor->step_clock_mode_enable(StepperMotor::FWD);
+ #endif
+ } else {
TBmotorDirecti=TBreverse;
- motor->step_clock_mode_enable(StepperMotor::BWD);
+ #if !defined(runner)
+ motor->step_clock_mode_enable(StepperMotor::BWD);
+ #endif
}
#if defined(pcSerial)
#if defined(inversione)
@@ -339,7 +400,8 @@
}
//*******************************************************
// aggiornamento parametri di lavoro fissi e da Tritecnica
-void aggiornaParametri(){
+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
@@ -349,17 +411,21 @@
rapportoRuote = pickNumber/cellsNumber; // calcola il rapporto tra il numero di becchi ed il numero di celle
SDsectorStep = (double)fixedStepGiroSD / (double)pickNumber;
TBsectorStep = (TBmotorSteps*TBreductionRatio)/cellsNumber;
- KcorT = (SDsectorStep/TBsectorStep);///2.0f;
+ #if defined(runner)
+ KcorT = (SDsectorStep/TBsectorStep)/2.0f;
+ #else
+ KcorT = (SDsectorStep/TBsectorStep);///2.0f;
+ #endif
angoloFase=angoloPh;
avvioGradi=angoloAv;
stepGrado=fixedStepGiroSD/360.0f;
TBdeltaStep=(fixedStepGiroSD/pickNumber)+(stepGrado*avvioGradi);
- TBfaseStep = (stepGrado*angoloFase);
+ 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{
+ } else {
intraPickDistance = seedPerimeter/25.0f; // 25 è il numero di fori presenti nel disco di semina
}
#if defined(pcSerial)
@@ -369,49 +435,70 @@
#endif
}
//*******************************************************
-void cambiaTB(double perio){
- // 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)){
+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(checkLoop)
- pc.printf("11a\n");
- pc.printf("11a TBper: %f \n",TBper);
+ #if defined(TBperSo)
+ pc.printf("TBper: %f MtS: %f\n",TBper,tractorSpeed_MtS_timed);
#endif
#endif
- if (TBper != NULL){
- motor->step_clock_mode_enable(StepperMotor::FWD);
- 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");
+ motor->run(StepperMotor::FWD,TBper);
+ 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
- #endif
- TBticker.detach();
- #if defined(pcSerial)
- #if defined(loStop)
- pc.printf("A1\n");
+ if (TBper != NULL) {
+ motor->step_clock_mode_enable(StepperMotor::FWD);
+ 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
- #endif
- motor->soft_hiz();
+ TBticker.detach();
+ #if defined(pcSerial)
+ #if defined(loStop)
+ pc.printf("A1\n");
+ #endif
+ #endif
+ motor->soft_hiz();
+ }
+ oldPeriodoTB=TBper;
}
- oldPeriodoTB=TBper;
}
- }
+ #endif
}
//*******************************************************
-void seedCorrect(){
+void seedCorrect()
+{
/*
posError determina la posizione relativa di TB rispetto ad SD
- la reale posizione di SD viene modificata in funzione della velocità per
+ 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à,
@@ -427,10 +514,10 @@
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;
@@ -439,29 +526,29 @@
stepGrado=fixedStepGiroSD/360.0f;
avvioGradi = costante da terminale tritecnica
TBdeltaStep=(fixedStepGiroSD/pickNumber)-(stepGrado*avvioGradi);
- TBfaseStep = (stepGrado*angoloFase);
+ TBfaseStep = (stepGrado*angoloFase);
*/
#if defined(Zucca)
- if ((tractorSpeed_MtS_timed>0.01f)){
- if (inhibit==0){
+ if ((tractorSpeed_MtS_timed>0.01f)) {
+ if (inhibit==0) {
double posError =0.0f;
double posSD=((double)SDactualPosition)/KcorT;
- posError = posSD - (double)TBactualPosition;
+ posError = posSD - (double)TBactualPosition;
// interviene sulla velocità di TB per raggiungere la corretta posizione relativa
- if((lowSpeed==0)&&(aspettaStart==0)){
+ if((lowSpeed==0)&&(aspettaStart==0)) {
//if (posError>50.0f){posError=50.0f;}
//if (posError<-50.0f){posError=-50.0f;}
- if ((posError >=1.0f)||(posError<=-1.0f)){
+ if ((posError >=1.0f)||(posError<=-1.0f)) {
ePpos = periodo *(1.0f+ ((posError/100.0f)));
#if defined(pcSerial)
#if defined(checkLoop)
- pc.printf("da zucca\n");
+ pc.printf("da zucca\n");
#endif
#endif
- if (ePpos>0.0f){
+ if (ePpos>0.0f) {
cambiaTB(ePpos);
- }else{
- cambiaTB(periodo);///2.0f);
+ } else {
+ cambiaTB(periodo)/2.0f;
}
#if defined(pcSerial)
#if defined(TBperS)
@@ -473,112 +560,138 @@
}
}
#else
- if ((tractorSpeed_MtS_timed>0.01f)){
- if (inhibit==0){
+ if ((tractorSpeed_MtS_timed>0.01f)) {
+ if (inhibit==0) {
double posError =0.0f;
double posSD=((double)SDactualPosition)/KcorT;
- posError = posSD - (double)TBactualPosition;
+ posError = posSD - (double)TBactualPosition;
// interviene sulla velocità di TB per raggiungere la corretta posizione relativa
- if((lowSpeed==0)&&(aspettaStart==0)){
+ if((lowSpeed==0)&&(aspettaStart==0)) {
double lowLim=-50.0f;
double higLim= 50.0f;
double divide= 100.0f;
- if (pickNumber <= 5){
+ if (pickNumber <= 5) {
lowLim=-500.0f;
higLim= 500.0f;
divide= 25.0f;
- }else{
- lowLim=-50.0f;
- higLim= 50.0f;
+ } else {
+ lowLim=-10.0f;
+ higLim= 130.0f;
divide= 100.0f;
}
- if (posError>higLim){posError=higLim;}
- if (posError<lowLim){posError=lowLim;}
- if ((posError >=1.0f)||(posError<=-1.0f)){
- ePpos = periodo /(1.0f+ ((posError/divide)));
+ if (posError>higLim) {
+ //posError=higLim;
+ posError=0.0f;
+ motor->soft_hiz();
+ }
+ if (posError<lowLim) {
+ posError=lowLim;
+ }
+ if ((posError >=1.0f)||(posError<=-1.0f)) {
+ #if defined(runner)
+ ePpos = periodo *(1.0f+ ((posError/divide)));
+ #else
+ ePpos = periodo /(1.0f+ ((posError/divide)));
+ #endif
#if defined(pcSerial)
#if defined(checkLoop)
pc.printf("12a ePpos:%f\n",ePpos);
#endif
#endif
- if (ePpos>0.0f){
+ if (ePpos>0.0f) {
cambiaTB(ePpos);
- }else{
+ } else {
cambiaTB(periodo);///2.0f);
}
}
#if defined(pcSerial)
#if defined(TBperS)
- pc.printf("TBpos: %f SDpos: %f SDact: %f Err: %f Correggi: %f\n",(double)TBactualPosition,posSD,(double)SDactualPosition,posError,ePpos);
+ pc.printf("TBpos: %f SDpos: %f SDact: %f Err: %f Correggi: %f periodo: %f \n",(double)TBactualPosition,posSD,(double)SDactualPosition,posError,ePpos,periodo);
#endif
#endif
}
}
}
#endif
- #if defined(pcSerial)
- #if defined(checkLoopa)
- pc.printf("12\n");
- #endif
- #endif
+#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];}
+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];}
+ 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
+#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)){
+void ciclaTB()
+{
+ if ((startCicloTB==1)&&(cicloTbinCorso==0)) {
#if defined(pcSerial)
#if defined(checkLoop)
pc.printf("14a TBperiod: %f\n",TBperiod);
#endif
#endif
- if (TBperiod >= (250.0f*2.5f)){
- if (TBperiod != NULL){
- motor->step_clock_mode_enable(StepperMotor::FWD);
- TBticker.attach_us(&step_TBPulseOut,TBperiod/2.5f); // clock time are milliseconds and attach seed motor stepper controls
+ #if defined(runner)
+ motor->run(StepperMotor::FWD,TBperiod);
+ #else
+ if (TBperiod >= (250.0f*2.0f)) {
+ if (TBperiod != NULL) {
+ motor->step_clock_mode_enable(StepperMotor::FWD);
+ TBticker.attach_us(&step_TBPulseOut,TBperiod/2.0f); // clock time are milliseconds and attach seed motor stepper controls
+ }
}
- cicloTbinCorso = 1;
- startCicloTB=0;
- }
+ #endif
+ cicloTbinCorso = 1;
+ startCicloTB=0;
}
- if ((loadDaCan==1)&&(loadDaCanInCorso==0)){
+ if ((loadDaCan==1)&&(loadDaCanInCorso==0)) {
#if defined(pcSerial)
#if defined(checkLoop)
pc.printf("14b\n");
#endif
#endif
- motor->step_clock_mode_enable(StepperMotor::FWD);
- TBticker.attach_us(&step_TBPulseOut,1000.0f); // clock time are milliseconds and attach seed motor stepper controls
+ #if defined(runner)
+ motor->run(StepperMotor::FWD,50.0f);
+ #else
+ motor->step_clock_mode_enable(StepperMotor::FWD);
+ 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)){
+ if ((stopCicloTB==1)&&(TBactualPosition>5)) {
#if defined(pcSerial)
#if defined(checkLoop)
pc.printf("14c\n");
#endif
#endif
- TBticker.detach();
+ #if !defined(runner)
+ TBticker.detach();
+ #endif
#if defined(pcSerial)
#if defined(loStop)
pc.printf("A2\n");
@@ -592,25 +705,26 @@
}
}
// ------------------------------------------------------------------------------------------------------------------------------------------------------------------
-void stepSetting(){
+void stepSetting()
+{
// Stepper driver init and set
TBmotorRst=0; // reset stepper driver
TBmotorDirecti=TBforward; // reset stepper direction
- TBmotorRst=1;
#if defined(pcSerial)
#if defined(checkLoop)
pc.printf("15\n");
#endif
#endif
+ TBmotorRst=1;
}
//****************************************
void dcSetting(){
- if ((speedFromPick==0)&&(encoder==false)){
- DcEncoder.rise(&sd25Fall);
- }
- if (encoder==true){
- DcEncoder.rise(&encoRise);
- }
+ if ((speedFromPick==0)&&(encoder==false)) {
+ DcEncoder.rise(&sd25Fall);
+ }
+ if (encoder==true) {
+ DcEncoder.rise(&encoRise);
+ }
#if defined(pcSerial)
#if defined(checkLoop)
pc.printf("16\n");
@@ -618,88 +732,111 @@
#endif
}
//*******************************************************
-void allarmi(){
- uint8_t alarmLowRegister1=0x00;
- alarmLowRegister=0x00;
- alarmHighRegister=0x80;
+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
- }
+ //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;
- }
+ //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(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 /0x00FF0000)&0x000000FF;
- val1[2]=(posForQuinc /0x0000FF00)&0x000000FF;
- val1[1]=(posForQuinc /0x000000FF)&0x000000FF;
+ 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 /0x0000FF00)&0x000000FF;
- val1[6]=(prePosSD /0x000000FF)&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))){
+ if(can1.write(CANMessage(TX_SI, *&val1,8))) {
checkState=0;
}
#endif
@@ -712,7 +849,8 @@
}
#endif
//*******************************************************
-void upDateSpeed(){
+void upDateSpeed()
+{
/*
aggiorna dati OPUSA3
val1[0] contiene il dato di velocità
@@ -742,26 +880,26 @@
bit 5= controller
bit 6= motore stepper
*/
- char val1[8]={0,0,0,0,0,0,0,0};
+ 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;
- #endif
+ 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;
+#endif
allarmi();
valore = totalSpeed*36.0f; // tractorSpeed_MtS_timed*36.0f;
val1[0]=valore;
@@ -771,133 +909,135 @@
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
+#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.009f);
- canUpdate.attach(&upDateSpeed,0.21f);
- #else
- canUpdate.attach(&upDateSpeed,0.407f);
+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
- 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)
+ #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];
- #if defined(pcSerial)
- #if defined(canDataReceived)
+ #endif
+ enableSimula= rxMsg.data[0];
+ speedSimula = rxMsg.data[1];
+ avviaSimula = rxMsg.data[2];
+ selezionato = rxMsg.data[3];
+ comandiDaCan = rxMsg.data[4];
+ #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;
+ #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;
}
- #if defined(pcSerial)
- #if defined(canDataReceivedR)
- pc.printf("Comandi: %x\n",resetComandi);
- #endif
- #endif
- if (speedSimula>45){
- speedSimula=45;
+ }
+ // modulo 2
+ if (RX_ID==0x102) {
+ if ((selezionato&0x02)==0x02) {
+ simOk=1;
+ } else {
+ simOk=0;
}
- // 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 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 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;
}
- // modulo 5
- if (RX_ID==0x108){
- if ((selezionato&0x10)==0x10){
- simOk=1;
- }else{
- simOk=0;
- }
- }
-
}
- if (rxMsg.id==RX_Settings){
- if (tractorSpeed_MtS_timed==0.0f){
+
+ }
+ 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=(rxMsg.data[2]/10000.0f); // deep of seeding
+ deepOfSeed=(rxMsg.data[2]/10000.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 (rxMsg.id==RX_AngoloPh){
- if (tractorSpeed_MtS_timed==0.0f){
+ if (rxMsg.id==RX_AngoloPh) {
+ if (tractorSpeed_MtS_timed==0.0f) {
#if defined(M1)
angoloPh = (double) rxMsg.data[0] ;
aggiornaParametri();
@@ -924,8 +1064,8 @@
#endif
}
}
- if (rxMsg.id==RX_AngoloAv){
- if (tractorSpeed_MtS_timed==0.0f){
+ if (rxMsg.id==RX_AngoloAv) {
+ if (tractorSpeed_MtS_timed==0.0f) {
#if defined(M1)
angoloAv = (double) rxMsg.data[0] ;
aggiornaParametri();
@@ -952,263 +1092,282 @@
#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];
+ 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 (rxMsg.id==RX_QuincSinc){
- masterSinc = (uint32_t) rxMsg.data[3] * 0x00FF0000;
- masterSinc = masterSinc + ((uint32_t) rxMsg.data[2] * 0x0000FF00);
- masterSinc = masterSinc + ((uint32_t) rxMsg.data[1] * 0x000000FF);
+ }
+ 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] * 0x0000FF00);
- mast2_Sinc = mast2_Sinc + ((uint32_t) rxMsg.data[6] * 0x000000FF);
+ 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 (rxMsg.id==RX_Configure){
+ }
+ if (tractorSpeed_MtS_timed <= 0.01f){
+ if (rxMsg.id==RX_Configure) {
uint8_t flags = rxMsg.data[0];
uint16_t steps = (uint32_t) rxMsg.data[2]*0x00000100;
steps = steps + ((uint32_t)rxMsg.data[1]);
TBmotorSteps =steps;
- stepSetting();
+ //stepSetting();
cellsCountSet = rxMsg.data[3];
- if ((flags&0x01)==0x01){
- if (encoder==false){
+ if ((flags&0x01)==0x01) {
+ if (encoder==false) {
encoder=true;
DcEncoder.rise(NULL);
dcSetting();
}
- }else{
- if (encoder==true){
+ } else {
+ if (encoder==true) {
encoder=false;
DcEncoder.rise(NULL);
dcSetting();
}
}
- if ((flags&0x02)==0x02){
+ if ((flags&0x02)==0x02) {
tankLevelEnable=true;
- }else{
+ } else {
tankLevelEnable=false;
}
- if ((flags&0x04)==0x04){
+ if ((flags&0x04)==0x04) {
seedSensorEnable=true;
- }else{
+ } else {
seedSensorEnable=false;
}
- if ((flags&0x08)==0x08){
+ if ((flags&0x08)==0x08) {
stendiNylonEnable=true;
- }else{
+ } else {
stendiNylonEnable=false;
}
- if ((flags&0x10)==0x10){
+ if ((flags&0x10)==0x10) {
canDataCheckEnable=true;
- }else{
+ } else {
canDataCheckEnable=false;
}
- if ((flags&0x20)==0x20){
+ if ((flags&0x20)==0x20) {
tamburoStandard=1;
- }else{
+ } else {
tamburoStandard=0;
}
- if ((flags&0x40)==0x40){
+ if ((flags&0x40)==0x40) {
currentCheckEnable=true;
- }else{
+ } else {
currentCheckEnable=false;
}
}
- }
- #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
+ }
+ }
+#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(){
-
+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;
//if (pwmCheck==1){
- timeout.detach();
- for (int ii=1;ii<20;ii++){
- SD_analogMatrix[ii]=SD_analogMatrix[ii+1];
- }
- SD_CurrentAnalog = floor(SDcurrent.read()*100)/100; // valore in ingresso compreso tra 0.00 e 1.00
- SD_analogMatrix[20]=SD_CurrentAnalog;
- if (SDmotorPWM==0.0f){
- SD_CurrentStart=SD_CurrentAnalog;
- }
- float sommaTutto=0.0f;
- for (int ii=1;ii<21;ii++){
- sommaTutto=sommaTutto+SD_analogMatrix[ii];
+ timeout.detach();
+ for (int ii=1; ii<20; ii++) {
+ SD_analogMatrix[ii]=SD_analogMatrix[ii+1];
+ }
+ SD_CurrentAnalog = floor(SDcurrent.read()*100)/100; // valore in ingresso compreso tra 0.00 e 1.00
+ SD_analogMatrix[20]=SD_CurrentAnalog;
+ if (SDmotorPWM==0.0f) {
+ SD_CurrentStart=SD_CurrentAnalog;
+ }
+ float sommaTutto=0.0f;
+ for (int ii=1; ii<21; ii++) {
+ sommaTutto=sommaTutto+SD_analogMatrix[ii];
+ }
+ float SD_CurrentAnalogica=sommaTutto/20.0f;
+ SD_CurrentScaled = floor(( (SD_CurrentAnalogica - SD_CurrentStart)*3.3f) / (SD_CurrentFactor/1000.0f)*10)/10;
+#if defined(pcSerial)
+#if defined(correnteAssorbita)
+ pc.printf("CorrenteStart: %f ",SD_CurrentStart);
+ pc.printf(" CorrenteScaled: %f ",SD_CurrentScaled);
+ pc.printf(" CorrenteMedia: %f \n",SD_CurrentAnalogica);
+#endif
+#endif
+ reduceCurrent=false;
+ incrementCurrent=false;
+ /*
+ if (SD_CurrentScaled < 3.0f){
+ #if defined(canbusActive)
+ all_lowBattery=1;
+ #endif
+ incrementCurrent=true;
+ }
+ */
+ if (SD_CurrentScaled > 10.0f) {
+ timeCurr.start();
+ if (timeCurr.read() > 5.0f) {
+#if defined(canbusActive)
+ all_overCurrDC=1;
+#endif
+ reduceCurrent=true;
+ timeCurr.reset();
}
- float SD_CurrentAnalogica=sommaTutto/20.0f;
- SD_CurrentScaled = floor(( (SD_CurrentAnalogica - SD_CurrentStart)*3.3f) / (SD_CurrentFactor/1000.0f)*10)/10;
- #if defined(pcSerial)
- #if defined(correnteAssorbita)
- pc.printf("CorrenteStart: %f ",SD_CurrentStart);
- pc.printf(" CorrenteScaled: %f ",SD_CurrentScaled);
- pc.printf(" CorrenteMedia: %f \n",SD_CurrentAnalogica);
- #endif
- #endif
- reduceCurrent=false;
- incrementCurrent=false;
- /*
- if (SD_CurrentScaled < 3.0f){
- #if defined(canbusActive)
- all_lowBattery=1;
- #endif
- incrementCurrent=true;
- }
- */
- if (SD_CurrentScaled > 10.0f){
- timeCurr.start();
- if (timeCurr.read() > 5.0f){
- #if defined(canbusActive)
- all_overCurrDC=1;
- #endif
- reduceCurrent=true;
- timeCurr.reset();
- }
- }else{
- timeCurr.stop();
- }
+ } else {
+ timeCurr.stop();
+ }
//}
- #if defined(pcSerial)
- #if defined(checkLoop)
- pc.printf("21\n");
- #endif
- #endif
+#if defined(pcSerial)
+#if defined(checkLoop)
+ pc.printf("21\n");
+#endif
+#endif
}
//*******************************************************
-void DC_prepare(){
+void DC_prepare()
+{
// direction or brake preparation
- if (DC_brake==1){
+ if (DC_brake==1) {
SDmotorInA=1;
SDmotorInB=1;
- }else{
- if (DC_forward==0){
+ } else {
+ if (DC_forward==0) {
SDmotorInA=1;
SDmotorInB=0;
- }else{
+ } 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
+ 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(){
- if (currentCheckEnable==true){
+void startDelay()
+{
+ if (currentCheckEnable==true) {
int ritardo =0;
ritardo = (int)((float)(dcActualDuty*800.0f));
- if (ritardo>250.0f){
+ if (ritardo>250.0f) {
timeout.attach_us(&DC_CheckCurrent,ritardo);
}
}
- #if defined(pcSerial)
- #if defined(checkLoop)
- pc.printf("23\n");
- #endif
- #endif
+#if defined(pcSerial)
+#if defined(checkLoop)
+ pc.printf("23\n");
+#endif
+#endif
}
//*******************************************************
-void quincTrigon(){
+void quincTrigon()
+{
quincClock=true;
- #if defined(pcSerial)
- #if defined(checkLoop)
- pc.printf("24\n");
- #endif
- #endif
+#if defined(pcSerial)
+#if defined(checkLoop)
+ pc.printf("24\n");
+#endif
+#endif
}
-void quincTrigof(){
+void quincTrigof()
+{
quincClock=false;
- #if defined(pcSerial)
- #if defined(checkLoop)
- pc.printf("25\n");
- #endif
- #endif
+#if defined(pcSerial)
+#if defined(checkLoop)
+ pc.printf("25\n");
+#endif
+#endif
}
//*******************************************************
-void quinCalc(){
+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 !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){
+ }
+ 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 ((((quinconceActive==0)&&(quincClock==true))||((quinconceActive==1)&&(quincClock==false)))&&(oldQuincIn==0)){
- oldQuincIn=1;
- if (quincStart==0){
- oldQuincTimeSD = (double) quincTimeSD.read_ms();
- quincTime.reset();
- quincStart=1;
- }
+ } else {
+ if (quincClock==true) {
+ oldQuincIn=0;
}
- if (quinconceActive==0){
- if (quincClock==false){
- oldQuincIn=0;
- }
- }else{
- if (quincClock==true){
- oldQuincIn=0;
- }
- }
- #endif
+ }
+#endif
//****************************************************************************************
- if (quincCnt>=4){
- if (countPicks==0){
- if ((sincroQui==1)&&(quincStart==0)){
+ if (quincCnt>=4) {
+ if (countPicks==0) {
+ if ((sincroQui==1)&&(quincStart==0)) {
// decelera
countPicks=1;
}
- if ((sincroQui==0)&&(quincStart==1)){
+ if ((sincroQui==0)&&(quincStart==1)) {
// accelera
countPicks=2;
}
}
- if ((sincroQui==1)&&(quincStart==1)){
- if (countPicks==1){ //decelera
+ if ((sincroQui==1)&&(quincStart==1)) {
+ if (countPicks==1) { //decelera
scostamento = oldQuincTimeSD;
- if (scostamento < (tempoBecchiPerQuinc*0.75f)){
+ if (scostamento < (tempoBecchiPerQuinc*0.75f)) {
double scostPerc = (scostamento/tempoBecchiPerQuinc);
percento -= ((double)quincPIDminus/100.0f)*(scostPerc);
#if defined(pcSerial)
@@ -1219,15 +1378,14 @@
}
//if (scostamento <15.0f){percento=0.0f;}
}
- if (countPicks==2){ //accelera
+ if (countPicks==2) { //accelera
scostamento = (double)quincTime.read_ms();
- if (scostamento < (tempoBecchiPerQuinc*0.75f)){
+ if (scostamento < (tempoBecchiPerQuinc*0.75f)) {
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);
+ pc.printf("ACCE scos1: %f tbpq: %f percento: %f \n",scostamento,tempoBecchiPerQuinc,percento);
#endif
#endif
}
@@ -1238,42 +1396,42 @@
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 (quincCnt>=3) {
+ if (speedFromMaster>0.0f) {
+ if (enableSimula==0) {
+ tractorSpeed_MtS_timed = speedFromMaster + percento;
}
+ }
}
#endif
}
-
+
//*******************************************************************
- if (canDataCheckEnable==true){
- if (canDataCheck==1){ // sincro da comunicazione can del valore di posizione del tamburo master
+ if (canDataCheckEnable==true) {
+ if (canDataCheck==1) { // sincro da comunicazione can del valore di posizione del tamburo master
canDataCheck=0;
double parametro = SDsectorStep/3.0f;
double differenza=0.0f;
#if defined(mezzo)
- if (quinconceActive==1){
+ if (quinconceActive==1) {
differenza = (double)masterSinc - (double)prePosSD;
- }else{
+ } else {
differenza = (double)mast2_Sinc - (double)prePosSD;
}
#else
differenza = (double)mast2_Sinc - (double)prePosSD;
#endif
- if ((differenza > 0.0f)&&(differenza < parametro)){
- double diffPerc = differenza / parametro;
+ if ((differenza > 0.0f)&&(differenza < parametro)) {
+ double diffPerc = differenza / parametro;
percento += ((double)quincPIDplus/100.0f)*abs(diffPerc);
#if defined(pcSerial)
- #if defined(quinca)
+ #if defined(quinca)
pc.printf("m1 %d m2 %d prePo %d diffe %f perce %f parm %f %\n",masterSinc, mast2_Sinc,prePosSD,differenza,percento, parametro);
#endif
#endif
}
- if ((differenza < 0.0f)&&(abs(differenza) < parametro)){
- double diffPerc = (double)differenza / parametro;
+ if ((differenza < 0.0f)&&(abs(differenza) < parametro)) {
+ double diffPerc = (double)differenza / parametro;
percento -= ((double)quincPIDminus/100.0f)*abs(diffPerc);
#if defined(pcSerial)
#if defined(quinca)
@@ -1283,9 +1441,9 @@
}
// questo e il secondo
#if !defined(speedMaster)
- if (quincCnt>=3){
- if (speedFromMaster>0.0f){
- if (enableSimula==0){
+ if (quincCnt>=3) {
+ if (speedFromMaster>0.0f) {
+ if (enableSimula==0) {
tractorSpeed_MtS_timed = speedFromMaster + percento;
}
}
@@ -1293,12 +1451,12 @@
#endif
}
}
-
+
}
- if ((percento) > ((double) quincLIMplus/100.0f)){
+ if ((percento) > ((double) quincLIMplus/100.0f)) {
percento= (double)quincLIMplus/100.0f;
}
- if ((percento) < (((double)quincLIMminus*-1.0f)/100.0f)){
+ if ((percento) < (((double)quincLIMminus*-1.0f)/100.0f)) {
percento=((double)quincLIMminus*-1.0f)/100.0f;
}
#if defined(pcSerial)
@@ -1307,61 +1465,70 @@
#endif
#endif
}
-// ----------------------------------------
+// ----------------------------------------
#if defined(seedSensor)
void resetDelay(){
delaySeedCheck.reset();
delaySeedCheck.stop();
- #if defined(pcSerial)
- #if defined(checkLoop)
- pc.printf("27\n");
+ #if defined(pcSerial)
+ #if defined(checkLoop)
+ pc.printf("27\n");
+ #endif
#endif
- #endif
}
#endif
+
// ------------------------------------------------------------------------------------------------------------------------------------------------------------------
// MAIN SECTION
// ---------------------------------------------------------------------------------------------------------------------------------------------------------------
//*******************************************************************************
-int main(){
+int main()
+{
#if defined(pcSerial)
#if defined(resetCpu)
pc.printf("RESET\n");
#endif
#endif
-
- wd.Configure(2.0); //watchdog set at xx seconds
-
- stepSetting();
-
+
+ #if defined(canbusActive)
+ can1.attach(&leggiCAN, CAN::RxIrq);
+ #endif
+
+ wait(1.0f);
+ wait(1.0f);
+ wait(1.0f);
+
+ //stepSetting();
+
TBmotor_SW=1;
- //----- Initialization
- /* Initializing SPI bus. */
- // dev_spi(mosi,miso,sclk)
- // D11= PA7; D12= PA6; D13= PA5
- DevSPI dev_spi(D11, D12, D13);
-
- /* 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) {
+ TBmotorDirecti=TBforward; // reset stepper direction
+ //----- Initialization
+ /* Initializing SPI bus. */
+ // dev_spi(mosi,miso,sclk)
+ // D11= PA7; D12= PA6; D13= PA5
+ DevSPI dev_spi(D11, D12, D13);
+
+ /* 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);
+ }
- for (int a=0; a<5;a++){
+ /* 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);
+ wait(1.0f);
+ for (int a=0; a<5; a++) {
mediaSpeed[a]=0;
}
-
+
// DC reset ad set
int decima = 100;
wait_ms(200);
@@ -1372,13 +1539,13 @@
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;
+ SD_CurrentStart=(floor((SD_CurrentStart/4.0f)*decima)/decima)-0.01f;
wait_ms(100);
DC_prepare();
-
- speedTimer.start(); // speed pulse timer
+
+ speedTimer.start(); // speed pulse timer
intraPickTimer.start();
- speedTimeOut.start();
+ speedTimeOut.start();
speedFilter.start();
seedFilter.start();
TBfilter.start();
@@ -1387,15 +1554,23 @@
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);
+ pwmCheck.rise(&startDelay);
wait_ms(500);
-
+
+ #if defined(runnerTos)
+ thread.start(step_Reading);
+ #endif
- if (inProva==0){
+ if (inProva==0) {
tractorSpeedRead.rise(&tractorReadSpeed);
+
+
#if !defined(speedMaster)
quinconceIn.rise(&quincTrigon);
quinconceIn.fall(&quincTrigof);
@@ -1408,10 +1583,10 @@
#if defined(seedSensor)
seedCheck.fall(&seedSensorTask);
#endif
- }else{
- tftUpdate.attach(&videoUpdate,0.125f);
- }
-
+ } else {
+ tftUpdate.attach(&videoUpdate,0.125f);
+ }
+
aggiornaParametri();
SDmotorPWM.period_us(periodoSD); // frequency 1KHz pilotaggio motore DC
@@ -1430,11 +1605,16 @@
pc.printf("11f\n");
#endif
#endif
- TBticker.attach_us(&step_TBPulseOut,500.0f); // clock time are seconds and attach seed motor stepper controls
- TATicker.attach(&invertiLo,2.0f);
+ TBticker.attach(&step_TBPulseOut,0.0005f); // clock time are seconds and attach seed motor stepper controls
+ TATicker.attach(&invertiLo,3.0f);
#else
// definire il pin di clock che è PB_3
- motor->step_clock_mode_enable(StepperMotor::FWD);
+ motor->set_home();
+ motor->go_to(50);
+ motor->wait_while_active();
+ #if !defined(runner)
+ motor->step_clock_mode_enable(StepperMotor::FWD);
+ #endif
#if defined(pcSerial)
#if defined(loStop)
pc.printf("A3\n");
@@ -1444,92 +1624,92 @@
// attiva l'out per il controllo dello stepper in stepClockMode
DigitalOut TBmotorStepOut(PB_3); // PowerStep01 Step Input
#endif // end prova stepper
-
- #if defined(canbusActive)
- can1.attach(&leggiCAN, CAN::RxIrq);
- #endif
-
-
+
+ wd.Configure(2.0); //watchdog set at xx seconds
+
+
//**************************************************************************************************************
// MAIN LOOP
//**************************************************************************************************************
- while (true){
+ while (true) {
// ripetitore segnale di velocità. Il set a 1 è nella task ad interrupt
- if (tractorSpeedRead==0){speedClock=0;}
-
+ if (tractorSpeedRead==0) {
+ speedClock=0;
+ }
+
// inversione segnali ingressi
#if !defined(simulaBanco)
seedWheelZeroPinInput = !seedWheelZeroPinInputRev;
#else
- if ((prePosSD-500) >= SDsectorStep){
+ if ((prePosSD-500) >= SDsectorStep) {
seedWheelZeroPinInput=1;
}
- if ((prePosSD > 500)&&(prePosSD<580)){
+ if ((prePosSD > 500)&&(prePosSD<580)) {
seedWheelZeroPinInput=0;
}
- #endif
+ #endif
TBzeroPinInput = !TBzeroPinInputRev;
-
+
// se quinconce attivo ed unita' master invia segnale di sincro
#if defined(speedMaster)
- if (seedWheelZeroPinInput==1){
+ if (seedWheelZeroPinInput==1) {
quinconceOut=0;
}
- if (((double)(prePosSD-500) >= (SDsectorStep/((double)quincSector)))&&(quinconceOut=1)){
+ if (((double)(prePosSD-500) >= (SDsectorStep/((double)quincSector)))&&(quinconceOut=1)) {
quinconceOut=1;
}
- if (quinconceActive==1){
- if ((quinconceOut==1)&&(oldQuinconceOut==1)){
+ if (quinconceActive==1) {
+ if ((quinconceOut==1)&&(oldQuinconceOut==1)) {
posForQuinc=500;
oldQuinconceOut=0;
}
- if (((double)posForQuinc-500.0f)> (SDsectorStep-200)){
+ if (((double)posForQuinc-500.0f)> (SDsectorStep-200)) {
oldQuinconceOut=1;
}
- }
+ }
#endif
// simulazione velocita
- if (enableSimula==1){
+ if (enableSimula==1) {
double TMT = 0.0f;
- if (speedSimula > 0){
+ if (speedSimula > 0) {
TMT = (double)(speedSimula) * 100.0f /3600.0f;
pulseSpeedInterval = pulseDistance / TMT;
- }else{
+ } else {
pulseSpeedInterval = 10000.0f;
- }
- if (avviaSimula==1){
- if(oldSimulaSpeed!=pulseSpeedInterval){
+ }
+ if (avviaSimula==1) {
+ if(oldSimulaSpeed!=pulseSpeedInterval) {
spedSimclock.attach_us(&speedSimulationClock,pulseSpeedInterval);
oldSimulaSpeed=pulseSpeedInterval;
}
- }else{
+ } else {
oldSimulaSpeed=10000.0f;
spedSimclock.detach();
}
- }else{
+ } else {
spedSimclock.detach();
}
-
+
//*******************************************************
// determina se sono in bassa velocità per il controllo di TB
- if (speedOfSeedWheel<=minSeedSpeed){
- if (lowSpeedRequired==0){
+ if (speedOfSeedWheel<=minSeedSpeed) {
+ if (lowSpeedRequired==0) {
ritardaLowSpeed.reset();
ritardaLowSpeed.start();
}
lowSpeedRequired=1;
- }else{
- if (lowSpeedRequired==1){
+ } else {
+ if (lowSpeedRequired==1) {
lowSpeedRequired=0;
ritardaLowSpeed.reset();
ritardaLowSpeed.stop();
}
}
- if (ritardaLowSpeed.read_ms()> 2000){
+ if (ritardaLowSpeed.read_ms()> 2000) {
lowSpeed=1;
- }else{
+ } else {
lowSpeed=0;
}
@@ -1538,31 +1718,31 @@
// LOGICAL CONTROLS
//**************************************************************************************************************
//**************************************************************************************************************
-
- if (inProva==0){
- if ((startCycleSimulation==0)&&(enableSimula==0)){
+
+ if (inProva==0) {
+ if ((startCycleSimulation==0)&&(enableSimula==0)) {
runRequestBuf=1;//0;
- }else{
+ } else {
runRequestBuf=1;//0;
}
- if ((tractorSpeedRead==0)&&(speedFilter.read_ms()>3)){
+ 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){
+ if ((seedWheelZeroPinInput==0)&&(oldSeedWheelZeroPinInput==1)) {
+ if(seedFilter.read_ms()>=4) {
oldSeedWheelZeroPinInput=0;
SDzeroDebounced=0;
}
}
- if ((seedWheelZeroPinInput==1)&&(oldSeedWheelZeroPinInput==0)){
+ if ((seedWheelZeroPinInput==1)&&(oldSeedWheelZeroPinInput==0)) {
timeIntraPick = (double)intraPickTimer.read_ms();
prePosSD=500; // preposizionamento SD
intraPickTimer.reset();
rotationTimeOut.reset();
- seedFilter.reset();
+ seedFilter.reset();
sincroTimer.reset();
oldSeedWheelZeroPinInput=1;
quincTime.reset();
@@ -1571,34 +1751,34 @@
sincroQui=1;
SDwheelTimer.reset();
#if defined(speedMaster)
- if (quinconceActive==0){
+ if (quinconceActive==0) {
posForQuinc=500;
}
#endif
- if (quincCnt<10){
+ if (quincCnt<10) {
quincCnt++;
}
- if ((aspettaStart==0)&&(lowSpeed==1)){
+ 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){
+ if (tamburoStandard==0) {
+ fase1=TBdeltaStep;
+ } else {
+ if(speedForCorrection >= speedOfSeedWheel) {
fase1=TBdeltaStep;
- }else{
+ } else {
//fase1=(TBdeltaStep)-(((speedOfSeedWheel-speedForCorrection)/maxWorkSpeed)*(TBfaseStep));
fase1=(TBdeltaStep)-(((speedOfSeedWheel)/maxWorkSpeed)*(TBfaseStep));
}
- if (fase1 > limite){
+ 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)
//forzaFase=1;
}
- if ((fase1 > (limite -20.0f))&&(fase1<(limite +5.0f))){
+ 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;
}
@@ -1618,34 +1798,34 @@
pc.printf(" SPEED: %f \n",tractorSpeed_MtS_timed);
#endif
#endif
- if (timeIntraPick >= (memoIntraPick*2)){
- if ((aspettaStart==0)){
- if (firstStart==0){
+ if (timeIntraPick >= (memoIntraPick*2)) {
+ if ((aspettaStart==0)) {
+ if (firstStart==0) {
all_pickSignal=1;
}
}
}
memoIntraPick = timeIntraPick;
- if ((speedFromPick==1)&&(encoder==false)){
+ 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
+ #if defined(pcSerial)
+ #if defined(Qnca)
+ pc.printf("perim: %f pickN: %f sped: %f\n", seedPerimeter, pickNumber,speedOfSeedWheel);
#endif
+ #endif
}
- if (encoder==false){
+ if (encoder==false) {
pulseRised2=1;
}
#if defined(speedMaster)
- if ((tractorSpeed_MtS_timed==0.0f)){
- if ((firstStart==0)&&(simOk==0)&&(enableSimula==0)){
+ if ((tractorSpeed_MtS_timed==0.0f)) {
+ if ((firstStart==0)&&(simOk==0)&&(enableSimula==0)) {
all_noSpeedSen=1;
}
}
double oldLastPr = (double)oldLastPulseRead*1.5f;
- if((double)speedTimeOut.read_us()> (oldLastPr)){
- if ((firstStart==0)&&(simOk==0)&&(enableSimula==0)){
+ if((double)speedTimeOut.read_us()> (oldLastPr)) {
+ if ((firstStart==0)&&(simOk==0)&&(enableSimula==0)) {
all_speedError =1;
}
}
@@ -1653,14 +1833,16 @@
//*******************************************
// 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
+ if (speedOfSeedWheel < 0.002f) {
+ #if defined(pcSerial)
+ #if defined(checkLoopb)
+ pc.printf("forza\n");
+ #endif
+ #endif
speedOfSeedWheel=0.001f;
}
+ aggioVelocita();
+ /*
realGiroSD = seedPerimeter / speedOfSeedWheel;
tempoBecco = (realGiroSD/360.0f)*16000.0f;
frequenzaReale = fixedStepGiroSD/realGiroSD;
@@ -1675,120 +1857,145 @@
pc.printf("sep: %f sposw: %f rgsd: %f tpb: %f fr: %f spr: %f swr: %f tbfr: %f \n",seedPerimeter,speedOfSeedWheel,realGiroSD,tempoBecco,frequenzaReale,semiPeriodoReale,seedWheelRPM,TBfrequency);
#endif
#endif
+ */
}
// ----------------------------------------
-// check SD fase
- if ((prePosSD >= fase)||(forzaFase==1)){//&&(prePosSD < (fase +30))){
+// check SD fase
+ if ((prePosSD >= fase)||(forzaFase==1)) { //&&(prePosSD < (fase +30))){
forzaFase=0;
- if (trigRepos==1){
+ if (trigRepos==1) {
SDactualPosition=0;
- if ((countCicli<30)&&(trigCicli==0)){
+ if ((countCicli<30)&&(trigCicli==0)) {
countCicli++;
trigCicli=1;
}
- if(countCicli>=cicliAspettaStart){
+ if(countCicli>=cicliAspettaStart) {
aspettaStart=0;
- #if defined(pcSerial)
- #if defined(checkLoop)
- pc.printf("NoAspetta\n");
- #endif
- #endif
+#if defined(pcSerial)
+#if defined(checkLoop)
+ pc.printf("NoAspetta\n");
+#endif
+#endif
}
- if ((lowSpeed==0)&&(aspettaStart==0)&&(lockStart==0)){
+ if ((lowSpeed==0)&&(aspettaStart==0)&&(lockStart==0)) {
syncroCheck=1;
beccoPronto=0;
- #if defined(pcSerial)
- #if defined(checkLoop)
- pc.printf("BeccoNo\n");
- #endif
- #endif
+#if defined(pcSerial)
+#if defined(checkLoop)
+ pc.printf("BeccoNo\n");
+#endif
+#endif
}
- if (trigTB==0){
+ if (trigTB==0) {
inhibit=1;
trigSD=1;
- }else{
+ } else {
inhibit=0;
trigTB=0;
trigSD=0;
}
trigRepos=0;
}
- }else{
+ } else {
trigCicli=0;
}
-// ----------------------------------------
+// ----------------------------------------
// filtra il segnale del tamburo per lo stop in fase del tamburo stesso
- if (TBzeroPinInput==0){if (TBfilter.read_ms()>=5){oldTBzeroPinInput=0;}}
- if ((TBzeroPinInput==1)&&(oldTBzeroPinInput==0)){
+ if (TBzeroPinInput==0) {
+ if (TBfilter.read_ms()>=2) {
+ oldTBzeroPinInput=0;
+ }
+ }
+ if ((TBzeroPinInput==1)&&(oldTBzeroPinInput==0)) {
oldTBzeroPinInput=1;
- if (loadDaCanInCorso==0){
+ if (loadDaCanInCorso==0) {
stopCicloTB=1;
startCicloTB=0;
}
TBfilter.reset();
TBzeroCyclePulse=1;
- TBactualPosition=0;
- if (cntTbError>0){
+ #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
+ if (cntTbError>0) {
cntCellsCorrect++;
}
- if (cntCellsCorrect>3){
+ if (cntCellsCorrect>3) {
cntTbError=0;
cntCellsCorrect=0;
}
// conteggio celle erogate
- if (cellsCounterLow < 0xFF){
+ if (cellsCounterLow < 0xFF) {
cellsCounterLow++;
- }else{
+ } else {
cellsCounterHig++;
cellsCounterLow=0;
}
// ciclo conteggio celle per carico manuale
- if (loadDaCanInCorso==1){
+ if (loadDaCanInCorso==1) {
cntCellsForLoad++;
- if (cntCellsForLoad >= 5){
- stopCicloTB=1;
+ if (cntCellsForLoad >= 5) {
+ stopCicloTB=1;
cntCellsForLoad=0;
}
- }else{
+ } else {
cntCellsForLoad=0;
}
// inibizione controllo di sincro per fuori fase
- if (trigSD==0){
+ if (trigSD==0) {
inhibit=1;
trigTB=1;
- }else{
+ } else {
inhibit=0;
trigTB=0;
trigSD=0;
}
// conta le celle indietro per sbloccare il tamburo
- if ((TBmotorDirecti==TBreverse)&&(erroreTamburo==1)){
+ if ((TBmotorDirecti==TBreverse)&&(erroreTamburo==1)) {
cntCellsForReload++;
- if (cntCellsForReload >= cellsCountSet){
+ if (cntCellsForReload >= cellsCountSet) {
TBmotorDirecti=TBforward; // rotazione normale
- motor->step_clock_mode_enable(StepperMotor::FWD);
+ #if defined(runner)
+ motor->run(StepperMotor::FWD);
+ #else
+ motor->step_clock_mode_enable(StepperMotor::FWD);
+ #endif
erroreTamburo=0;
cntCellsCorrect=0;
- }
+ }
}
#if defined(seedSensor)
resetDelay();
delaySeedCheck.start();
#endif
}
- if ((double)TBactualPosition > ((TBgiroStep/cellsNumber)*1.2f)&&(erroreTamburo==0)){
- if (firstStart==0){
- if (cntTbError>2){
+ if ((double)TBactualPosition > ((TBgiroStep/cellsNumber)*1.8f)&&(erroreTamburo==0)) {
+ if (firstStart==0) {
+ if (cntTbError>2) {
all_cellSignal=1;
#if defined(seedSensor)
resetDelay();
#endif
}
}
- if (erroreTamburo==0){
+ if (erroreTamburo==0) {
erroreTamburo=1;
TBmotorDirecti=TBreverse; // rotazione inversa
- motor->step_clock_mode_enable(StepperMotor::BWD);
+ #if defined(runner)
+ motor->run(StepperMotor::BWD);
+ #else
+ motor->step_clock_mode_enable(StepperMotor::BWD);
+ #endif
cntCellsForReload=0;
cntTbError++;
#if defined(seedSensor)
@@ -1796,8 +2003,8 @@
#endif
}
}
- if (((double)TBactualPosition > ((TBgiroStep/cellsNumber)*3.0f))||(cntTbError>4)){
- if (firstStart==0){
+ if (((double)TBactualPosition > ((TBgiroStep/cellsNumber)*3.0f))||(cntTbError>4)) {
+ if (firstStart==0) {
all_noStepRota=1;
#if defined(seedSensor)
resetDelay();
@@ -1805,66 +2012,68 @@
}
cntTbError=0;
}
-// ----------------------------------------
+// ----------------------------------------
// read and manage joystick
- if (loadDaCan==1){
- if (tractorSpeed_MtS_timed==0.0f){
- #if defined(pcSerial)
- #if defined(checkLoop)
- pc.printf("daCAN\n");
- #endif
+ if (loadDaCan==1) {
+ 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
- //
+
+ //***************************************************************************************************
+ // pulseRised define the event of speed wheel pulse occurs
+ //
//double maxInterval = pulseDistance/minWorkSpeed;
//double minIntervalPulse = pulseDistance/maxWorkSpeed;
- if (pulseRised==1){
- if (enableSpeed<10){enableSpeed++;}
+ 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)){
- if((quincCnt<3)||(speedFromMaster==0.0f)||(enableSimula==1)){
+ if(enableSpeed>=2) {
+ if ((pulseSpeedInterval>=0.0f)) { //minIntervalPulse)&&(pulseSpeedInterval<maxInterval)){
+ if((quincCnt<3)||(speedFromMaster==0.0f)||(enableSimula==1)) {
tractorSpeed_MtS_timed = ((pulseDistance / pulseSpeedInterval)); // tractor speed (unit= Mt/S) from pulse time interval
}
- if (checkSDrotation==0){
+ if (checkSDrotation==0) {
checkSDrotation=1;
SDwheelTimer.start();
}
}
}
speedTimeOut.reset();
- }else{
+ } else {
double oldLastPr = (double)oldLastPulseRead*1.7f;
- if((double)speedTimeOut.read_us()> (oldLastPr)){
+ if((double)speedTimeOut.read_us()> (oldLastPr)) {
tractorSpeed_MtS_timed = 0.0f;
- #if defined(seedSensor)
- resetDelay();
- #endif
+#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();
+
+#if defined(seedSensor)
+ if (seedSensorEnable==true) {
+ if (delaySeedCheck.read_ms()>100) {
+ if (seedSee==0) {
+ all_noSeedOnCe=1;
}
+ resetDelay();
}
- #endif
+ }
+#endif
// esegue il controllo di velocità minima
/*if ((double)speedTimer.read_ms()>=maxInterval){
tractorSpeed_MtS_timed = 0.0f;
@@ -1874,305 +2083,348 @@
/*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;}}
- if ((tractorSpeed_MtS_timed>0.01f)){
+ //***************************************************************************************************************
+ // cycle logic control section
+ //***************************************************************************************************************
+ if (enableSimula==1) {
+ if(simOk==0) {
+ tractorSpeed_MtS_timed=0.0f;
+ }
+ }
+ if ((tractorSpeed_MtS_timed>0.01f)) {
+ #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*25.5f))*1000.0f; // tempo tra due impulsi dai becchi in millisecondi ( circa 157mS a 4,5Km/h)
#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);
-
+ #if defined(Qnce)
+ pc.printf("tempoGiroSD: %f SDreductionRatio: %f tempoBecchi:%f\n",tempoGiroSD,SDreductionRatio,tempoTraBecchi_mS);
#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*25.5f))*1000.0f; // tempo tra due impulsi dai becchi in millisecondi ( circa 157mS a 4,5Km/h)
+ #if !defined(speedMaster)
double pippo=0.0f;
- #if !defined(speedMaster)
- pippo = seedPerimeter / speedFromMaster;
- #endif
+ pippo = seedPerimeter / speedFromMaster;
tempoBecchiPerQuinc = (pippo / pickNumber)*1000.0f;
+ #endif
+ }
+ //*******************************************
+ // segue calcolo duty cycle comando motore DC per allinearsi con la velocità del trattore
+ double dutyTeorico = 0.00;
+ if ((tractorSpeed_MtS_timed>0.0)&&(tractorSpeed_MtS_timed<tabSpeed[0])) {
+ dutyTeorico = tabComan[0];
+ }
+ for (int ii = 0; ii<16; ii++) {
+ if ((tractorSpeed_MtS_timed>=tabSpeed[ii])&&(tractorSpeed_MtS_timed<tabSpeed[ii+1])) {
+ dutyTeorico = tabComan[ii+1];
}
- //*******************************************
- // segue calcolo duty cycle comando motore DC per allinearsi con la velocità del trattore
- double dutyTeorico = 0.00;
- if ((tractorSpeed_MtS_timed>0.0)&&(tractorSpeed_MtS_timed<tabSpeed[0])){dutyTeorico = tabComan[0];}
- for (int ii = 0;ii<16;ii++){
- if ((tractorSpeed_MtS_timed>=tabSpeed[ii])&&(tractorSpeed_MtS_timed<tabSpeed[ii+1])){
- dutyTeorico = tabComan[ii+1];
- }
- }
- if (tractorSpeed_MtS_timed > tabSpeed[16]){dutyTeorico=tractorSpeed_MtS_timed/maxWorkSpeed;}
- #if !defined(speedMaster)
- quinCalc();
+ }
+ if (tractorSpeed_MtS_timed > tabSpeed[16]) {
+ dutyTeorico=tractorSpeed_MtS_timed/maxWorkSpeed;
+ }
+ #if !defined(speedMaster)
+ quinCalc();
+ #endif
+ #if defined(pcSerial)
+ #if defined(Qncd)
+ pc.printf("enableSpeed: %d pulseRised2: %d quincCnt: %d\n",enableSpeed,pulseRised2,quincCnt);
#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)
+ #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)
}
- 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 ((correzione < 0.0f)&&(dutyTeorico+correzione<0.0f)){correzione = -1.0f*dutyTeorico;}
- }
- pulseRised1=0;
- pulseRised2=0;
+ } 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(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);
+ #if defined(Qnce)
+ pc.printf("timeHole: %d TempoBecchi: %f erroreTempo: %f\n",memoTimeHole,tempoTraBecchi_mS,erroreTempo);
#endif
#endif
}
- // introduce il controllo di corrente
- if (currentCheckEnable==true){
- if (incrementCurrent){
- boostDcOut +=0.005f;
- }
- if (reduceCurrent){
- boostDcOut -=0.005f;
+ 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 (boostDcOut >= 0.2f){
- boostDcOut=0.2f;
- all_genericals=1;
+ if ((correzione < 0.0f)&&(dutyTeorico+correzione<0.0f)) {
+ correzione = -1.0f*dutyTeorico;
}
- if (boostDcOut <=-0.2f){
- boostDcOut=-0.2f;
- all_genericals=1;
- }
- correzione += boostDcOut;
}
- DC_brake=0;
- DC_forward=1;
- DC_prepare();
+ 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
+ }
+ // introduce il controllo di corrente
+ if (currentCheckEnable==true) {
+ if (incrementCurrent) {
+ boostDcOut +=0.005f;
+ }
+ if (reduceCurrent) {
+ boostDcOut -=0.005f;
+ }
+ if (boostDcOut >= 0.2f) {
+ boostDcOut=0.2f;
+ all_genericals=1;
+ }
+ if (boostDcOut <=-0.2f) {
+ boostDcOut=-0.2f;
+ all_genericals=1;
+ }
+ correzione += boostDcOut;
+ }
+ 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;
- }
+ // 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{
+ if((quincCnt>=3)) {
+ if (correzioneAttiva==1) {
+ dcActualDuty = dutyTeorico + correzione;
+ } else {
dcActualDuty = dutyTeorico;
}
- if (dcActualDuty <=0.0f){dcActualDuty=0.05f;}
- if (dcActualDuty > 0.95f){dcActualDuty = 0.95f;}//dcMaxSpeed;}
- if (olddcActualDuty!=dcActualDuty){
- SDmotorPWM.write(1.0f-dcActualDuty);
- olddcActualDuty=dcActualDuty;
+ } else {
+ dcActualDuty = dutyTeorico;
+ }
+ if (dcActualDuty <=0.0f) {
+ dcActualDuty=0.05f;
+ }
+ if (dcActualDuty > 0.95f) {
+ dcActualDuty = 0.95f;
+ }
+ if (olddcActualDuty!=dcActualDuty) {
+ SDmotorPWM.write(1.0f-dcActualDuty);
+ olddcActualDuty=dcActualDuty;
+ }
+ // allarme
+ if (SDwheelTimer.read_ms()>4000) {
+ if (firstStart==0) {
+ all_noDcRotati=1;
}
- // allarme
- if (SDwheelTimer.read_ms()>4000){
- if (firstStart==0){
- all_noDcRotati=1;
- }
- #if defined(pcSerial)
- #if defined(VediAllarmi)
- pc.printf("allarme no DC rotation");
- #endif
+ #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;
+ //***************************************************************************************************************
+ // CONTROLLA TAMBURO
+ //***************************************************************************************************************
+ if(lowSpeed==0) {
+ if (syncroCheck==1) {
+ syncroCheck=0;
+ lockStart=1;
+ periodo = TBperiod;
+ #if !defined(runner)
motor->step_clock_mode_enable(StepperMotor::FWD);
- 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;
+ #endif
+ if (aspettaStart==0) {
#if defined(pcSerial)
#if defined(checkLoop)
- pc.printf("AspettaSI\n");
+ pc.printf("da sincro\n");
#endif
#endif
- if (TBzeroCyclePulse==1){
- #if defined(pcSerial)
- #if defined(checkLoop)
- pc.printf("15f\n");
- #endif
- #endif
- TBticker.detach();
- #if defined(pcSerial)
- #if defined(loStop)
- pc.printf("A4\n");
- #endif
- #endif
- motor->soft_hiz();
- }
+ cambiaTB(periodo);
}
- }else{ // fine ciclo fuori da low speed
+ }
+ // controllo di stop
+ double memoIntraP = (double)memoIntraPick*1.8f;
+ if ((double)rotationTimeOut.read_ms()> (memoIntraP)) {
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/2.0f)+((speedOfSeedWheel/maxWorkSpeed)*ritardoMassimo)))); //
- if (tempoDiSincro <= 1){tempoDiSincro=1;}
- if ((sincroTimer.read_ms()>= tempoDiSincro)){
- if (tractorSpeed_MtS_timed >= minWorkSpeed){
- startCicloTB=1;
- #if defined(pcSerial)
- #if defined(checkLoop)
- pc.printf("startTB\n");
- #endif
- #endif
- }
- beccoPronto=0;
- }
- }else{
- // tamburo per zucca
- if (speedOfSeedWheel >= minWorkSpeed){startCicloTB=1;}
- beccoPronto=0;
- }
- }
+ aspettaStart=1;
+ countCicli=0;
#if defined(pcSerial)
#if defined(checkLoop)
- pc.printf("lowSpeed\n");
+ pc.printf("AspettaSI\n");
#endif
#endif
- ciclaTB();
- }
- //*************************************************************
- }else{ // fine ciclo con velocita maggiore di 0
- 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");
+ if (TBzeroCyclePulse==1) {
+ #if defined(pcSerial)
+ #if defined(checkLoop)
+ pc.printf("15f\n");
+ #endif
+ #endif
+ #if !defined(runner)
+ TBticker.detach();
+ #endif
+ #if defined(pcSerial)
+ #if defined(loStop)
+ pc.printf("A4\n");
+ #endif
#endif
- #endif
- speedOfSeedWheel=0.0f;
- cycleStopRequest=0;
- DC_brake=1;
- DC_prepare();
- #if defined(pcSerial)
- #if defined(checkLoop)
- pc.printf("17h\n");
- #endif
+ motor->soft_hiz();
+ }
+ }
+ } 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;
+ #if defined(pcSerial)
+ #if defined(checkLoop)
+ pc.printf("startTB\n");
+ #endif
+ #endif
+ }
+ beccoPronto=0;
+ }
+ } else {
+ // tamburo per zucca
+ if (speedOfSeedWheel >= minWorkSpeed) {
+ startCicloTB=1;
+ }
+ beccoPronto=0;
+ }
+ }
+ #if defined(pcSerial)
+ #if defined(checkLoop)
+ pc.printf("lowSpeed\n");
#endif
+ #endif
+ ciclaTB();
+ }
+ //*************************************************************
+ } else { // fine ciclo con velocita maggiore di 0
+ 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();
- #if defined(pcSerial)
- #if defined(loStop)
- pc.printf("A5\n");
- #endif
+ #endif
+ #if defined(pcSerial)
+ #if defined(loStop)
+ pc.printf("A5\n");
#endif
- motor->soft_hiz();
- pntMedia=0;
- #if defined(pcSerial)
- #if defined(stopSignal)
- pc.printf("stop\n");
- #endif
+ #endif
+ motor->soft_hiz();
+ 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