Forigo
Messa in campo 4 file - 26/06/2020 Francia
Dependencies: mbed X_NUCLEO_IHM03A1_for
Fork of
FORIGO_Modula_V7_3_VdcStep_maggio2020
by 
Francesco Pistone
Diff: main.cpp
- Revision:
- 5:2a3a64b52f54
- Parent:
- 4:de1b296e9757
- Child:
- 6:e8c18f0f399a
--- a/main.cpp Wed Feb 13 07:29:30 2019 +0000
+++ b/main.cpp Sun Feb 17 08:10:57 2019 +0000
@@ -246,6 +246,10 @@
//*******************************************************
// interrupt task for tractor speed reading
//*******************************************************
+void tractorReadSpeedOff(){
+ speedClock=0;
+ oldTractorSpeedRead=0;
+}
void tractorReadSpeed(){
if ((oldTractorSpeedRead==0)){
lastPulseRead=speedTimer.read_us();
@@ -253,10 +257,10 @@
speedTimer.reset();
pulseRised=1;
oldTractorSpeedRead=1;
- //spazioCoperto+= pulseDistance;
+ speedClock=1;
+
}
speedFilter.reset();
- speedClock=1;
}
//*******************************************************
void speedMediaCalc(){
@@ -400,7 +404,6 @@
if (inhibit==0){
double posError =0.0f;
double posSD=((double)SDactualPosition)/KcorT;
- //double posSD=((double)SDactualPosition);
posError = posSD - (double)TBactualPosition;
// interviene sulla velocità di TB per raggiungere la corretta posizione relativa
if((lowSpeed==0)&&(aspettaStart==0)){
@@ -719,7 +722,6 @@
#endif
firstStart=0;
}
-
if (rxMsg.id==RX_ID){
#if defined(pcSerial)
#if defined(canDataReceiveda)
@@ -815,8 +817,8 @@
pickNumber = rxMsg.data[0]; // numero di becchi installati sulla ruota di semina
cellsNumber = rxMsg.data[1]; // numero di celle del tamburo
deepOfSeed=(rxMsg.data[2]/10000.0f); // deep of seeding
- seedWheelDiameter = ((rxMsg.data[4]*0xFF)+rxMsg.data[3])/10000.0f; // seed wheel diameter setting
- speedWheelDiameter = ((rxMsg.data[6]*0xFF)+rxMsg.data[5])/10000.0f; // variable for tractor speed calculation (need to be set from UI) ( Unit= meters )
+ seedWheelDiameter = ((rxMsg.data[4]*0x100)+rxMsg.data[3])/10000.0f; // seed wheel diameter setting
+ speedWheelDiameter = ((rxMsg.data[6]*0x100)+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();
}
@@ -1128,6 +1130,7 @@
sincroQui=0;
quincStart=0;
countPicks=0;
+ // questo e il primo
#if !defined(speedMaster)
if (quincCnt>=3){
if (speedFromMaster>0.0f){
@@ -1172,15 +1175,14 @@
#endif
#endif
}
-
+ // questo e il secondo
#if !defined(speedMaster)
if (quincCnt>=3){
- if (speedFromMaster>0.0f){
- //Questo
- if (enableSimula==0){
- tractorSpeed_MtS_timed = speedFromMaster + percento;
- }
+ if (speedFromMaster>0.0f){
+ if (enableSimula==0){
+ tractorSpeed_MtS_timed = speedFromMaster + percento;
}
+ }
}
#endif
}
@@ -1267,15 +1269,16 @@
/* Attaching and enabling an interrupt handler. */
motor->attach_flag_irq(&my_flag_irq_handler);
motor->enable_flag_irq();
- //motor->disable_flag_irq();
+ motor->disable_flag_irq();
/* Attaching an error handler */
- motor->attach_error_handler(&my_error_handler);
+ //motor->attach_error_handler(&my_error_handler);
//thread.start(step_Reading);
if (inProva==0){
tractorSpeedRead.rise(&tractorReadSpeed);
+ tractorSpeedRead.fall(&tractorReadSpeedOff);
#if !defined(speedMaster)
quinconceIn.rise(&quincTrigon);
quinconceIn.fall(&quincTrigof);
@@ -1324,8 +1327,14 @@
//**************************************************************************************************************
while (true){
// ripetitore segnale di velocità
- if (tractorSpeedRead==0){speedClock=0;}
-
+ #if defined(clockSpeedOut)
+ clocca++;
+ if (clocca >= 10){
+ speedClock = !speedClock;
+ clocca=0;
+ }
+ #endif
+
// inversione segnali ingressi
#if !defined(simulaBanco)
seedWheelZeroPinInput = !seedWheelZeroPinInputRev;
@@ -1338,7 +1347,7 @@
}
#endif
TBzeroPinInput = !TBzeroPinInputRev;
-
+
// se quinconce attivo ed unita' master invia segnale di sincro
#if defined(speedMaster)
if (seedWheelZeroPinInput==1){
@@ -1358,10 +1367,6 @@
}
#endif
- #if defined(overWriteCanSimulation)
- leggiCAN();
- #endif
-
// simulazione velocita
if (enableSimula==1){
double TMT = 0.0f;
@@ -1373,11 +1378,6 @@
}
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;
}
@@ -1388,7 +1388,7 @@
}else{
spedSimclock.detach();
}
-
+
//*******************************************************
// determina se sono in bassa velocità per il controllo di TB
if (speedOfSeedWheel<=minSeedSpeed){
@@ -1410,7 +1410,6 @@
}else{
lowSpeed=0;
}
-
//**************************************************************************************************************
//**************************************************************************************************************
@@ -1428,9 +1427,6 @@
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
// ----------------------------------------
@@ -1692,7 +1688,7 @@
if (oldZeroCycle!=zeroRequestBuf){
zeroCycle=1;
zeroCycleEnd=0;
- SDzeroed=0;
+ SDzeroed=1;
TBzeroed=0;
zeroTrigger=0;
oldZeroCycle = zeroRequestBuf;
@@ -1719,13 +1715,6 @@
if((quincCnt<3)||(speedFromMaster==0.0f)||(enableSimula==1)){
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();
@@ -1771,6 +1760,11 @@
//***************************************************************************************************************
if (enableSimula==1){if(simOk==0){tractorSpeed_MtS_timed=0.0f;}}
if ((tractorSpeed_MtS_timed>0.01f)){
+ #if defined(pcSerial)
+ #if defined(Qncc)
+ pc.printf("TsP: %f SpW: %f InPic: %f EPerc: %f Duty:%f \n",tractorSpeed_MtS_timed,speedOfSeedWheel,timeIntraPick, errorePercentuale, dcActualDuty);
+ #endif
+ #endif
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)
@@ -1998,6 +1992,8 @@
OLDpulseSpeedInterval=1000.01f;
cicloTbinCorso=0;
cntTbError=0;
+ olddcActualDuty=0.0f;
+ speedOfSeedWheel=0.0f;
if (cycleStopRequest==1){
zeroDelay.reset();
zeroDelay.start();
@@ -2123,7 +2119,7 @@
ritardoComandoStop.stop();
timeOutZeroSD.stop();
timeOutZeroSD.reset();
- noSDzeroRequest=0;
+ //noSDzeroRequest=0;
}
if (TBzeroCyclePulse==1){
TBticker.detach();
@@ -2141,13 +2137,14 @@
runRequestBuf=1;
zeroRequestBuf=0;
cycleStopRequest=0;
- SDzeroed=0;
+ SDzeroed=1;
TBzeroed=0;
timeOutZeroSD.stop();
timeOutZeroSD.reset();
DC_brake=1;
DC_prepare();
SDzeroed=1;
+ noSDzeroRequest=0;
}
}
@@ -2161,146 +2158,7 @@
SDzeroCyclePulse=0;
TBzeroCyclePulse=0;
//*************************************************************************************************
- }else{//end ciclo normale
-//*************************************************************************************************
-// task di prova della scheda
-//*************************************************************************************************
- #if defined(provaScheda)
- clocca++;
- //led = !led;
- //txMsg.clear();
- //txMsg << clocca;
- //test.printf("aogs \n");
- //if(can1.write(txMsg)){
- #if defined(pcSerial)
- pc.printf("Can write OK \n");
- #endif
- //}
- switch (clocca){
- case 1:
- TBmotorStepOut=1; // define step command for up down motor driver
- break;
- case 2:
- SDmotorPWM=1; // define step command for seeding whell motor driver
- break;
- case 3:
- speedClock=1; // define input of
- break;
- case 4:
- break;
- case 5:
- SDmotorInA=1;
- SDmotorInB=0;
- break;
- case 6:
- break;
- case 7:
- break;
- case 8:
- break;
- case 9:
- break;
- case 10:
- break;
- case 11:
- break;
- case 12:
- break;
- case 13:
- break;
- case 14:
- SDmotorPWM=1; // power mosfet 2 command out
- break;
- case 15:
- break;
- case 16:
- case 17:
- break;
- case 18:
- TBmotorStepOut=0; // define step command for up down motor driver
- SDmotorPWM=0; // define step command for seeding whell motor driver
- speedClock=0; // define input of
- SDmotorInA=0;
- SDmotorInB=0;
- SDmotorPWM=0; // power mosfet 2 command out
- break;
- default:
- clocca=0;
- break;
- }
- wait_ms(100);
- #endif // end prova scheda
-
- #if defined(provaDC)
- int rampa=1000;
- int pausa=3000;
- switch (clocca){
- case 0:
- DC_brake=0;
- DC_forward=1;
- duty_DC+=0.01f;
- if (duty_DC>=1.0f){
- duty_DC=1.0f;
- clocca=1;
- }
- wait_ms(rampa);
- break;
- case 1:
- wait_ms(pausa*4);
- clocca=2;
- break;
- case 2:
- DC_brake=0;
- DC_forward=1;
- duty_DC-=0.01f;
- if (duty_DC<=0.0f){
- duty_DC=0.0f;
- clocca=3;
- }
- wait_ms(rampa);
- break;
- case 3:
- wait_ms(pausa);
- clocca=4;
- break;
- case 4:
- DC_brake=0;
- DC_forward=1;
- duty_DC+=0.01f;
- if (duty_DC>=1.0f){
- duty_DC=1.0f;
- clocca=5;
- }
- wait_ms(rampa);
- break;
- case 5:
- wait_ms(pausa);
- clocca=6;
- break;
- case 6:
- DC_brake=0;
- DC_forward=1;
- duty_DC-=0.01f;
- if (duty_DC<=0.0f){
- duty_DC=0.0f;
- clocca=7;
- }
- wait_ms(rampa);
- break;
- case 7:
- wait_ms(pausa);
- clocca=0;
- break;
- default:
- break;
- }
- if (oldDuty_DC != duty_DC){
- SDmotorPWM.write(1.0f-duty_DC); // duty cycle = stop
- oldDuty_DC=duty_DC;
- DC_prepare();
- }
- #endif
- }//end in prova
+ }
wd.Service(); // kick the dog before the timeout
} // end while
} // end main