Francesco Pistone
/
FORIGO_Modula_V6_R2_DE_2019
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.
Diff: main.cpp
- Revision:
- 3:c0f11ca4df02
- Parent:
- 2:d9c7430ae953
- Child:
- 4:d32258ec411f
--- a/main.cpp Fri Jun 15 07:32:35 2018 +0000
+++ b/main.cpp Fri Jun 15 17:10:37 2018 +0000
@@ -91,6 +91,7 @@
// TASK SECTION
// ------------------------------------------------------------------------------------------------------------------------------------------------------------------
//************************************************************************
+//************************************************************************
// rise of seed speed 25 pulse sensor
void sd25Fall(){
timeHole=metalTimer.read_ms();
@@ -404,9 +405,9 @@
val1[3]=0;
val1[3]=val1[3]+(tractorSpeedRead*0x01);
- val1[3]=val1[3]+(TBzeroPinInputRev*0x02);
+ val1[3]=val1[3]+(TBzeroPinInput*0x02);
val1[3]=val1[3]+(ElementPosition*0x04);
- val1[3]=val1[3]+(seedWheelZeroPinInputRev*0x08);
+ val1[3]=val1[3]+(seedWheelZeroPinInput*0x08);
#if defined(simulaPerCan)
if (buttonUser==0){
val1[1]=0x02;
@@ -440,9 +441,11 @@
#if defined(canbusActive)
if(can1.read(rxMsg)) {
if (firstStart==1){
+ /*
#if defined(speedMaster)
sincUpdate.attach(&upDateSincro,0.01f);
#endif
+ */
canUpdate.attach(&upDateSpeed,0.11f);
firstStart=0;
}
@@ -741,13 +744,12 @@
percento=1.0f;
scostamento = (double)quincTime.read_ms();
scostamento2 = oldQuincTimeSD;
- double pippo = (tractorSpeed_MtS_timed/1.25f)*0.05f;
+ double pippo = 0.025f-((tractorSpeed_MtS_timed/1.25f)*0.02f);
double halfPippo= tempoTraBecchi_mS/2.0f;
- if ((scostamento > 5.0f)&&(scostamento<halfPippo)){
- percento = 1.0f-pippo;
- }
- if ((scostamento2 > 5.0f)&&(scostamento2<halfPippo)){
- percento = 1.0f+pippo;
+ if ((scostamento2 > 1.0f)&&(scostamento2<halfPippo)){
+ percento = 1.0f+(pippo);///3.0f);
+ }else if ((scostamento > 1.0f)&&(scostamento<halfPippo)){
+ percento = 1.0f-(pippo);///1.0f);
}
if (abs(percento) > 1.15f){percento=1.15f;}
if (abs(percento) < 0.80f){percento=0.80f;}
@@ -755,16 +757,15 @@
#if defined(Qnc)
pc.printf(" s1: %f", scostamento);
pc.printf(" s2: %f", scostamento2);
- pc.printf(" er: %f", quincError);
pc.printf(" pc: %f", percento);
- pc.printf(" cr: %f\n",correzione);
+ pc.printf(" en: %d\n",quinconceActive);
#endif
#endif
}
}
// ------------------------------------------------------------------------------------------------------------------------------------------------------------------
// MAIN SECTION
-// ------------------------------------------------------------------------------------------------------------------------------------------------------------------
+// ---------------------------------------------------------------------------------------------------------------------------------------------------------------
int main()
{
@@ -891,15 +892,15 @@
if (tractorSpeedRead==0){speedClock=0;}
// inversione segnali ingressi
+ seedWheelZeroPinInput = !seedWheelZeroPinInputRev;
TBzeroPinInput = !TBzeroPinInputRev;
- seedWheelZeroPinInput = !seedWheelZeroPinInputRev;
// se quinconce attivo ed unita' master invia segnale di sincro
#if defined(speedMaster)
if (seedWheelZeroPinInput==1){
quinconceOut=1;
}
- if ((double)SDactualPosition >= (SDsectorStep/2.0f)){
+ if ((double)(prePosSD-500) >= (SDsectorStep/2.0f)){
quinconceOut=0;
}
#endif
@@ -1299,14 +1300,26 @@
if ((pulseRised1==1)&&(enableSpeed>3)&&(pulseRised2==1)){
double erroreTempo = 0.0f;
if(speedFromPick==1){
- double giorgio = timeIntraPick/percento;
- //erroreTempo = (double)timeIntraPick - tempoTraBecchi_mS; // errore tra il tempo previsto ed il tempo reale ( >0 se sto andando più piano del previsto)
- erroreTempo = giorgio - tempoTraBecchi_mS; // errore tra il tempo previsto ed il tempo reale ( >0 se sto andando più piano del previsto)
+ #if !defined(speedMaster)
+ double giorgio = timeIntraPick/percento;
+ //erroreTempo = (double)timeIntraPick - tempoTraBecchi_mS; // errore tra il tempo previsto ed il tempo reale ( >0 se sto andando più piano del previsto)
+ if (giorgio >0.0f){
+ erroreTempo = giorgio - tempoTraBecchi_mS; // errore tra il tempo previsto ed il tempo reale ( >0 se sto andando più piano del previsto)
+ }else{
+ erroreTempo = (double)timeIntraPick - tempoTraBecchi_mS;
+ }
+ #else
+ erroreTempo = (double)timeIntraPick - tempoTraBecchi_mS;
+ #endif
}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 k1=errorePercentuale*0.265f;
+ #if defined(speedMaster)
+ double k1=errorePercentuale*185.265f;
+ #else
+ double k1=errorePercentuale*0.265f;
+ #endif
double k3=0.0f;
double k4=0.0f;
double k5=0.0f;
@@ -1315,17 +1328,23 @@
k4=0.207;
k5=0.310;
}else{
- k3=0.1075f;
- k4=0.211f;
- k5=0.315f;
+ k3=0.03f;
+ k4=0.06f;
+ k5=0.20f;
}
- if ((errorePercentuale > 25.0f)||(errorePercentuale < -25.0f)){
+ double L1 = 0.001f;
+ double L_1=-0.001;
+ double L2 = 0.007f;
+ double L_2=-0.007f;
+ double L3 = 0.01f;
+ double L_3=-0.01f;
+ if ((errorePercentuale > L3)||(errorePercentuale < L_3)){
k1=errorePercentuale*k5;
}
- if (((errorePercentuale >= 10.0f)&&(errorePercentuale<=25.0f))||((errorePercentuale <= -10.0f)&&(errorePercentuale>=-25.0f))){
+ if (((errorePercentuale >= L2)&&(errorePercentuale<=L3))||((errorePercentuale <= L_2)&&(errorePercentuale>=L_3))){
k1=errorePercentuale*k4;
}
- if ((errorePercentuale < 10.0f)||(errorePercentuale > -10.0f)){
+ if (((errorePercentuale < L2)&&(errorePercentuale>L1))||((errorePercentuale > L_2)&&(errorePercentuale<L_1))){
k1=errorePercentuale*k3;
}
double memoCorrezione = (dutyTeorico *k1);