Francesco Pistone
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){