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);