Diff: main.cpp

Revision:

12:b0fc1d313813

Parent:

10:9e70619e97ab

Child:

13:d1030d4e51a8

--- a/main.cpp	Fri Mar 15 11:09:37 2019 +0000
+++ b/main.cpp	Mon Mar 25 11:32:34 2019 +0000
@@ -724,6 +724,10 @@
     TBmotorRst=1;
 }
 //****************************************
+void controllaCorrente(){
+    correggiCorrente=1;
+}
+//****************************************
 void dcSetting(){
     if ((speedFromPick==0)&&(encoder==false)) {
         DcEncoder.rise(&sd25Fall);
@@ -1203,57 +1207,9 @@
     //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];
-    }
-    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();
-    }
-    //}
-    #if defined(pcSerial)
-        #if defined(checkLoop)
-            pc.printf("21\n");
-        #endif
-    #endif
+    SD_CurrentScaled = floor((  (SD_CurrentStart-SD_CurrentAnalog)*3.3f)  /  (SD_CurrentFactor/1000.0f)*10)/10;
 }
 //*******************************************************
 void DC_prepare()
@@ -1289,20 +1245,11 @@
     #endif
 }
 //*******************************************************
-void startDelay()
-{
-    if (currentCheckEnable==true) {
-        int ritardo =0;
-        ritardo = (int)((float)(dcActualDuty*800.0f));
-        if (ritardo>250.0f) {
-            timeout.attach_us(&DC_CheckCurrent,ritardo);
-        }
-    }
-    #if defined(pcSerial)
-        #if defined(checkLoop)
-            pc.printf("23\n");
-        #endif
-    #endif
+void startDelay(){
+    SD_CurrentStart = floor(SDcurrent.read()*100)/100;              // valore in ingresso compreso tra 0.00 e 1.00
+    int ritardo =0;
+    ritardo = (int)((float)(dcActualDuty*800.0f));
+    timeout.attach_us(&DC_CheckCurrent,ritardo);
 }
 //*******************************************************
 void quincTrigon(){
@@ -1581,6 +1528,8 @@
     pwmCheck.rise(&startDelay);
     wait_ms(500);
 
+    dcVeri.attach(&controllaCorrente,2.0f);
+
     #if defined(runnerTos)
         thread.start(step_Reading);
     #endif
@@ -2057,9 +2006,7 @@
             //double maxInterval = pulseDistance/minWorkSpeed;
             //double minIntervalPulse = pulseDistance/maxWorkSpeed;
             if (pulseRised==1) {
-                if (enableSpeed<10) {
-                    enableSpeed++;
-                }
+                if (enableSpeed<10) {enableSpeed++;}
                 pulseRised=0;
                 pulseRised1=1;
                 speedMediaCalc();
@@ -2151,12 +2098,22 @@
                 //*******************************************
                 // segue calcolo duty cycle comando motore DC per allinearsi con la velocità del trattore
                 double dutyTeorico = 0.00;
+                double deltaV=0.0f;
+                double deltaD=0.0f;
+                double setV=0.0f;
+                double teoriaC=0.0f;
+                
                 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];
+                        // esegue l'interpolazione dei valori stimati di duty in base alla velocità
+                        deltaV=tabSpeed[ii+1]-tabSpeed[ii];
+                        deltaD=tabComan[ii+1]-tabComan[ii];
+                        setV = tractorSpeed_MtS_timed-tabSpeed[ii];
+                        teoriaC=(setV/deltaV)*deltaD;
+                        dutyTeorico = tabComan[ii]+teoriaC; // era ii+1 al 23/03/19
                     }
                 }
                 if (tractorSpeed_MtS_timed > tabSpeed[16]) {
@@ -2241,25 +2198,34 @@
                             pc.printf("TsP: %f SpW: %f InPic: %f   EPerc: %f Duty:%f \n",tractorSpeed_MtS_timed,speedOfSeedWheel,timeIntraPick, errorePercentuale, dcActualDuty);
                         #endif
                     #endif
+                    #if defined(pcSerial)
+                        #if defined(boost)
+                            pc.printf("boost: %f teory: %f real: %f curr: %f cc:%d check: %d\n",boostDcOut,dutyTeorico, dcActualDuty,SD_CurrentScaled,correggiCorrente,currentCheckEnable);
+                        #endif
+                    #endif
                 }
-                // introduce il controllo di corrente
-                if (currentCheckEnable==true) {
-                    if (incrementCurrent) {
-                        boostDcOut +=0.005f;
-                    }
-                    if (reduceCurrent) {
-                        boostDcOut -=0.005f;
+                    // introduce il controllo di corrente
+                    if (correggiCorrente==1){
+                        if (SD_CurrentScaled < 1.6f){
+                            boostDcOut +=0.01f;
+                        }
+                        if (SD_CurrentScaled > 2.6f){
+                            boostDcOut -=0.01f;
+                        }
+                        if (boostDcOut >= 0.2f){
+                            boostDcOut=0.2f;
+                            all_genericals=1;
+                        }
+                        if (boostDcOut <=-0.2f){
+                            boostDcOut=-0.2f;
+                            all_genericals=1;
+                        }   
+                        correggiCorrente=0;
                     }
-                    if (boostDcOut >= 0.2f) {
-                        boostDcOut=0.2f;
-                        all_genericals=1;
+                    if (currentCheckEnable==true){
+                        correzione += boostDcOut;
+                        boostDcOut=0.0f;
                     }
-                    if (boostDcOut <=-0.2f) {
-                        boostDcOut=-0.2f;
-                        all_genericals=1;
-                    }
-                    correzione += boostDcOut;
-                }
                 DC_brake=0;
                 DC_forward=1;
                 DC_prepare();