Francesco Pistone
new
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();
}