Francesco Pistone
/
FORIGO_Modula_V6_R2C_DE_HwV5_OTTOBRE2018
tr
Diff: main.cpp
- Revision:
- 7:c9fd242538d9
- Parent:
- 6:3fca0ca1949e
- Child:
- 8:0e643ea7834f
--- a/main.cpp Thu Jul 05 16:30:54 2018 +0000 +++ b/main.cpp Thu Jul 12 06:07:14 2018 +0000 @@ -110,8 +110,13 @@ metalTimer.reset(); if (encoder==true){ speedOfSeedWheel=((seedPerimeter/((SDreductionRatio*25.5f)))/(double)memo_TimeHole)*1000000.0f; //mtS + pulseRised2=1; } } +// rise of seed presence sensor +void seedSensorTask(){ + seedSee=1; +} //************************************************** // generate speed clock when speed is simulated from Tritecnica display void speedSimulationClock(){ @@ -119,6 +124,7 @@ oldLastPulseRead=lastPulseRead; speedTimer.reset(); pulseRised=1; + speedFilter.reset(); } //******************************************************* // interrupt task for tractor speed reading @@ -175,7 +181,6 @@ K_WheelRPM = 60.0f/seedPerimeter; // calcola il K per i giri al minuto della ruota di semina K_WhellFrequency = (seedWheelMotorSteps*SDreductionRatio)/60.0f; // calcola il K per la frequenza di comando del motore di semina rapportoRuote = pickNumber/cellsNumber; // calcola il rapporto tra il numero di becchi ed il numero di celle - K_TBfrequency = (TBmotorSteps*TBreductionRatio)/240.0f; K_percentuale = TBmotorSteps*TBreductionRatio; SDsectorStep = (double)fixedStepGiroSD / (double)pickNumber; TBsectorStep = (TBmotorSteps*TBreductionRatio)/cellsNumber; @@ -186,6 +191,8 @@ TBdeltaStep=(fixedStepGiroSD/pickNumber)+(stepGrado*avvioGradi); TBfaseStep = (stepGrado*angoloFase); K_WheelRPM = 60.0f/seedPerimeter; // calcola il K per i giri al minuto della ruota di semina 25.4 25,40 + TBgiroStep = TBmotorSteps*TBreductionRatio; + K_TBfrequency = TBgiroStep/60.0f; // 1600 * 1.65625f /60 = 44 44,00 if (speedFromPick==1) { intraPickDistance = seedPerimeter/pickNumber; }else{ @@ -304,6 +311,65 @@ loadDaCan=0; } } +// ------------------------------------------------------------------------------------------------------------------------------------------------------------------ +void stepSetting(){ + // Stepper driver init and set + TBmotorRst=1; // reset stepper driver + TBmotorDirecti=1; // reset stepper direction + // M1 M2 M3 RESOLUTION + // 0 0 0 1/2 + // 1 0 0 1/8 + // 0 1 0 1/16 + // 1 1 0 1/32 + // 0 0 1 1/64 + // 1 0 1 1/128 + // 0 1 1 1/10 + // 1 1 1 1/20 + if (TBmotorSteps==400){ + TBmotor_M1=1; + TBmotor_M2=1; + TBmotor_M3=1; + }else if (TBmotorSteps==1600){ + TBmotor_M1=0; + TBmotor_M2=1; + TBmotor_M3=1; + }else if (TBmotorSteps==3200){ + TBmotor_M1=1; + TBmotor_M2=0; + TBmotor_M3=1; + }else if (TBmotorSteps==6400){ + TBmotor_M1=0; + TBmotor_M2=0; + TBmotor_M3=1; + }else if (TBmotorSteps==12800){ + TBmotor_M1=1; + TBmotor_M2=1; + TBmotor_M3=0; + }else if (TBmotorSteps==25600){ + TBmotor_M1=0; + TBmotor_M2=1; + TBmotor_M3=0; + }else if (TBmotorSteps==2000){ + TBmotor_M1=1; + TBmotor_M2=0; + TBmotor_M3=0; + }else if (TBmotorSteps==4000){ + TBmotor_M1=0; + TBmotor_M2=0; + TBmotor_M3=0; + } + TBmotorRst=0; +} +//**************************************** +void dcSetting(){ + if ((speedFromPick==0)&&(encoder==false)){ + DcEncoder.rise(&sd25Fall); + } + if (encoder==true){ + DcEncoder.rise(&encoRise); + //ElementPosition.fall(&encoRise); + } +} //******************************************************* void allarmi(){ uint8_t alarmLowRegister1=0x00; @@ -317,7 +383,9 @@ alarmLowRegister=alarmLowRegister+(all_overCurrDC*0x10); // fatto alarmLowRegister=alarmLowRegister+(all_stopSistem*0x20); // verificarne la necessità //alarmLowRegister=alarmLowRegister+(all_upElements*0x40); // manca il sensore - //alarmLowRegister=alarmLowRegister+(all_noSeedOnCe*0x80); // manca il sensore + if (seedSensorEnable==true){ + alarmLowRegister=alarmLowRegister+(all_noSeedOnCe*0x80); // manca il sensore + } //alarmLowRegister1=alarmLowRegister1+(all_cfgnErrors*0x01); // da scrivere alarmLowRegister1=alarmLowRegister1+(all_noDcRotati*0x02); // fatto @@ -367,11 +435,12 @@ #if defined(speedMaster) void upDateSincro(){ char val1[8]={0,0,0,0,0,0,0,0}; - 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[3]=(posForQuinc /0x00FF0000)&0x000000FF; + val1[2]=(posForQuinc /0x0000FF00)&0x000000FF; + val1[1]=(posForQuinc /0x000000FF)&0x000000FF; + val1[0]=posForQuinc & 0x000000FF; + //double pass = tractorSpeed_MtS_timed*100.0f; + double pass = speedOfSeedWheel*100.0f; val1[4]=(uint8_t)(pass)&0x000000FF; val1[5]=(prePosSD /0x0000FF00)&0x000000FF; val1[6]=(prePosSD /0x000000FF)&0x000000FF; @@ -421,7 +490,7 @@ val1[3]=0; val1[3]=val1[3]+(tractorSpeedRead*0x01); val1[3]=val1[3]+(TBzeroPinInput*0x02); - val1[3]=val1[3]+(ElementPosition*0x04); + val1[3]=val1[3]+(DcEncoder*0x04); val1[3]=val1[3]+(seedWheelZeroPinInput*0x08); #if defined(simulaPerCan) if (buttonUser==0){ @@ -458,21 +527,23 @@ if (firstStart==1){ #if defined(speedMaster) sincUpdate.attach(&upDateSincro,0.009f); + canUpdate.attach(&upDateSpeed,0.21f); + #else + canUpdate.attach(&upDateSpeed,0.407f); #endif - canUpdate.attach(&upDateSpeed,0.21f); firstStart=0; } if (rxMsg.id==RX_ID){ #if defined(pcSerial) - #if defined(canDataReceived) + #if defined(canDataReceiveda) pc.printf("Messaggio ricevuto\n"); #endif #endif } if (rxMsg.id==RX_Broadcast){ #if defined(pcSerial) - #if defined(canDataReceived) + #if defined(canDataReceivedb) pc.printf("BroadCast ricevuto\n"); #endif #endif @@ -483,7 +554,7 @@ comandiDaCan = rxMsg.data[4]; #if defined(pcSerial) #if defined(canDataReceived) - pc.printf("Comandi: %x\n",comandiDaCan); + pc.printf("Speed simula %d \n",speedSimula); #endif #endif switch (comandiDaCan){ @@ -504,7 +575,7 @@ break; } #if defined(pcSerial) - #if defined(canDataReceived) + #if defined(canDataReceivedR) pc.printf("Comandi: %x\n",resetComandi); #endif #endif @@ -562,12 +633,6 @@ speedWheelDiameter = ((rxMsg.data[6]*0xFF)+rxMsg.data[5])/10000.0f; // variable for tractor speed calculation (need to be set from UI) ( Unit= meters ) speedWheelPulse = rxMsg.data[7]; // variable which define the number of pulse each turn of tractor speed wheel (need to be set from UI) aggiornaParametri(); - #if defined(pcSerial) - #if defined(canDataReceived) - pc.printf("Seed wheel diameter %f \n",seedWheelDiameter); - pc.printf("Speed wheel diameter %f \n",speedWheelDiameter); - #endif - #endif } } if (rxMsg.id==RX_AngoloPh){ @@ -638,10 +703,10 @@ } } if (rxMsg.id==RX_QuincSinc){ - masterSinc = (uint32_t) rxMsg.data[0] * 0x00FF0000; - masterSinc = masterSinc + ((uint32_t) rxMsg.data[1] * 0x0000FF00); - masterSinc = masterSinc + ((uint32_t) rxMsg.data[2] * 0x000000FF); - masterSinc = masterSinc + ((uint32_t) rxMsg.data[3]); + masterSinc = (uint32_t) rxMsg.data[3] * 0x00FF0000; + masterSinc = masterSinc + ((uint32_t) rxMsg.data[2] * 0x0000FF00); + masterSinc = masterSinc + ((uint32_t) rxMsg.data[1] * 0x000000FF); + masterSinc = masterSinc + ((uint32_t) rxMsg.data[0]); 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); @@ -652,11 +717,20 @@ uint8_t flags = rxMsg.data[0]; uint16_t steps = (uint32_t) rxMsg.data[1]*0xFF00; steps = steps + ((uint32_t)rxMsg.data[2]); + stepSetting(); cellsCountSet = rxMsg.data[3]; if ((flags&0x01)==0x01){ - encoder=true; + if (encoder==false){ + encoder=true; + DcEncoder.rise(NULL); + dcSetting(); + } }else{ - encoder=false; + if (encoder==true){ + encoder=false; + DcEncoder.rise(NULL); + dcSetting(); + } } if ((flags&0x02)==0x02){ tankLevelEnable=true; @@ -760,7 +834,18 @@ //******************************************************* void DC_prepare(){ // direction or brake preparation - if (DC_brake==1){SDmotorInA=1;SDmotorInB=1;}else{if (DC_forward==1){SDmotorInA=1;SDmotorInB=0;}else{SDmotorInA=0;SDmotorInB=1;}} + if (DC_brake==1){ + SDmotorInA=1; + SDmotorInB=1; + }else{ + if (DC_forward==0){ + SDmotorInA=1; + SDmotorInB=0; + }else{ + SDmotorInA=0; + SDmotorInB=1; + } + } // fault reading if (SDmotorInA==1){SD_faultA=1;}else{SD_faultA=0;} if (SDmotorInB==1){SD_faultB=1;}else{SD_faultB=0;} @@ -813,7 +898,7 @@ } #endif //**************************************************************************************** - if (quincCnt>=6){ + if (quincCnt>=4){ if (countPicks==0){ if ((sincroQui==1)&&(quincStart==0)){ // decelera @@ -824,26 +909,25 @@ countPicks=2; } } - 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); + percento -= ((double)quincPIDminus/100.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 (scostamento <15.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); + percento += ((double)quincPIDplus/100.0f)*(scostPerc); #if defined(pcSerial) #if defined(laq) pc.printf( @@ -851,18 +935,25 @@ #endif #endif } - if (scostamento <10.0f){percento=0.0f;} + //if (scostamento <15.0f){percento=0.0f;} } sincroQui=0; quincStart=0; countPicks=0; + #if !defined(speedMaster) + if (quincCnt>=5){ + if (speedFromMaster>0.0f){ + tractorSpeed_MtS_timed = speedFromMaster + percento; + } + } + #endif } //******************************************************************* 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 parametro = SDsectorStep/3.0f; double differenza=0.0f; #if defined(mezzo) if (quinconceActive==1){ @@ -875,7 +966,7 @@ #endif if ((differenza > 0.0f)&&(differenza < parametro)){ double diffPerc = differenza / parametro; - percento += ((double)quincPIDplus/1000.0f)*abs(diffPerc); + percento += ((double)quincPIDplus/100.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); @@ -884,13 +975,21 @@ } if ((differenza < 0.0f)&&(abs(differenza) < parametro)){ double diffPerc = (double)differenza / parametro; - percento -= ((double)quincPIDminus/1000.0f)*abs(diffPerc); + percento -= ((double)quincPIDminus/100.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 !defined(speedMaster) + if (quincCnt>=5){ + if (speedFromMaster>0.0f){ + tractorSpeed_MtS_timed = speedFromMaster + percento; + } + } + #endif } } @@ -902,6 +1001,13 @@ percento=((double)quincLIMminus*-1.0f)/100.0f; } } +// ---------------------------------------- +#if defined(seedSensor) + void resetDelay(){ + delaySeedCheck.reset(); + delaySeedCheck.stop(); + } +#endif // ------------------------------------------------------------------------------------------------------------------------------------------------------------------ // MAIN SECTION // --------------------------------------------------------------------------------------------------------------------------------------------------------------- @@ -910,57 +1016,13 @@ { //wait(1.0f); wd.Configure(2); //watchdog set at xx seconds - + + stepSetting(); + for (int a=0; a<5;a++){ mediaSpeed[a]=0; } - // Stepper driver init and set - TBmotorRst=0; // reset stepper driver - TBmotorDirecti=0; // reset stepper direction - // M1 M2 M3 RESOLUTION - // 0 0 0 1/2 - // 1 0 0 1/8 - // 0 1 0 1/16 - // 1 1 0 1/32 - // 0 0 1 1/64 - // 1 0 1 1/128 - // 0 1 1 1/10 - // 1 1 1 1/20 - if (TBmotorSteps==400){ - TBmotor_M1=0; - TBmotor_M2=0; - TBmotor_M3=0; - }else if (TBmotorSteps==1600){ - TBmotor_M1=1; - TBmotor_M2=0; - TBmotor_M3=0; - }else if (TBmotorSteps==3200){ - TBmotor_M1=0; - TBmotor_M2=1; - TBmotor_M3=0; - }else if (TBmotorSteps==6400){ - TBmotor_M1=1; - TBmotor_M2=1; - TBmotor_M3=0; - }else if (TBmotorSteps==12800){ - TBmotor_M1=0; - TBmotor_M2=0; - TBmotor_M3=1; - }else if (TBmotorSteps==25600){ - TBmotor_M1=1; - TBmotor_M2=0; - TBmotor_M3=1; - }else if (TBmotorSteps==2000){ - TBmotor_M1=0; - TBmotor_M2=1; - TBmotor_M3=1; - }else if (TBmotorSteps==4000){ - TBmotor_M1=1; - TBmotor_M2=1; - TBmotor_M3=1; - } - TBmotorRst=1; // DC reset ad set int decima = 100; @@ -977,9 +1039,6 @@ DC_prepare(); speedTimer.start(); // speed pulse timer - if (encoder==true){ - intraEncoTimer.start(); - } intraPickTimer.start(); speedTimeOut.start(); speedFilter.start(); @@ -990,6 +1049,7 @@ metalTimer.start(); quincTime.start(); quincTimeSD.start(); + //intraEncoTimer.start(); //******************************************************* // controls for check DC motor current @@ -1005,13 +1065,10 @@ 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)&&(encoder==false)){ - ElementPosition.rise(&sd25Fall); - } - if (encoder==true){ - ElementPosition.rise(&encoRise); - //ElementPosition.fall(&encoRise); - } + dcSetting(); + #if defined(seedSensor) + seedCheck.rise(&seedSensorTask); + #endif }else{ tftUpdate.attach(&videoUpdate,0.125f); } @@ -1019,12 +1076,12 @@ aggiornaParametri(); SDmotorPWM.period_us(periodoSD); // frequency 1KHz pilotaggio motore DC - SDmotorPWM.write(0.0f); // duty cycle = stop - TBmotorDirecti=0; // tb motor direction set + SDmotorPWM.write(1.0f); // duty cycle = stop + TBmotorDirecti=1; // tb motor direction set #if defined(provaStepper) - TBmotorRst=1; - TBticker.attach_us(&step_TBPulseOut,3000.0f); // clock time are seconds and attach seed motor stepper controls + TBmotorRst=0; + TBticker.attach_us(&step_TBPulseOut,1000.0f); // clock time are seconds and attach seed motor stepper controls #endif // end prova stepper #if defined(canbusActive) @@ -1054,14 +1111,20 @@ // se quinconce attivo ed unita' master invia segnale di sincro #if defined(speedMaster) if (seedWheelZeroPinInput==1){ - quinconceOut=1; + quinconceOut=0; } if (((double)(prePosSD-500) >= (SDsectorStep/((double)quincSector)))&&(quinconceOut=1)){ - quinconceOut=0; - if (quinconceActive==1){ + quinconceOut=1; + } + if (quinconceActive==1){ + if ((quinconceOut==1)&&(oldQuinconceOut==1)){ posForQuinc=500; - } - } + oldQuinconceOut=0; + } + if (((double)posForQuinc-500.0f)> (SDsectorStep-200)){ + oldQuinconceOut=1; + } + } #endif #if defined(overWriteCanSimulation) @@ -1079,6 +1142,11 @@ } if (avviaSimula==1){ if(oldSimulaSpeed!=pulseSpeedInterval){ + #if defined(pcSerial) + #if defined(canDataReceived) + pc.printf("Pulseinterval %f \n",pulseSpeedInterval); + #endif + #endif spedSimclock.attach_us(&speedSimulationClock,pulseSpeedInterval); oldSimulaSpeed=pulseSpeedInterval; } @@ -1120,9 +1188,18 @@ //************************************************************************************************************** if (inProva==0){ - if ((startCycleSimulation==0)&&(enableSimula==0)){zeroRequestBuf=1;runRequestBuf=0;enableCycle=1; - }else{zeroRequestBuf=1;runRequestBuf=0;enableCycle=1;} - if ((tractorSpeedRead==0)&&(speedFilter.read_ms()>3)){oldTractorSpeedRead=0;} + if ((startCycleSimulation==0)&&(enableSimula==0)){ + zeroRequestBuf=1; + runRequestBuf=0; + enableCycle=1; + }else{ + zeroRequestBuf=1; + runRequestBuf=0; + enableCycle=1; + } + if ((tractorSpeedRead==0)&&(speedFilter.read_ms()>3)){ + oldTractorSpeedRead=0; + } // ---------------------------------------- // filtra il segnale dei becchi per misurare il tempo di intervallo tra loro // ---------------------------------------- @@ -1209,31 +1286,20 @@ #endif #endif } + if (encoder==false){ + pulseRised2=1; + } #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.5f; if((double)speedTimeOut.read_us()> (oldLastPr)){ if ((firstStart==0)&&(simOk==0)&&(enableSimula==0)&&(zeroCycleEnd==1)){ all_speedError =1; } - #if defined(pcSerial) - #if defined(canDataReceived) - pc.printf("allarme errore speed sensor"); - #endif - #endif - } #endif //******************************************* @@ -1246,10 +1312,7 @@ semiPeriodoReale = (1000000.0f/frequenzaReale); tempoTraBecchi_mS = 0.0f; seedWheelRPM = (speedOfSeedWheel)*K_WheelRPM ; // calcola i giri al minuto della ruota di semina 7.37 31,75 - rapportoRuote = pickNumber/cellsNumber; // 0.67 calcola il rapporto tra il numero di becchi ed il numero di celle 0.8 1,00 TBrpm = seedWheelRPM*rapportoRuote; // 5.896 31,75 - TBgiroStep = TBmotorSteps*TBreductionRatio; - K_TBfrequency = TBgiroStep/60.0f; // 1600 * 1.65625f /60 = 44 44,00 TBfrequency = (TBrpm*K_TBfrequency); // 130Hz a 0,29Mts 1397,00 a 1,25mt/s con 15 becchi e 15 celle TBperiod=1000000.0f/TBfrequency; // 715uS } @@ -1311,42 +1374,45 @@ trigSD=0; } // torna indietro per sbloccare il tamburo - if ((TBmotorDirecti==1)&&(erroreTamburo==1)){ + if ((TBmotorDirecti==0)&&(erroreTamburo==1)){ cntCellsForReload++; if (cntCellsForReload >= cellsCountSet){ - TBmotorDirecti=0; + TBmotorDirecti=1; erroreTamburo=0; } } + #if defined(seedSensor) + resetDelay(); + delaySeedCheck.start(); + #endif } if ((double)TBactualPosition > ((TBgiroStep/cellsNumber)*1.2f)){ if (firstStart==0){ if (cntTbError>2){ all_cellSignal=1; + #if defined(seedSensor) + resetDelay(); + #endif } } if (erroreTamburo==0){ erroreTamburo=1; - TBmotorDirecti=1; + TBmotorDirecti=0; cntCellsForReload=0; cntTbError++; + #if defined(seedSensor) + resetDelay(); + #endif } - #if defined(pcSerial) - #if defined(canDataReceived) - pc.printf("allarme error cells"); - #endif - #endif } if (((double)TBactualPosition > ((TBgiroStep/cellsNumber)*3.0f))||(cntTbError>4)){ if (firstStart==0){ all_noStepRota=1; + #if defined(seedSensor) + resetDelay(); + #endif } cntTbError=0; - #if defined(pcSerial) - #if defined(canDataReceived) - pc.printf("allarme no cells sensor"); - #endif - #endif } // ---------------------------------------- // read and manage joystick @@ -1404,14 +1470,16 @@ // 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)||(speedFromMaster==0.0f)){ + 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 + #endif*/ if (checkSDrotation==0){ checkSDrotation=1; SDwheelTimer.start(); @@ -1423,21 +1491,35 @@ double oldLastPr = (double)oldLastPulseRead*1.7f; if((double)speedTimeOut.read_us()> (oldLastPr)){ tractorSpeed_MtS_timed = 0.0f; + #if defined(seedSensor) + resetDelay(); + #endif pntMedia=0; speedTimeOut.reset(); enableSpeed=0; quincCnt=0; } } + + #if defined(seedSensor) + if (seedSensorEnable==true){ + if (delaySeedCheck.read_ms()>100){ + if (seedSee==0){ + all_noSeedOnCe=1; + } + resetDelay(); + } + } + #endif // esegue il controllo di velocità minima - if ((double)speedTimer.read_ms()>=maxInterval){ + /*if ((double)speedTimer.read_ms()>=maxInterval){ tractorSpeed_MtS_timed = 0.0f; enableSpeed=0; - } + }*/ // esegue il controllo di velocità massima - if ((double)speedTimer.read_ms()<=minIntervalPulse){ + /*if ((double)speedTimer.read_ms()<=minIntervalPulse){ tractorSpeed_MtS_timed = 4.5f; - } + }*/ //*************************************************************************************************************** // cycle logic control section //*************************************************************************************************************** @@ -1496,16 +1578,16 @@ k6=20.50f; }else{*/ k3=0.050f; - k4=2.103f; - k5=20.00f; - k6=30.50f; + k4=1.103f; + k5=10.00f; + k6=20.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 L1 = 0.045f; + double L_1=-0.045f; + double L2 = 0.150f; + double L_2=-0.150f; + double L3 = 0.301f; + double L_3=-0.301f; double k1=0.0f; if ((errorePercentuale > L3)||(errorePercentuale < L_3)){ k1=errorePercentuale*k6; @@ -1578,10 +1660,10 @@ }else{ dcActualDuty = dutyTeorico; } - if (dcActualDuty <=0.0f){dcActualDuty=0.10f;} - if (dcActualDuty > 0.90f){dcActualDuty = 0.90f;}//dcMaxSpeed;} + if (dcActualDuty <=0.0f){dcActualDuty=0.05f;} + if (dcActualDuty > 0.95f){dcActualDuty = 0.95f;}//dcMaxSpeed;} if (olddcActualDuty!=dcActualDuty){ - SDmotorPWM.write(dcActualDuty); + SDmotorPWM.write(1.0f-dcActualDuty); olddcActualDuty=dcActualDuty; } // allarme @@ -1648,6 +1730,9 @@ }else{ // fine ciclo con velocita maggiore di 0 SDwheelTimer.stop(); SDwheelTimer.reset(); + #if defined(seedSensor) + resetDelay(); + #endif checkSDrotation=0; oldFaseLavoro=0; aspettaStart=1; @@ -1694,7 +1779,7 @@ avvicinamento=1; avvicinamentoOn.reset(); avvicinamentoOn.start(); - SDmotorPWM.write(0.32f); // duty cycle = 60% of power + SDmotorPWM.write(1.0f-0.32f); // duty cycle = 60% of power DC_forward=1; DC_brake=0; DC_prepare(); @@ -1703,13 +1788,13 @@ } if (avvicinamento==1){ if(avvicinamentoOn.read_ms()>300){ - SDmotorPWM.write(0.7f); + SDmotorPWM.write(1.0f-0.7f); avvicinamentoOn.reset(); avvicinamentoOff.reset(); avvicinamentoOff.start(); } if(avvicinamentoOff.read_ms()>100){ - SDmotorPWM.write(0.32f); + SDmotorPWM.write(1.0f-0.32f); avvicinamentoOff.reset(); avvicinamentoOff.stop(); avvicinamentoOn.start(); @@ -1731,7 +1816,28 @@ //ritardoComandoStop.stop(); } }else{ - if (quinconceActive==0){ + #if defined(mezzo) + if (quinconceActive==0){ + if (SDzeroCyclePulse==1){ + avvicinamento=0; + DC_brake=1; + DC_prepare(); + SDzeroed=1; + ritardoStop.reset(); + ritardoStop.stop(); + } + }else{ + if (quincTimeSet.read_ms()>700){ + avvicinamento=0; + DC_brake=1; + DC_prepare(); + SDzeroed=1; + ritardoStop.reset(); + ritardoStop.stop(); + quincTimeSet.stop(); + } + } + #else if (SDzeroCyclePulse==1){ avvicinamento=0; DC_brake=1; @@ -1740,25 +1846,10 @@ ritardoStop.reset(); ritardoStop.stop(); } - }else{ - if (quincTimeSet.read_ms()>500){ - avvicinamento=0; - DC_brake=1; - DC_prepare(); - SDzeroed=1; - ritardoStop.reset(); - ritardoStop.stop(); - quincTimeSet.stop(); - } - } + #endif } // azzera tutto in time out if (timeOutZeroSD.read_ms()>=10000){ - #if defined(pcSerial) - #if defined(canDataReceived) - pc.printf("allarme no zero"); - #endif - #endif if (firstStart==0){ all_no_Zeroing=1; } @@ -1887,8 +1978,8 @@ DC_brake=0; DC_forward=1; duty_DC+=0.01f; - if (duty_DC>=0.2f){ - duty_DC=0.2f; + if (duty_DC>=1.0f){ + duty_DC=1.0f; clocca=1; } wait_ms(rampa); @@ -1943,7 +2034,7 @@ break; } if (oldDuty_DC != duty_DC){ - SDmotorPWM.write(duty_DC); // duty cycle = stop + SDmotorPWM.write(1.0f-duty_DC); // duty cycle = stop oldDuty_DC=duty_DC; DC_prepare(); }