Diff: main.cpp

Revision:

2:d9c7430ae953

Parent:

1:e88bf5011af6

Child:

3:c0f11ca4df02

diff -r e88bf5011af6 -r d9c7430ae953 main.cpp
--- a/main.cpp	Fri Jun 15 07:01:13 2018 +0000
+++ b/main.cpp	Fri Jun 15 07:32:35 2018 +0000
@@ -621,8 +621,6 @@
                 masterSinc = masterSinc + ((uint32_t) rxMsg.data[1] * 0x0000FF00);              
                 masterSinc = masterSinc + ((uint32_t) rxMsg.data[2] * 0x000000FF);              
                 masterSinc = masterSinc + ((uint32_t) rxMsg.data[3]);
-                quincVerify=1;
-                //pc.printf("pippo");
             }
         } 
     #endif
@@ -709,153 +707,60 @@
     }
 }
 //*******************************************************
-void quinCalc(double parametro){
-    //double riferimento=tempoTraBecchi_mS/parametro;
+void quinCalc(){
     // riceve l'impulso di sincro dal master e fa partire il timer di verifica dell'errore
-    if (quinconceActive==0){
+    #if !defined(mezzo)
         if ((quincClock==true)&&(oldQuincIn==0)){
             oldQuincIn=1;
             oldQuincTimeSD = (double) quincTimeSD.read_ms();
-            if (trigSlave==false){
-                quincInibit=true;
-                trigDaMaster=true;
-            }else{
-                quincInibit=false;
-                trigDaMaster=false;
-                trigSlave=false;
-            }
             if (quincStart==0){
                 quincTime.reset();
                 quincStart=1;
             }
         }
-        if( quincClock==false){
+        if(quincClock==false){
             oldQuincIn=0;
         }
-    }else{
-        if ((quincClock==false)&&(oldQuincIn==0)){
+    #else
+        if ((((quinconceActive==0)&&(quincClock==true))||((quinconceActive==1)&&(quincClock==false)))&&(oldQuincIn==0)){
             oldQuincIn=1;
             oldQuincTimeSD = (double) quincTimeSD.read_ms();
-            if (trigSlave==false){
-                quincInibit=true;
-                trigDaMaster=true;
-            }else{
-                quincInibit=false;
-                trigDaMaster=false;
-                trigSlave=false;
-            }
             if (quincStart==0){
                 quincTime.reset();
                 quincStart=1;
             }
         }
-        if( quincClock==true){
+        if((((quincClock==false)&&(quinconceActive==0))||((quincClock==true)&&(quinconceActive==1)))){
             oldQuincIn=0;
         }
-    }
+    #endif
     // riceve l'impulso di passaggio del becco locale e determina il valore dell'errore
     // controllo di linea e di quinconcio
-    if ((quinconceActive==0)||(quinconceActive==1)){
-        if ((quincStart==1)&&(SDzeroDebounced==1)){
-            if (quincMemoEnable==false){
-                memoDuty=correzione;
-                oldCorrezione=correzione;
-                //quincMemoEnable=true;
-            }
-            quincStart=0;
-            percento=1.0f;
-            scostamento = (double)quincTime.read_ms();
-            scostamento2 = oldQuincTimeSD;
-            /*if ((scostamento <= scostamento2)&&(scostamento>10.0f)&&(scostamento <200.0f)){
-                quincError = scostamento;
-                percento = 1.0f + (abs(quincError)/100.0f);
-            }
-            if ((scostamento > scostamento2)&&(scostamento2>10.0f)&&(scostamento2 <200.0f)){
-                quincError = scostamento2;
-                percento = 1.0f - (abs(quincError)/100.0f);
-            }*/
-            double pippo = (tractorSpeed_MtS_timed/1.25f)*0.05f;
-            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 (abs(percento) > 1.15f){percento=1.15f;}
-            if (abs(percento) < 0.80f){percento=0.80f;}
-            #if defined(pcSerial)
-                #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);
-                #endif
+    if ((quincStart==1)&&(SDzeroDebounced==1)){
+        quincStart=0;
+        percento=1.0f;
+        scostamento = (double)quincTime.read_ms();
+        scostamento2 = oldQuincTimeSD;
+        double pippo = (tractorSpeed_MtS_timed/1.25f)*0.05f;
+        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 (abs(percento) > 1.15f){percento=1.15f;}
+        if (abs(percento) < 0.80f){percento=0.80f;}
+        #if defined(pcSerial)
+            #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);
             #endif
-        }
-        /*if ((abs(percento) != 1.0f)&&(abs(percento) > 0.0f)){
-            correzione = memoDuty* abs(percento);
-            correggiSD=true;
-            cntQuinc=0;
-        }else{
-            correzione = memoDuty;
-            correggiSD=false;
-            if (cntQuinc>=2){
-                quincMemoEnable=false;
-            }
-        }*/
+        #endif
     }
-    /*
-    // gestione con confronto encoder 
-    // serve l'encoder fisico sugli elementi slave perchè altrimenti la ruota cambia velocità ma non cambia
-    // il clock dell'encoder virtuale e quindi l'errore non può essere rilevato correttamente
-    if ((quinconceActive==0)&&(quincEnable==1)){
-        // da encoder master
-        if (quincVerify==1){
-            quincVerify=0;
-            quincStart=0;
-            memoDuty=dcActualDuty;
-            if (quincInibit==false){
-                percento=0.0f;
-                quincError = 0.0f;
-                if (masterSinc > prePosSD){
-                    quincError = masterSinc - prePosSD;
-                    percento = 1.0f + ((abs(quincError)/SDsectorStep));
-                }
-                if (masterSinc < prePosSD){
-                    quincError = prePosSD -masterSinc;
-                    percento = 1.0f - ((abs(quincError)/SDsectorStep));
-                }
-                // se quinconce richiesto deve andare al valore di riferimento
-                if (abs(percento) > 1.55f){percento=1.55f;}
-                if (abs(percento) < 0.40f){percento=0.40f;}
-                if (abs(percento) >0.0f){
-                    if (abs(percento)>1.0f){
-                        correzione=correzione+(abs(percento)*0.01f);
-                    }else{
-                        correzione=correzione-(abs(percento)*0.01f);
-                    }
-                    //dcActualDuty = memoDuty * abs(percento);
-                }else{
-                    correzione=correzione;
-                    //dcActualDuty = memoDuty;
-                }
-            }
-            #if defined(pcSerial)
-                #if defined(Qnc)
-                    pc.printf(" trigMaster: %d", trigDaMaster);
-                    pc.printf(" trigSlave: %d", trigSlave);
-                    pc.printf(" quincinibit: %d", quincInibit);
-                    pc.printf(" masterSinc: %d", masterSinc);
-                    pc.printf(" prePosSD: %d",prePosSD);
-                    pc.printf(" percento: %f",percento);
-                    pc.printf(" correzione: %f\n",correzione);
-                #endif
-            #endif
-        }
-    }
-    */
 }
 // ------------------------------------------------------------------------------------------------------------------------------------------------------------------
 // MAIN SECTION
@@ -1062,7 +967,6 @@
             // filtra il segnale dei becchi per misurare il tempo di intervallo tra loro
             // ----------------------------------------
             if (seedWheelZeroPinInput==0){
-                //trigSlave=false;
                 if(seedFilter.read_ms()>=4){
                     oldSeedWheelZeroPinInput=0;
                     SDzeroDebounced=0;
@@ -1078,14 +982,6 @@
                 oldSeedWheelZeroPinInput=1;
                 quincTimeSD.reset();
                 SDzeroDebounced=1;
-                if  (trigDaMaster==false){
-                    quincInibit=true;
-                    trigSlave=true;
-                }else{
-                    quincInibit=false;
-                    trigSlave=false;
-                    trigDaMaster=false;
-                }
                 SDwheelTimer.reset();
                 if (quincCnt<10){
                     quincCnt++;
@@ -1395,22 +1291,11 @@
                         }
                     }
                     if (tractorSpeed_MtS_timed > tabSpeed[16]){dutyTeorico=tractorSpeed_MtS_timed/1.25f;}
-//                    if ((pulseRised1==1)&&(enableSpeed>3)&&(pulseRised2==1)&&(quincMemoEnable==false)){
-                        #if !defined(speedMaster)
-                            #if defined(mezzo)
-                                if (quincEnable==1){
-                                    if (quinconceActive==1){
-                                        quinCalc(2.0f);
-                                    }else{
-                                        quinCalc(1.0f);
-                                    }
-                                }else{
-                                    quincMemoEnable=false;
-                                }
-                            #else
-                                quinCalc(1.0f);
-                            #endif
-                        #endif
+                    #if !defined(speedMaster)
+                        if (quincEnable==1){
+                            quinCalc();
+                        }
+                    #endif
                     if ((pulseRised1==1)&&(enableSpeed>3)&&(pulseRised2==1)){
                         double erroreTempo = 0.0f;
                         if(speedFromPick==1){
@@ -1474,25 +1359,6 @@
                             pc.printf(" DUTY PRE FINALE: %f",dcActualDuty);
                         #endif
                     #endif
-                    // prova a mantenere il quinconce quando attivo
-                    //dcActualDuty=dcActualDuty;
-                    /*
-                    #if !defined(speedMaster)
-                        #if defined(mezzo)
-                            if (quincEnable==1){
-                                if (quinconceActive==1){
-                                    quinCalc(2.0f);
-                                }else{
-                                    quinCalc(1.0f);
-                                }
-                            }else{
-                                quincMemoEnable=false;
-                            }
-                        #else
-                            quinCalc(1.0f);
-                        #endif
-                    #endif
-                    */
                     DC_brake=0;
                     DC_forward=1;
                     DC_prepare();