Francesco Pistone
/
FORIGO_Modula_V6_R2C_DE_HwV5_OTTOBRE2018
tr
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;}