Francesco Pistone
/
FORIGO_Modula_V6_R2C_DE_HwV5_10gen2019_2019
new
Diff: main.cpp
- Revision:
- 4:d32258ec411f
- Parent:
- 3:c0f11ca4df02
- Child:
- 5:3b95bbfe2dc9
--- a/main.cpp Fri Jun 15 17:10:37 2018 +0000 +++ b/main.cpp Wed Jun 27 15:33:42 2018 +0000 @@ -255,6 +255,15 @@ } } } + } +} +//******************************************************* +void videoUpdate(){ + for(int aa=0;aa<4;aa++){speedForDisplay[aa]=speedForDisplay[aa+1];} + speedForDisplay[4]=tractorSpeed_MtS_timed; + totalSpeed=0.0f; + for (int aa=0; aa<5; aa++){totalSpeed += speedForDisplay[aa];} + totalSpeed = totalSpeed / 5.0f; #if defined(pcSerial) #if defined(SDreset) pc.printf("Fase: %d",fase); @@ -264,15 +273,6 @@ pc.printf(" Trigger: %d \n", trigRepos); #endif #endif - } -} -//******************************************************* -void videoUpdate(){ - for(int aa=0;aa<4;aa++){speedForDisplay[aa]=speedForDisplay[aa+1];} - speedForDisplay[4]=tractorSpeed_MtS_timed; - totalSpeed=0.0f; - for (int aa=0; aa<5; aa++){totalSpeed += speedForDisplay[aa];} - totalSpeed = totalSpeed / 5.0f; } //******************************************************* void ciclaTB(){ @@ -615,7 +615,11 @@ } if (rxMsg.id==RX_Quinconce){ if (tractorSpeed_MtS_timed==0.0f){ - quinconceActive = (uint8_t) rxMsg.data[0]; + quinconceActive = (uint8_t) rxMsg.data[0]; + quincPIDminus = 3;//(uint8_t) rxMsg.data[1]; + quincPIDplus = 3;//(uint8_t) rxMsg.data[2]; + quincLIMminus = 10;//(uint8_t) rxMsg.data[3]; + quincLIMplus = 10;//(uint8_t) rxMsg.data[4]; aggiornaParametri(); } } @@ -679,11 +683,17 @@ incrementCurrent=true; } */ - if (SD_CurrentScaled > 13.0f){ - #if defined(canbusActive) - all_overCurrDC=1; - #endif - reduceCurrent=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(); } //} } @@ -702,12 +712,11 @@ timeout.attach_us(&DC_CheckCurrent,ritardo); } //******************************************************* -void quincTrigga(){ - if (quinconceIn==1){ - quincClock=true; - }else{ - quincClock=false; - } +void quincTrigon(){ + quincClock=true; +} +void quincTrigof(){ + quincClock=false; } //******************************************************* void quinCalc(){ @@ -726,6 +735,11 @@ } #else if ((((quinconceActive==0)&&(quincClock==true))||((quinconceActive==1)&&(quincClock==false)))&&(oldQuincIn==0)){ + #if defined(pcSerial) + #if defined(quinca) + pc.printf("Active %d Clock: %d oldQin %d\n",quinconceActive,quincClock, oldQuincIn); + #endif + #endif oldQuincIn=1; oldQuincTimeSD = (double) quincTimeSD.read_ms(); if (quincStart==0){ @@ -733,31 +747,49 @@ quincStart=1; } } - if((((quincClock==false)&&(quinconceActive==0))||((quincClock==true)&&(quinconceActive==1)))){ - oldQuincIn=0; + if (quinconceActive==0){ + if (quincClock==false){ + oldQuincIn=0; + } + }else{ + if (quincClock==true){ + oldQuincIn=0; + } } #endif // riceve l'impulso di passaggio del becco locale e determina il valore dell'errore // controllo di linea e di quinconcio - if ((quincStart==1)&&(SDzeroDebounced==1)){ + if ((quincStart==1)&&(sincroQui==1)){ quincStart=0; - percento=1.0f; - scostamento = (double)quincTime.read_ms(); + sincroQui=0; scostamento2 = oldQuincTimeSD; - double pippo = 0.025f-((tractorSpeed_MtS_timed/1.25f)*0.02f); - double halfPippo= tempoTraBecchi_mS/2.0f; - 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); + scostamento = (double)quincTime.read_ms(); + if (((scostamento2 < scostamento))||(scostamento==0.0f)){ + if (scostamento>0.0f){ + percento += (double) quincPIDplus*(scostamento2/25.0f);///2.0f;//(scostamento2/tempoTraBecchi_mS)*4.0f; + }else{ + percento += (double) quincPIDplus;///2.0f;//(scostamento2/tempoTraBecchi_mS)*4.0f; + } + flippa=true; } - if (abs(percento) > 1.15f){percento=1.15f;} - if (abs(percento) < 0.80f){percento=0.80f;} + if ((scostamento < scostamento2)&&(scostamento>0.0f)){ + percento -= (double) quincPIDminus*(scostamento/25.0f);//(scostamento/tempoTraBecchi_mS)*4.0f; + if (flippa==true){ + percento=0.0f; + flippa=false; + //quincCnt=2; + } + } + if ((percento) > (double) quincLIMplus){percento= (double)quincLIMplus;} + if ((percento) < ((double)quincLIMminus*-1.0f)){percento=((double)quincLIMminus*-1.0f);} #if defined(pcSerial) #if defined(Qnc) + pc.printf("IP: %f", timeIntraPick); + pc.printf(" TB: %f", tempoTraBecchi_mS); pc.printf(" s1: %f", scostamento); pc.printf(" s2: %f", scostamento2); pc.printf(" pc: %f", percento); + pc.printf(" co: %f",correzione); pc.printf(" en: %d\n",quinconceActive); #endif #endif @@ -857,8 +889,8 @@ if (inProva==0){ tractorSpeedRead.rise(&tractorReadSpeed); #if !defined(speedMaster) - quinconceIn.rise(&quincTrigga); - quinconceIn.fall(&quincTrigga); + quinconceIn.rise(&quincTrigon); + quinconceIn.fall(&quincTrigof); #endif tftUpdate.attach(&videoUpdate,0.50f); seedCorrection.attach(&seedCorrect,0.005f); // con 16 becchi a 4,5Kmh ci sono 37mS tra un becco e l'altro, quindi 8 correzioni di tb @@ -892,7 +924,16 @@ if (tractorSpeedRead==0){speedClock=0;} // inversione segnali ingressi - seedWheelZeroPinInput = !seedWheelZeroPinInputRev; + #if !defined(simulaBanco) + seedWheelZeroPinInput = !seedWheelZeroPinInputRev; + #else + if ((prePosSD-500) >= SDsectorStep){ + seedWheelZeroPinInput=1; + } + if ((prePosSD > 500)&&(prePosSD<580)){ + seedWheelZeroPinInput=0; + } + #endif TBzeroPinInput = !TBzeroPinInputRev; // se quinconce attivo ed unita' master invia segnale di sincro @@ -967,14 +1008,14 @@ // ---------------------------------------- // filtra il segnale dei becchi per misurare il tempo di intervallo tra loro // ---------------------------------------- - if (seedWheelZeroPinInput==0){ + if ((seedWheelZeroPinInput==0)&&(oldSeedWheelZeroPinInput==1)){ if(seedFilter.read_ms()>=4){ oldSeedWheelZeroPinInput=0; SDzeroDebounced=0; } } if ((seedWheelZeroPinInput==1)&&(oldSeedWheelZeroPinInput==0)){ - timeIntraPick = (double)intraPickTimer.read_ms(); + timeIntraPick = (double)intraPickTimer.read_ms()-percento; prePosSD=500; // preposizionamento SD intraPickTimer.reset(); rotationTimeOut.reset(); @@ -983,18 +1024,16 @@ oldSeedWheelZeroPinInput=1; quincTimeSD.reset(); SDzeroDebounced=1; + sincroQui=1; SDwheelTimer.reset(); if (quincCnt<10){ quincCnt++; } - if (quincCnt >9){ + if (quincCnt >5){ quincEnable=1; }else{ quincEnable=0; } - if (cntQuinc<10){ - cntQuinc++; - } if ((aspettaStart==0)&&(lowSpeed==1)){ beccoPronto=1; } @@ -1003,12 +1042,6 @@ double fase1=0.0f; forzaFase=0; double limite=fixedStepGiroSD/pickNumber; - if (pickCounterLow< 0xFF){ - pickCounterLow++; - }else{ - pickCounterHig++; - pickCounterLow=0; - } if (tamburoStandard==0){ fase1=TBdeltaStep; }else{ @@ -1052,17 +1085,24 @@ memoIntraPick = timeIntraPick; if (speedFromPick==1){ speedOfSeedWheel=((seedPerimeter / pickNumber)/timeIntraPick)*1000.0f; - } - if ((tractorSpeed_MtS_timed==0.0f)&&(zeroCycle==0)&&(zeroCycleEnd==1)){ - if ((firstStart==0)&&(simOk==0)&&(enableSimula==0)&&(zeroCycleEnd==1)){ - all_noSpeedSen=1; - } #if defined(pcSerial) - #if defined(canDataReceived) - pc.printf("allarme no speed sensor"); + #if defined(Qnca) + pc.printf("perim: %f pickN: %f sped: %f\n", seedPerimeter, pickNumber,speedOfSeedWheel); #endif #endif } + #if defined(speedMaster) + if ((tractorSpeed_MtS_timed==0.0f)&&(zeroCycle==0)&&(zeroCycleEnd==1)){ + if ((firstStart==0)&&(simOk==0)&&(enableSimula==0)&&(zeroCycleEnd==1)){ + all_noSpeedSen=1; + } + #if defined(pcSerial) + #if defined(canDataReceived) + pc.printf("allarme no speed sensor"); + #endif + #endif + } + #endif pulseRised2=1; #if defined(speedMaster) double oldLastPr = (double)oldLastPulseRead*1.2f; @@ -1249,15 +1289,11 @@ } } } - if ((pulseSpeedInterval>=minIntervalPulse)&&(pulseSpeedInterval<maxInterval)){ - enableSDcontrol =1; - } speedTimeOut.reset(); }else{ double oldLastPr = (double)oldLastPulseRead*1.7f; if((double)speedTimeOut.read_us()> (oldLastPr)){ tractorSpeed_MtS_timed = 0.0f; - enableSDcontrol =0; pntMedia=0; speedTimeOut.reset(); enableSpeed=0; @@ -1265,8 +1301,8 @@ } } // esegue il controllo di velocità minima - if ((double)speedTimer.read_ms()>=maxInterval){tractorSpeed_MtS_timed = 0.0f; - enableSDcontrol =0; + if ((double)speedTimer.read_ms()>=maxInterval){ + tractorSpeed_MtS_timed = 0.0f; enableSpeed=0; } //*************************************************************************************************************** @@ -1293,17 +1329,15 @@ } if (tractorSpeed_MtS_timed > tabSpeed[16]){dutyTeorico=tractorSpeed_MtS_timed/1.25f;} #if !defined(speedMaster) - if (quincEnable==1){ - quinCalc(); - } + quinCalc(); #endif if ((pulseRised1==1)&&(enableSpeed>3)&&(pulseRised2==1)){ double erroreTempo = 0.0f; + double giorgio = 0.0f; if(speedFromPick==1){ #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){ + giorgio = timeIntraPick;//-percento; + if ((giorgio >0.0f)&&(quincEnable==1)){ 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; @@ -1315,47 +1349,59 @@ 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; - #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; - if (tractorSpeed_MtS_timed <= minSeedSpeed){ - k3=0.105; - k4=0.207; - k5=0.310; - }else{ - k3=0.03f; - k4=0.06f; - k5=0.20f; + double k6=0.0f; + /*if (tractorSpeed_MtS_timed <= minSeedSpeed){ + k3=1.030f; + k4=5.103f; + k5=10.00f; + k6=20.50f; + }else{*/ + k3=0.050f; + k4=2.103f; + k5=20.00f; + k6=30.50f; + //} + double L1 = 0.025f; + double L_1=-0.025f; + double L2 = 0.100f; + double L_2=-0.100f; + double L3 = 0.401f; + double L_3=-0.401f; + double k1=0.0f; + if ((errorePercentuale > L3)||(errorePercentuale < L_3)){ + k1=errorePercentuale*k6; } - 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)){ + if (((errorePercentuale >= L2)&&(errorePercentuale<=L3))||((errorePercentuale <= L_2)&&(errorePercentuale>=L_3))){ k1=errorePercentuale*k5; } - if (((errorePercentuale >= L2)&&(errorePercentuale<=L3))||((errorePercentuale <= L_2)&&(errorePercentuale>=L_3))){ + if (((errorePercentuale < L2)&&(errorePercentuale>L1))||((errorePercentuale > L_2)&&(errorePercentuale<L_1))){ k1=errorePercentuale*k4; } - if (((errorePercentuale < L2)&&(errorePercentuale>L1))||((errorePercentuale > L_2)&&(errorePercentuale<L_1))){ + if ((errorePercentuale < L1)||(errorePercentuale > L_1)){ k1=errorePercentuale*k3; } - double memoCorrezione = (dutyTeorico *k1); + double memoCorrezione = k1; + if (quincCnt >= 2){ + correzione = correzione + memoCorrezione; + if (correzione > (1.0f - dutyTeorico)){correzione = (1.0f - dutyTeorico);} + if ((correzione < 0.0f)&&(dutyTeorico+correzione<0.0f)){correzione = -1.0f*dutyTeorico;} + } + pulseRised1=0; + pulseRised2=0; #if defined(pcSerial) - #if defined(Qnc) - pc.printf("IntraPick: %f TempoBecchi: %f ErrPerc: %f\n",timeIntraPick, tempoTraBecchi_mS,errorePercentuale); + #if defined(Qnca) + pc.printf("ErTem: %f giorgio: %f K1: %f Corr: %f MemoCorr:%f DutyTeo: %f \n",erroreTempo, giorgio, k1,correzione, memoCorrezione, dutyTeorico); + pc.printf("TsP: %f SpW: %f InPic: %f TBec: %f EPerc: %f Duty:%f \n",tractorSpeed_MtS_timed,speedOfSeedWheel,timeIntraPick, tempoTraBecchi_mS,errorePercentuale, dcActualDuty); #endif #endif - correzione = correzione + memoCorrezione; - pulseRised1=0; - pulseRised2=0; + #if defined(pcSerial) + #if defined(Qncb) + pc.printf("TsP: %f SpW: %f InPic: %f EPerc: %f Duty:%f \n",tractorSpeed_MtS_timed,speedOfSeedWheel,timeIntraPick, errorePercentuale, dcActualDuty); + #endif + #endif } // introduce il controllo di corrente if (incrementCurrent){ @@ -1372,19 +1418,16 @@ boostDcOut=-0.2f; all_genericals=1; } + //correzione += boostDcOut; - #if defined(pcSerial) - #if defined(Qnca) - pc.printf(" DUTY PRE FINALE: %f",dcActualDuty); - #endif - #endif + DC_brake=0; DC_forward=1; DC_prepare(); // il semiperiodoreale è calcolato sulla lettura del passaggio becchi reale if (correggiSD == true){ - seedWheelPeriod = semiPeriodoReale / abs(percento); + seedWheelPeriod = semiPeriodoReale;// / abs(percento); }else{ seedWheelPeriod = semiPeriodoReale; } @@ -1394,25 +1437,21 @@ oldSeedWheelPeriod=seedWheelPeriod; } - //if (correzione > (dutyTeorico*0.20)){correzione = dutyTeorico*0.20;} - if (correzione > 0.30f){correzione = 0.30f;} - if (correzione < -0.30f){correzione = -0.30f;} - if((enableSDcontrol==1)){ + if((quincCnt>=3)){ if (correzioneAttiva==1){ dcActualDuty = dutyTeorico + correzione; - //if (dcActualDuty<dcStarting){dcActualDuty=dcStarting;} + }else{ + dcActualDuty = dutyTeorico; } }else{ dcActualDuty = dutyTeorico; } - if (dcActualDuty <0.0f){dcActualDuty=0.0f;} - if (dcActualDuty > 1.0f){dcActualDuty = dcMaxSpeed;} - #if defined(pcSerial) - #if defined(Qnca) - pc.printf(" DUTY FINALE: %f \n",dcActualDuty); - #endif - #endif - SDmotorPWM.write(dcActualDuty); + if (dcActualDuty <=0.0f){dcActualDuty=0.10f;} + if (dcActualDuty > 0.90f){dcActualDuty = 0.90f;}//dcMaxSpeed;} + if (olddcActualDuty!=dcActualDuty){ + SDmotorPWM.write(dcActualDuty); + olddcActualDuty=dcActualDuty; + } // allarme if (SDwheelTimer.read_ms()>4000){ if (firstStart==0){