Francesco Pistone
/
FORIGO_Modula_V6_R2_DE_2019
new
Diff: main.cpp
- Revision:
- 1:e88bf5011af6
- Parent:
- 0:1e09cd7d66b4
- Child:
- 2:d9c7430ae953
--- a/main.cpp Wed Jun 13 08:52:42 2018 +0000 +++ b/main.cpp Fri Jun 15 07:01:13 2018 +0000 @@ -1,4 +1,5 @@ - //******************************************************************************************************************** + +//******************************************************************************************************************** //******************************************************************************************************************** // FIRMWARE SEMINATRICE MODULA // VERSIONE PER SCHEDA DI CONTROLLO CON DRIVER INTEGRATI @@ -709,11 +710,12 @@ } //******************************************************* void quinCalc(double parametro){ - double riferimento=tempoTraBecchi_mS/parametro; + //double riferimento=tempoTraBecchi_mS/parametro; // riceve l'impulso di sincro dal master e fa partire il timer di verifica dell'errore if (quinconceActive==0){ if ((quincClock==true)&&(oldQuincIn==0)){ oldQuincIn=1; + oldQuincTimeSD = (double) quincTimeSD.read_ms(); if (trigSlave==false){ quincInibit=true; trigDaMaster=true; @@ -733,6 +735,7 @@ }else{ if ((quincClock==false)&&(oldQuincIn==0)){ oldQuincIn=1; + oldQuincTimeSD = (double) quincTimeSD.read_ms(); if (trigSlave==false){ quincInibit=true; trigDaMaster=true; @@ -754,37 +757,54 @@ // controllo di linea e di quinconcio if ((quinconceActive==0)||(quinconceActive==1)){ if ((quincStart==1)&&(SDzeroDebounced==1)){ - if (quincInibit==false){ + if (quincMemoEnable==false){ memoDuty=correzione; - accelera=0; - decelera=0; - quincStart=0; - percento=1.0f; - scostamento = (double)quincTime.read_ms(); - quincError = 0.0f; - double halfRef=riferimento/2.0f; - if ((scostamento > 0.0f)&&(scostamento < halfRef)){ - decelera=1; - accelera=0; - percento = 1.0f - (abs(scostamento)/riferimento); - } - if ((scostamento < riferimento)&&(scostamento > halfRef)){ - decelera=0; - accelera=1; - quincError = riferimento - scostamento; - percento = 1.0f + (abs(quincError)/riferimento); - } - if (abs(percento) > 1.15f){percento=1.15f;} - if (abs(percento) < 0.5f){percento=0.5f;} + 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 + #endif } - if (abs(percento) >0.0f){ + /*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; + } + }*/ } /* // gestione con confronto encoder @@ -797,19 +817,13 @@ quincStart=0; memoDuty=dcActualDuty; if (quincInibit==false){ - accelera=0; - decelera=0; percento=0.0f; quincError = 0.0f; if (masterSinc > prePosSD){ - decelera=0; - accelera=1; quincError = masterSinc - prePosSD; percento = 1.0f + ((abs(quincError)/SDsectorStep)); } if (masterSinc < prePosSD){ - decelera=1; - accelera=0; quincError = prePosSD -masterSinc; percento = 1.0f - ((abs(quincError)/SDsectorStep)); } @@ -926,6 +940,7 @@ rotationTimeOut.start(); metalTimer.start(); quincTime.start(); + quincTimeSD.start(); //******************************************************* // controls for check DC motor current @@ -1061,6 +1076,7 @@ seedFilter.reset(); sincroTimer.reset(); oldSeedWheelZeroPinInput=1; + quincTimeSD.reset(); SDzeroDebounced=1; if (trigDaMaster==false){ quincInibit=true; @@ -1074,11 +1090,14 @@ if (quincCnt<10){ quincCnt++; } - if (quincCnt >3){ + if (quincCnt >9){ quincEnable=1; }else{ quincEnable=0; } + if (cntQuinc<10){ + cntQuinc++; + } if ((aspettaStart==0)&&(lowSpeed==1)){ beccoPronto=1; } @@ -1376,21 +1395,60 @@ } } 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 ((pulseRised1==1)&&(enableSpeed>3)&&(pulseRised2==1)){ double erroreTempo = 0.0f; if(speedFromPick==1){ - erroreTempo = (double)timeIntraPick - tempoTraBecchi_mS; // errore tra il tempo previsto ed il tempo reale ( >0 se sto andando più piano del previsto) + 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) }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.065f;double k3=0.0f;double k4=0.0f;double k5=0.0f; - if (tractorSpeed_MtS_timed <= minSeedSpeed){k3=0.105;k4=0.207;k5=0.310; - }else{k3=0.1075f;k4=0.211f;k5=0.315f;} - if (errorePercentuale > 25.0f){k1=errorePercentuale*k5;} - if ((errorePercentuale >= 10.0f)&&(errorePercentuale<=25.0f)){k1=errorePercentuale*k4;} - if (errorePercentuale < 10.0f){k1=errorePercentuale*k3;} + double k1=errorePercentuale*0.265f; + double k3=0.0f; + double k4=0.0f; + double k5=0.0f; + if (tractorSpeed_MtS_timed <= minSeedSpeed){ + k3=0.105; + k4=0.207; + k5=0.310; + }else{ + k3=0.1075f; + k4=0.211f; + k5=0.315f; + } + if ((errorePercentuale > 25.0f)||(errorePercentuale < -25.0f)){ + k1=errorePercentuale*k5; + } + if (((errorePercentuale >= 10.0f)&&(errorePercentuale<=25.0f))||((errorePercentuale <= -10.0f)&&(errorePercentuale>=-25.0f))){ + k1=errorePercentuale*k4; + } + if ((errorePercentuale < 10.0f)||(errorePercentuale > -10.0f)){ + k1=errorePercentuale*k3; + } double memoCorrezione = (dutyTeorico *k1); + #if defined(pcSerial) + #if defined(Qnc) + pc.printf("IntraPick: %f TempoBecchi: %f ErrPerc: %f\n",timeIntraPick, tempoTraBecchi_mS,errorePercentuale); + #endif + #endif correzione = correzione + memoCorrezione; pulseRised1=0; pulseRised2=0; @@ -1410,19 +1468,16 @@ boostDcOut=-0.2f; all_genericals=1; } - correzione += boostDcOut; + //correzione += boostDcOut; #if defined(pcSerial) #if defined(Qnca) pc.printf(" DUTY PRE FINALE: %f",dcActualDuty); #endif #endif // prova a mantenere il quinconce quando attivo - #if defined(speedMaster) - //dcActualDuty=dcActualDuty; - DC_brake=0; - DC_forward=1; - DC_prepare(); - #else + //dcActualDuty=dcActualDuty; + /* + #if !defined(speedMaster) #if defined(mezzo) if (quincEnable==1){ if (quinconceActive==1){ @@ -1431,17 +1486,16 @@ quinCalc(1.0f); } }else{ - DC_brake=0; - DC_forward=1; - DC_prepare(); + quincMemoEnable=false; } #else quinCalc(1.0f); - DC_brake=0; - DC_forward=1; - DC_prepare(); #endif #endif + */ + DC_brake=0; + DC_forward=1; + DC_prepare(); // il semiperiodoreale è calcolato sulla lettura del passaggio becchi reale if (correggiSD == true){ @@ -1456,12 +1510,12 @@ } //if (correzione > (dutyTeorico*0.20)){correzione = dutyTeorico*0.20;} - if (correzione > 0.30){correzione = 0.30;} - if (correzione < -0.30){correzione = -0.30;} + if (correzione > 0.30f){correzione = 0.30f;} + if (correzione < -0.30f){correzione = -0.30f;} if((enableSDcontrol==1)){ if (correzioneAttiva==1){ dcActualDuty = dutyTeorico + correzione; - if (dcActualDuty<dcStarting){dcActualDuty=dcStarting;} + //if (dcActualDuty<dcStarting){dcActualDuty=dcStarting;} } }else{ dcActualDuty = dutyTeorico;