tr

Dependencies:   mbed CANMsg

Revision:
9:503e2aba047c
Parent:
8:0e643ea7834f
Child:
10:d85ed006056e
--- a/main.cpp	Tue Jul 24 08:22:50 2018 +0000
+++ b/main.cpp	Tue Oct 02 07:05:52 2018 +0000
@@ -168,7 +168,11 @@
 void step_TBPulseOut(){
     TBmotorStepOut=!TBmotorStepOut;
     if (TBmotorStepOut==0){
-        TBactualPosition++;
+        if (TBmotorDirecti==1){
+            TBactualPosition++;
+        }else{
+            TBactualPosition--;
+        }
     }
 }
 //*******************************************************
@@ -315,7 +319,9 @@
 void stepSetting(){
     // Stepper driver init and set
     TBmotorRst=1;                               // reset stepper driver
-    TBmotorDirecti=1;                           // reset stepper direction
+    if (tractorSpeed_MtS_timed==0.0f){
+        TBmotorDirecti=1;                           // reset stepper direction
+    }
     // M1   M2  M3  RESOLUTION
     // 0    0   0       1/2
     // 1    0   0       1/8
@@ -1356,12 +1362,21 @@
                 TBfilter.reset();
                 TBzeroCyclePulse=1;
                 TBactualPosition=0;
+                if (cntTbError>0){
+                    cntCellsCorrect++;
+                }
+                if (cntCellsCorrect>3){
+                    cntTbError=0;
+                    cntCellsCorrect=0;
+                }
+                // conteggio celle erogate
                 if (cellsCounterLow < 0xFF){
                     cellsCounterLow++;
                 }else{
                     cellsCounterHig++;
                     cellsCounterLow=0;
                 }
+                // ciclo conteggio celle per carico manuale
                 if (loadDaCanInCorso==1){
                     cntCellsForLoad++;
                     if (cntCellsForLoad >= 5){
@@ -1371,6 +1386,7 @@
                 }else{
                     cntCellsForLoad=0;
                 }
+                // inibizione controllo di sincro per fuori fase
                 if (trigSD==0){
                     inhibit=1;
                     trigTB=1;
@@ -1379,12 +1395,13 @@
                     trigTB=0;
                     trigSD=0;
                 }
-                // torna indietro per sbloccare il tamburo
+                // conta le celle indietro per sbloccare il tamburo
                 if ((TBmotorDirecti==0)&&(erroreTamburo==1)){
                     cntCellsForReload++;
                     if (cntCellsForReload >= cellsCountSet){
-                        TBmotorDirecti=1;
+                        TBmotorDirecti=1;       // rotazione normale
                         erroreTamburo=0;
+                        cntCellsCorrect=0;
                     }    
                 }
                 #if defined(seedSensor)
@@ -1392,7 +1409,7 @@
                     delaySeedCheck.start();
                 #endif
             }
-            if ((double)TBactualPosition > ((TBgiroStep/cellsNumber)*1.2f)){
+            if ((double)TBactualPosition > ((TBgiroStep/cellsNumber)*1.2f)&&(erroreTamburo==0)){
                 if (firstStart==0){
                     if (cntTbError>2){
                         all_cellSignal=1;
@@ -1403,7 +1420,7 @@
                 }
                 if (erroreTamburo==0){
                     erroreTamburo=1;
-                    TBmotorDirecti=0;
+                    TBmotorDirecti=0;       // rotazione inversa
                     cntCellsForReload=0;
                     cntTbError++;
                     #if defined(seedSensor)