Francesco Pistone
new
Diff: main.cpp
- Revision:
- 2:d9c7430ae953
- Parent:
- 1:e88bf5011af6
- Child:
- 3:c0f11ca4df02
--- a/main.cpp Fri Jun 15 07:01:13 2018 +0000
+++ b/main.cpp Fri Jun 15 07:32:35 2018 +0000
@@ -621,8 +621,6 @@
masterSinc = masterSinc + ((uint32_t) rxMsg.data[1] * 0x0000FF00);
masterSinc = masterSinc + ((uint32_t) rxMsg.data[2] * 0x000000FF);
masterSinc = masterSinc + ((uint32_t) rxMsg.data[3]);
- quincVerify=1;
- //pc.printf("pippo");
}
}
#endif
@@ -709,153 +707,60 @@
}
}
//*******************************************************
-void quinCalc(double parametro){
- //double riferimento=tempoTraBecchi_mS/parametro;
+void quinCalc(){
// riceve l'impulso di sincro dal master e fa partire il timer di verifica dell'errore
- if (quinconceActive==0){
+ #if !defined(mezzo)
if ((quincClock==true)&&(oldQuincIn==0)){
oldQuincIn=1;
oldQuincTimeSD = (double) quincTimeSD.read_ms();
- if (trigSlave==false){
- quincInibit=true;
- trigDaMaster=true;
- }else{
- quincInibit=false;
- trigDaMaster=false;
- trigSlave=false;
- }
if (quincStart==0){
quincTime.reset();
quincStart=1;
}
}
- if( quincClock==false){
+ if(quincClock==false){
oldQuincIn=0;
}
- }else{
- if ((quincClock==false)&&(oldQuincIn==0)){
+ #else
+ if ((((quinconceActive==0)&&(quincClock==true))||((quinconceActive==1)&&(quincClock==false)))&&(oldQuincIn==0)){
oldQuincIn=1;
oldQuincTimeSD = (double) quincTimeSD.read_ms();
- if (trigSlave==false){
- quincInibit=true;
- trigDaMaster=true;
- }else{
- quincInibit=false;
- trigDaMaster=false;
- trigSlave=false;
- }
if (quincStart==0){
quincTime.reset();
quincStart=1;
}
}
- if( quincClock==true){
+ if((((quincClock==false)&&(quinconceActive==0))||((quincClock==true)&&(quinconceActive==1)))){
oldQuincIn=0;
}
- }
+ #endif
// riceve l'impulso di passaggio del becco locale e determina il valore dell'errore
// controllo di linea e di quinconcio
- if ((quinconceActive==0)||(quinconceActive==1)){
- if ((quincStart==1)&&(SDzeroDebounced==1)){
- if (quincMemoEnable==false){
- memoDuty=correzione;
- 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
+ if ((quincStart==1)&&(SDzeroDebounced==1)){
+ quincStart=0;
+ percento=1.0f;
+ scostamento = (double)quincTime.read_ms();
+ scostamento2 = oldQuincTimeSD;
+ 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
- }
- /*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;
- }
- }*/
+ #endif
}
- /*
- // gestione con confronto encoder
- // serve l'encoder fisico sugli elementi slave perchè altrimenti la ruota cambia velocità ma non cambia
- // il clock dell'encoder virtuale e quindi l'errore non può essere rilevato correttamente
- if ((quinconceActive==0)&&(quincEnable==1)){
- // da encoder master
- if (quincVerify==1){
- quincVerify=0;
- quincStart=0;
- memoDuty=dcActualDuty;
- if (quincInibit==false){
- percento=0.0f;
- quincError = 0.0f;
- if (masterSinc > prePosSD){
- quincError = masterSinc - prePosSD;
- percento = 1.0f + ((abs(quincError)/SDsectorStep));
- }
- if (masterSinc < prePosSD){
- quincError = prePosSD -masterSinc;
- percento = 1.0f - ((abs(quincError)/SDsectorStep));
- }
- // se quinconce richiesto deve andare al valore di riferimento
- if (abs(percento) > 1.55f){percento=1.55f;}
- if (abs(percento) < 0.40f){percento=0.40f;}
- if (abs(percento) >0.0f){
- if (abs(percento)>1.0f){
- correzione=correzione+(abs(percento)*0.01f);
- }else{
- correzione=correzione-(abs(percento)*0.01f);
- }
- //dcActualDuty = memoDuty * abs(percento);
- }else{
- correzione=correzione;
- //dcActualDuty = memoDuty;
- }
- }
- #if defined(pcSerial)
- #if defined(Qnc)
- pc.printf(" trigMaster: %d", trigDaMaster);
- pc.printf(" trigSlave: %d", trigSlave);
- pc.printf(" quincinibit: %d", quincInibit);
- pc.printf(" masterSinc: %d", masterSinc);
- pc.printf(" prePosSD: %d",prePosSD);
- pc.printf(" percento: %f",percento);
- pc.printf(" correzione: %f\n",correzione);
- #endif
- #endif
- }
- }
- */
}
// ------------------------------------------------------------------------------------------------------------------------------------------------------------------
// MAIN SECTION
@@ -1062,7 +967,6 @@
// filtra il segnale dei becchi per misurare il tempo di intervallo tra loro
// ----------------------------------------
if (seedWheelZeroPinInput==0){
- //trigSlave=false;
if(seedFilter.read_ms()>=4){
oldSeedWheelZeroPinInput=0;
SDzeroDebounced=0;
@@ -1078,14 +982,6 @@
oldSeedWheelZeroPinInput=1;
quincTimeSD.reset();
SDzeroDebounced=1;
- if (trigDaMaster==false){
- quincInibit=true;
- trigSlave=true;
- }else{
- quincInibit=false;
- trigSlave=false;
- trigDaMaster=false;
- }
SDwheelTimer.reset();
if (quincCnt<10){
quincCnt++;
@@ -1395,22 +1291,11 @@
}
}
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 !defined(speedMaster)
+ if (quincEnable==1){
+ quinCalc();
+ }
+ #endif
if ((pulseRised1==1)&&(enableSpeed>3)&&(pulseRised2==1)){
double erroreTempo = 0.0f;
if(speedFromPick==1){
@@ -1474,25 +1359,6 @@
pc.printf(" DUTY PRE FINALE: %f",dcActualDuty);
#endif
#endif
- // prova a mantenere il quinconce quando attivo
- //dcActualDuty=dcActualDuty;
- /*
- #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
- */
DC_brake=0;
DC_forward=1;
DC_prepare();