Francesco Pistone
new
Diff: main.cpp
- Revision:
- 1:e88bf5011af6
- Parent:
- 0:1e09cd7d66b4
- Child:
- 2:d9c7430ae953
diff -r 1e09cd7d66b4 -r e88bf5011af6 main.cpp
--- 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;