Francesco Pistone
new
Diff: main.cpp
- Revision:
- 5:3b95bbfe2dc9
- Parent:
- 4:d32258ec411f
- Child:
- 6:3fca0ca1949e
--- a/main.cpp Wed Jun 27 15:33:42 2018 +0000
+++ b/main.cpp Wed Jul 04 05:51:03 2018 +0000
@@ -102,6 +102,16 @@
speedOfSeedWheel=((seedPerimeter/25.0f)/(double)memo_TimeHole)*1000.0f; //mtS
}
}
+// rise of seed speed motor encoder
+void encoRise(){
+ timeHole=metalTimer.read_us();
+ int memo_TimeHole= (memoTimeHole + timeHole)/ (int)2;
+ memoTimeHole = timeHole;
+ metalTimer.reset();
+ if (encoder==true){
+ speedOfSeedWheel=((seedPerimeter/((SDreductionRatio*25.5f)))/(double)memo_TimeHole)*1000000.0f; //mtS
+ }
+}
//**************************************************
// generate speed clock when speed is simulated from Tritecnica display
void speedSimulationClock(){
@@ -143,6 +153,9 @@
void step_SDPulseOut(){
SDactualPosition++;
prePosSD++;
+ #if defined(speedMaster)
+ posForQuinc++;
+ #endif
}
//*******************************************************
void step_TBPulseOut(){
@@ -357,10 +370,15 @@
#if defined(speedMaster)
void upDateSincro(){
char val1[8]={0,0,0,0,0,0,0,0};
- val1[0]=(prePosSD /0x00FF0000)&0x000000FF;
- val1[1]=(prePosSD /0x0000FF00)&0x000000FF;
- val1[2]=(prePosSD /0x000000FF)&0x000000FF;
- val1[3]=prePosSD & 0x000000FF;
+ val1[0]=(posForQuinc /0x00FF0000)&0x000000FF;
+ val1[1]=(posForQuinc /0x0000FF00)&0x000000FF;
+ val1[2]=(posForQuinc /0x000000FF)&0x000000FF;
+ val1[3]=posForQuinc & 0x000000FF;
+ double pass = tractorSpeed_MtS_timed*100.0f;
+ val1[4]=(uint8_t)(pass)&0x000000FF;
+ val1[5]=(prePosSD /0x0000FF00)&0x000000FF;
+ val1[6]=(prePosSD /0x000000FF)&0x000000FF;
+ val1[7]=prePosSD & 0x000000FF;
#if defined(canbusActive)
#if defined(speedMaster)
if(can1.write(CANMessage(TX_SI, *&val1,8))){
@@ -441,12 +459,10 @@
#if defined(canbusActive)
if(can1.read(rxMsg)) {
if (firstStart==1){
- /*
#if defined(speedMaster)
- sincUpdate.attach(&upDateSincro,0.01f);
+ sincUpdate.attach(&upDateSincro,0.009f);
#endif
- */
- canUpdate.attach(&upDateSpeed,0.11f);
+ canUpdate.attach(&upDateSpeed,0.21f);
firstStart=0;
}
@@ -616,10 +632,11 @@
if (rxMsg.id==RX_Quinconce){
if (tractorSpeed_MtS_timed==0.0f){
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];
+ quincPIDminus = (uint8_t) rxMsg.data[1];
+ quincPIDplus = (uint8_t) rxMsg.data[2];
+ quincLIMminus = (uint8_t) rxMsg.data[3];
+ quincLIMplus = (uint8_t) rxMsg.data[4];
+ quincSector = (uint8_t) rxMsg.data[5];
aggiornaParametri();
}
}
@@ -628,6 +645,11 @@
masterSinc = masterSinc + ((uint32_t) rxMsg.data[1] * 0x0000FF00);
masterSinc = masterSinc + ((uint32_t) rxMsg.data[2] * 0x000000FF);
masterSinc = masterSinc + ((uint32_t) rxMsg.data[3]);
+ speedFromMaster = (double)rxMsg.data[4]/100.0f;
+ mast2_Sinc = ((uint32_t) rxMsg.data[5] * 0x0000FF00);
+ mast2_Sinc = mast2_Sinc + ((uint32_t) rxMsg.data[6] * 0x000000FF);
+ mast2_Sinc = mast2_Sinc + ((uint32_t) rxMsg.data[7]);
+ canDataCheck=1;
}
}
#endif
@@ -724,8 +746,8 @@
#if !defined(mezzo)
if ((quincClock==true)&&(oldQuincIn==0)){
oldQuincIn=1;
- oldQuincTimeSD = (double) quincTimeSD.read_ms();
if (quincStart==0){
+ oldQuincTimeSD = (double) quincTimeSD.read_ms();
quincTime.reset();
quincStart=1;
}
@@ -735,14 +757,9 @@
}
#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){
+ oldQuincTimeSD = (double) quincTimeSD.read_ms();
quincTime.reset();
quincStart=1;
}
@@ -757,42 +774,94 @@
}
}
#endif
- // riceve l'impulso di passaggio del becco locale e determina il valore dell'errore
- // controllo di linea e di quinconcio
- if ((quincStart==1)&&(sincroQui==1)){
- quincStart=0;
- sincroQui=0;
- scostamento2 = oldQuincTimeSD;
- 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;
+ //****************************************************************************************
+ if (quincCnt>=6){
+ if (countPicks==0){
+ if ((sincroQui==1)&&(quincStart==0)){
+ // decelera
+ countPicks=1;
}
- flippa=true;
- }
- 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 ((sincroQui==0)&&(quincStart==1)){
+ // accelera
+ countPicks=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
+
+ if ((sincroQui==1)&&(quincStart==1)){
+ if (countPicks==1){ //decelera
+ scostamento = oldQuincTimeSD;
+ if (scostamento < (tempoBecchiPerQuinc*0.75f)){
+ double scostPerc = (scostamento/tempoBecchiPerQuinc);
+ percento -= ((double)quincPIDminus/1000.0f)*(scostPerc);
+ #if defined(pcSerial)
+ #if defined(laq)
+ pc.printf("RALL scos2: %f tbpq: %f percento: %f \n",scostamento,tempoBecchiPerQuinc,percento);
+ #endif
+ #endif
+ }
+ if (scostamento <10.0f){percento=0.0f;}
+ }
+ if (countPicks==2){ //accelera
+ scostamento = (double)quincTime.read_ms();
+ if (scostamento < (tempoBecchiPerQuinc*0.75f)){
+ double scostPerc = (scostamento/tempoBecchiPerQuinc);
+ percento += ((double)quincPIDplus/1000.0f)*(scostPerc);
+ #if defined(pcSerial)
+ #if defined(laq)
+ pc.printf(
+ "ACCE scos1: %f tbpq: %f percento: %f \n",scostamento,tempoBecchiPerQuinc,percento);
+ #endif
+ #endif
+ }
+ if (scostamento <10.0f){percento=0.0f;}
+ }
+ sincroQui=0;
+ quincStart=0;
+ countPicks=0;
+ }
+
+ //*******************************************************************
+ if (canDataCheckEnable==true){
+ if (canDataCheck==1){ // sincro da comunicazione can del valore di posizione del tamburo master
+ canDataCheck=0;
+ double parametro = SDsectorStep/5.0f;
+ double differenza=0.0f;
+ #if defined(mezzo)
+ if (quinconceActive==1){
+ differenza = (double)masterSinc - (double)prePosSD;
+ }else{
+ differenza = (double)mast2_Sinc - (double)prePosSD;
+ }
+ #else
+ differenza = (double)mast2_Sinc - (double)prePosSD;
+ #endif
+ if ((differenza > 0.0f)&&(differenza < parametro)){
+ double diffPerc = differenza / parametro;
+ percento += ((double)quincPIDplus/1000.0f)*abs(diffPerc);
+ #if defined(pcSerial)
+ #if defined(quinca)
+ pc.printf("m1 %d m2 %d prePo %d diffe %f perce %f parm %f %\n",masterSinc, mast2_Sinc,prePosSD,differenza,percento, parametro);
+ #endif
+ #endif
+ }
+ if ((differenza < 0.0f)&&(abs(differenza) < parametro)){
+ double diffPerc = (double)differenza / parametro;
+ percento -= ((double)quincPIDminus/1000.0f)*abs(diffPerc);
+ #if defined(pcSerial)
+ #if defined(quinca)
+ pc.printf("m1 %d m2 %d prePo %d diffe %f perce %f parm %f %\n",masterSinc, mast2_Sinc,prePosSD,differenza,percento, parametro);
+ #endif
+ #endif
+ }
+ }
+ }
+
+ }
+ if ((percento) > ((double) quincLIMplus/100.0f)){
+ percento= (double)quincLIMplus/100.0f;
+ }
+ if ((percento) < (((double)quincLIMminus*-1.0f)/100.0f)){
+ percento=((double)quincLIMminus*-1.0f)/100.0f;
}
}
// ------------------------------------------------------------------------------------------------------------------------------------------------------------------
@@ -801,6 +870,7 @@
int main()
{
+ //wait(1.0f);
wd.Configure(2); //watchdog set at xx seconds
for (int a=0; a<5;a++){
@@ -869,6 +939,9 @@
DC_prepare();
speedTimer.start(); // speed pulse timer
+ if (encoder==true){
+ intraEncoTimer.start();
+ }
intraPickTimer.start();
speedTimeOut.start();
speedFilter.start();
@@ -882,9 +955,7 @@
//*******************************************************
// controls for check DC motor current
- //currentCheck.attach(&DC_CheckCurrent, 0.10f);
pwmCheck.rise(&startDelay);
- //tbCorrection.attach(&correction,0.1f;
if (inProva==0){
tractorSpeedRead.rise(&tractorReadSpeed);
@@ -892,11 +963,17 @@
quinconceIn.rise(&quincTrigon);
quinconceIn.fall(&quincTrigof);
#endif
- tftUpdate.attach(&videoUpdate,0.50f);
+ #if defined(speedMaster)
+ tftUpdate.attach(&videoUpdate,0.50f);
+ #endif
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
- if (speedFromPick==0){
+ if ((speedFromPick==0)&&(encoder==false)){
ElementPosition.rise(&sd25Fall);
}
+ if (encoder==true){
+ ElementPosition.rise(&encoRise);
+ //ElementPosition.fall(&encoRise);
+ }
}else{
tftUpdate.attach(&videoUpdate,0.125f);
}
@@ -941,8 +1018,11 @@
if (seedWheelZeroPinInput==1){
quinconceOut=1;
}
- if ((double)(prePosSD-500) >= (SDsectorStep/2.0f)){
+ if (((double)(prePosSD-500) >= (SDsectorStep/((double)quincSector)))&&(quinconceOut=1)){
quinconceOut=0;
+ if (quinconceActive==1){
+ posForQuinc=500;
+ }
}
#endif
@@ -1015,7 +1095,7 @@
}
}
if ((seedWheelZeroPinInput==1)&&(oldSeedWheelZeroPinInput==0)){
- timeIntraPick = (double)intraPickTimer.read_ms()-percento;
+ timeIntraPick = (double)intraPickTimer.read_ms();
prePosSD=500; // preposizionamento SD
intraPickTimer.reset();
rotationTimeOut.reset();
@@ -1026,14 +1106,14 @@
SDzeroDebounced=1;
sincroQui=1;
SDwheelTimer.reset();
+ #if defined(speedMaster)
+ if (quinconceActive==0){
+ posForQuinc=500;
+ }
+ #endif
if (quincCnt<10){
quincCnt++;
}
- if (quincCnt >5){
- quincEnable=1;
- }else{
- quincEnable=0;
- }
if ((aspettaStart==0)&&(lowSpeed==1)){
beccoPronto=1;
}
@@ -1083,7 +1163,7 @@
}
}
memoIntraPick = timeIntraPick;
- if (speedFromPick==1){
+ if ((speedFromPick==1)&&(encoder==false)){
speedOfSeedWheel=((seedPerimeter / pickNumber)/timeIntraPick)*1000.0f;
#if defined(pcSerial)
#if defined(Qnca)
@@ -1105,7 +1185,7 @@
#endif
pulseRised2=1;
#if defined(speedMaster)
- double oldLastPr = (double)oldLastPulseRead*1.2f;
+ double oldLastPr = (double)oldLastPulseRead*1.5f;
if((double)speedTimeOut.read_us()> (oldLastPr)){
if ((firstStart==0)&&(simOk==0)&&(enableSimula==0)&&(zeroCycleEnd==1)){
all_speedError =1;
@@ -1195,7 +1275,7 @@
// torna indietro per sbloccare il tamburo
if ((TBmotorDirecti==1)&&(erroreTamburo==1)){
cntCellsForReload++;
- if (cntCellsForReload > 3){
+ if (cntCellsForReload > 2){
TBmotorDirecti=0;
erroreTamburo=0;
}
@@ -1203,7 +1283,9 @@
}
if ((double)TBactualPosition > ((TBgiroStep/cellsNumber)*1.2f)){
if (firstStart==0){
- all_cellSignal=1;
+ if (cntTbError>2){
+ all_cellSignal=1;
+ }
}
if (erroreTamburo==0){
erroreTamburo=1;
@@ -1217,7 +1299,7 @@
#endif
#endif
}
- if (((double)TBactualPosition > ((TBgiroStep/cellsNumber)*3.0f))||(cntTbError>2)){
+ if (((double)TBactualPosition > ((TBgiroStep/cellsNumber)*3.0f))||(cntTbError>4)){
if (firstStart==0){
all_noStepRota=1;
}
@@ -1278,11 +1360,20 @@
double minIntervalPulse = pulseDistance/maxWorkSpeed;
if (pulseRised==1){
if (enableSpeed<10){enableSpeed++;}
- pulseRised=0;pulseRised1=1;speedMediaCalc();
+ pulseRised=0;
+ pulseRised1=1;
+ speedMediaCalc();
// calcola velocità trattore
if(enableSpeed>=2){
if ((pulseSpeedInterval>=0.0f)){ //minIntervalPulse)&&(pulseSpeedInterval<maxInterval)){
tractorSpeed_MtS_timed = ((pulseDistance / pulseSpeedInterval)); // tractor speed (unit= Mt/S) from pulse time interval
+ #if !defined(speedMaster)
+ if (quincCnt>=5){
+ if (speedFromMaster>0.0f){
+ tractorSpeed_MtS_timed = speedFromMaster + percento;
+ }
+ }
+ #endif
if (checkSDrotation==0){
checkSDrotation=1;
SDwheelTimer.start();
@@ -1313,10 +1404,19 @@
cycleStopRequest=1;
// calcola il tempo teorico di passaggio becchi sulla base della velocità del trattore
tempoGiroSD = seedPerimeter / tractorSpeed_MtS_timed; // tempo Teorico impiegato dalla ruota di semina per fare un giro completo (a 4,5Km/h impiega 1,89 secondi)
- if (speedFromPick==1) {
- tempoTraBecchi_mS = (tempoGiroSD / pickNumber)*1000.0f; // tempo tra due impulsi dai becchi in millisecondi ( circa 157mS a 4,5Km/h)
+ if (encoder==false){
+ if (speedFromPick==1) {
+ tempoTraBecchi_mS = (tempoGiroSD / pickNumber)*1000.0f; // tempo tra due impulsi dai becchi in millisecondi ( circa 157mS a 4,5Km/h)
+ }else{
+ tempoTraBecchi_mS = (tempoGiroSD / 25.0f)*1000.0f; // tempo tra due impulsi dai becchi in millisecondi ( circa 157mS a 4,5Km/h)
+ }
}else{
- tempoTraBecchi_mS = (tempoGiroSD / 25.0f)*1000.0f; // tempo tra due impulsi dai becchi in millisecondi ( circa 157mS a 4,5Km/h)
+ tempoTraBecchi_mS = (tempoGiroSD / (SDreductionRatio*25.5f))*1000.0f; // tempo tra due impulsi dai becchi in millisecondi ( circa 157mS a 4,5Km/h)
+ double pippo=0.0f;
+ #if !defined(speedMaster)
+ pippo = seedPerimeter / speedFromMaster;
+ #endif
+ tempoBecchiPerQuinc = (pippo / pickNumber)*1000.0f;
}
//*******************************************
// segue calcolo duty cycle comando motore DC per allinearsi con la velocità del trattore
@@ -1331,22 +1431,16 @@
#if !defined(speedMaster)
quinCalc();
#endif
- if ((pulseRised1==1)&&(enableSpeed>3)&&(pulseRised2==1)){
+ if ((enableSpeed>3)&&(pulseRised2==1)&&(quincCnt>=2)){
double erroreTempo = 0.0f;
- double giorgio = 0.0f;
- if(speedFromPick==1){
- #if !defined(speedMaster)
- 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;
- }
- #else
+ if(encoder==false){
+ if(speedFromPick==1){
erroreTempo = (double)timeIntraPick - tempoTraBecchi_mS;
- #endif
+ }else{
+ erroreTempo = (double)memoTimeHole - 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)
+ erroreTempo = ((double)memoTimeHole/1000.0f) - 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 k3=0.0f;
@@ -1393,7 +1487,7 @@
pulseRised2=0;
#if defined(pcSerial)
#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("ErTem: %f K1: %f Corr: %f MemoCorr:%f DutyTeo: %f \n",erroreTempo, 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
@@ -1426,11 +1520,7 @@
DC_prepare();
// il semiperiodoreale è calcolato sulla lettura del passaggio becchi reale
- if (correggiSD == true){
- seedWheelPeriod = semiPeriodoReale;// / abs(percento);
- }else{
- seedWheelPeriod = semiPeriodoReale;
- }
+ seedWheelPeriod = semiPeriodoReale;
if (seedWheelPeriod < 180.0f){seedWheelPeriod = 180.0f;}
if ((oldSeedWheelPeriod!=seedWheelPeriod)&&(seedWheelPeriod >=180.0f )){
SDticker.attach_us(&step_SDPulseOut,seedWheelPeriod); // clock time are milliseconds and attach seed motor stepper controls
@@ -1489,10 +1579,14 @@
if (beccoPronto==1){
if (tamburoStandard==1){
double ritardoMassimo = 0.0f;
- if(speedFromPick==1){
+ if (encoder==false){
+ if(speedFromPick==1){
+ ritardoMassimo = (double)timeIntraPick;
+ }else{
+ ritardoMassimo = (double)memoTimeHole;
+ }
+ }else{
ritardoMassimo = (double)timeIntraPick;
- }else{
- ritardoMassimo = (double)memoTimeHole;
}
int tempoDiSincro = (int)((double)(ritardoMassimo - ((tempoBecco/2.0f)+((speedOfSeedWheel/1.25f)*ritardoMassimo)))); //
if (tempoDiSincro <= 1){tempoDiSincro=1;}