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:
- 6:e8c18f0f399a
- Parent:
- 5:2a3a64b52f54
- Child:
- 8:310f9e4eac7b
--- a/main.cpp Sun Feb 17 08:10:57 2019 +0000
+++ b/main.cpp Tue Feb 19 16:58:02 2019 +0000
@@ -24,6 +24,7 @@
// 09 06 2018 - INSERITO CONTROLLO DI FASE CON ENCODER MASTER PER QUINCONCE - DATO SCAMBIATO IN CAN
// 03 01 2019 - INSERITA GESTIONE IN RTOS PER IL DRIVER POWERSTEP01
// 10 02 2019 - INSERITO FUNZIONAMENTO STEPPER IN MODALITA' CONTROLLO DI TENSIONE E STEP DA CLOCKPIN
+// 16 02 2019 - SOSTITUITA LIBRERIA MBED PER PROBLEMI DI COMPILAZIONE DEL FIRMWARE
/********************
IL FIRMWARE SI COMPONE DI 10 FILES:
- main.cpp
@@ -94,7 +95,6 @@
//********************************************************************************************************************
//********************************************************************************************************************
-//Thread thread;
/* Variables -----------------------------------------------------------------*/
@@ -182,26 +182,6 @@
}
//*******************************************************************************
-// FREE RUNNING RTOS THREAD FOR DRUM STEPPER POSITION READING
-//*******************************************************************************
-void step_Reading(){
- while(true){
- /* Get current position of device and print to the console */
- #if defined(pcSerial)
- #if defined(rtosData)
- printf(" Position: %d.\r\n", motor->get_position());
- #endif
- #endif
- TBpassPosition= (uint32_t) motor->get_position();
- if (TBpassPosition > TBoldPosition){
- //TBactualPosition= ((TBpassPosition-TBoldPosition)*TBreductionRatio);//*10;
- }else{
- //TBactualPosition=((((2097152-TBoldPosition)+TBpassPosition))*TBreductionRatio);//*10;
- }
- //wait_us(50); // 50 mS di intervallo lettura
- }
-}
-//*******************************************************************************
//********************************************************************************************************************
// ------------------------------------------------------------------------------------------------------------------------------------------------------------------
// TASK SECTION
@@ -217,6 +197,11 @@
if (speedFromPick==0){
speedOfSeedWheel=((seedPerimeter/25.0f)/(double)memo_TimeHole)*1000.0f; //mtS
}
+ #if defined(pcSerial)
+ #if defined(checkLoop)
+ pc.printf("1\n");
+ #endif
+ #endif
}
// rise of seed speed motor encoder
void encoRise(){
@@ -227,12 +212,22 @@
if (encoder==true){
speedOfSeedWheel=((seedPerimeter/((SDreductionRatio*25.5f)))/(double)memo_TimeHole)*1000000.0f; //mtS
pulseRised2=1;
- //SDactualPosition+=SDstepEnco;
}
+ #if defined(pcSerial)
+ #if defined(checkLoop)
+ pc.printf("2\n");
+ #endif
+ #endif
}
+//**************************************************
// rise of seed presence sensor
void seedSensorTask(){
seedSee=1;
+ #if defined(pcSerial)
+ #if defined(checkLoop)
+ pc.printf("3\n");
+ #endif
+ #endif
}
//**************************************************
// generate speed clock when speed is simulated from Tritecnica display
@@ -242,14 +237,15 @@
speedTimer.reset();
pulseRised=1;
speedFilter.reset();
+ #if defined(pcSerial)
+ #if defined(checkLoop)
+ pc.printf("4\n");
+ #endif
+ #endif
}
//*******************************************************
// interrupt task for tractor speed reading
//*******************************************************
-void tractorReadSpeedOff(){
- speedClock=0;
- oldTractorSpeedRead=0;
-}
void tractorReadSpeed(){
if ((oldTractorSpeedRead==0)){
lastPulseRead=speedTimer.read_us();
@@ -258,9 +254,13 @@
pulseRised=1;
oldTractorSpeedRead=1;
speedClock=1;
-
}
speedFilter.reset();
+ #if defined(pcSerial)
+ #if defined(checkLoop)
+ pc.printf("5\n");
+ #endif
+ #endif
}
//*******************************************************
void speedMediaCalc(){
@@ -272,6 +272,11 @@
}
mediaSpeed[0]=lastPd;
OLDpulseSpeedInterval=pulseSpeedInterval;
+ #if defined(pcSerial)
+ #if defined(checkLoop)
+ pc.printf("6\n");
+ #endif
+ #endif
}
//*******************************************************
@@ -284,6 +289,11 @@
#if defined(speedMaster)
posForQuinc++;
#endif
+ #if defined(pcSerial)
+ #if defined(checkLoop)
+ pc.printf("7\n");
+ #endif
+ #endif
}
//*******************************************************
void step_TBPulseOut(){
@@ -291,13 +301,23 @@
if (TBmotorStepOut==0){
if (TBmotorDirecti==TBforward){
TBactualPosition++;
- //}else{
- // TBactualPosition--;
}
}
+ #if defined(pcSerial)
+ #if defined(stepTamb)
+ pc.printf("step\n");
+ #endif
+ #endif
+ /*
+ #if defined(pcSerial)
+ #if defined(checkLoop)
+ pc.printf("8\n");
+ #endif
+ #endif
+ */
}
//*******************************************************
-void inverti(){
+void invertiLo(){
if (TBmotorDirecti==TBreverse){
TBmotorDirecti=TBforward;
motor->step_clock_mode_enable(StepperMotor::FWD);
@@ -311,6 +331,11 @@
pc.printf("posizione %d \n",TBactualPosition);
#endif
#endif
+ #if defined(pcSerial)
+ #if defined(checkLoop)
+ pc.printf("9\n");
+ #endif
+ #endif
}
//*******************************************************
// aggiornamento parametri di lavoro fissi e da Tritecnica
@@ -322,7 +347,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_percentuale = TBmotorSteps*TBreductionRatio;
SDsectorStep = (double)fixedStepGiroSD / (double)pickNumber;
TBsectorStep = (TBmotorSteps*TBreductionRatio)/cellsNumber;
KcorT = (SDsectorStep/TBsectorStep);///2.0f;
@@ -338,27 +362,45 @@
}else{
intraPickDistance = seedPerimeter/25.0f; // 25 è il numero di fori presenti nel disco di semina
}
- SDstepEnco = 9000/(25.5*SDreductionRatio);
+ #if defined(pcSerial)
+ #if defined(checkLoop)
+ pc.printf("10\n");
+ #endif
+ #endif
}
//*******************************************************
void cambiaTB(double perio){
// update TB frequency
double limite=500.0f;
double TBper=0.0f;
+ double scala =2.0f;
if (aspettaStart==0){
if (perio<limite){perio=limite;}
- double scala =0.0f;
- if (lowSpeed==1){
- scala =2.0f;
- }else{
- scala =2.0f;
- }
TBper=perio/scala;
if (oldPeriodoTB!=TBper){
if (TBper >= (limite/2.0f)){
- TBticker.attach_us(&step_TBPulseOut,TBper); // clock time are milliseconds and attach seed motor stepper controls
+ #if defined(pcSerial)
+ #if defined(checkLoop)
+ pc.printf("11a\n");
+ pc.printf("11a TBper: %f \n",TBper);
+ #endif
+ #endif
+ if (TBper != NULL){
+ motor->step_clock_mode_enable(StepperMotor::FWD);
+ TBticker.attach_us(&step_TBPulseOut,TBper); // clock time are milliseconds and attach seed motor stepper controls
+ }
}else{
+ #if defined(pcSerial)
+ #if defined(checkLoop)
+ pc.printf("11b\n");
+ #endif
+ #endif
TBticker.detach();
+ #if defined(pcSerial)
+ #if defined(loStop)
+ pc.printf("A1\n");
+ #endif
+ #endif
motor->soft_hiz();
}
oldPeriodoTB=TBper;
@@ -411,6 +453,11 @@
//if (posError<-50.0f){posError=-50.0f;}
if ((posError >=1.0f)||(posError<=-1.0f)){
ePpos = periodo *(1.0f+ ((posError/100.0f)));
+ #if defined(pcSerial)
+ #if defined(checkLoop)
+ pc.printf("da zucca\n");
+ #endif
+ #endif
if (ePpos>0.0f){
cambiaTB(ePpos);
}else{
@@ -449,17 +496,31 @@
if (posError<lowLim){posError=lowLim;}
if ((posError >=1.0f)||(posError<=-1.0f)){
ePpos = periodo /(1.0f+ ((posError/divide)));
- cambiaTB(ePpos);
- }
#if defined(pcSerial)
- #if defined(TBperS)
- pc.printf("TBpos: %f SDpos: %f SDact: %f Err: %f Correggi: %f\n",(double)TBactualPosition,posSD,(double)SDactualPosition,posError,ePpos);
+ #if defined(checkLoop)
+ pc.printf("12a ePpos:%f\n",ePpos);
#endif
#endif
+ if (ePpos>0.0f){
+ cambiaTB(ePpos);
+ }else{
+ cambiaTB(periodo);///2.0f);
+ }
+ }
+ #if defined(pcSerial)
+ #if defined(TBperS)
+ pc.printf("TBpos: %f SDpos: %f SDact: %f Err: %f Correggi: %f\n",(double)TBactualPosition,posSD,(double)SDactualPosition,posError,ePpos);
+ #endif
+ #endif
}
}
}
#endif
+ #if defined(pcSerial)
+ #if defined(checkLoopa)
+ pc.printf("12\n");
+ #endif
+ #endif
}
//*******************************************************
void videoUpdate(){
@@ -477,22 +538,52 @@
pc.printf(" Trigger: %d \n", trigRepos);
#endif
#endif
+ #if defined(pcSerial)
+ #if defined(checkLoop)
+ pc.printf("13\n");
+ #endif
+ #endif
}
//*******************************************************
void ciclaTB(){
if ((startCicloTB==1)&&(cicloTbinCorso==0)){
- motor->step_clock_mode_enable(StepperMotor::FWD);
- TBticker.attach_us(&step_TBPulseOut,TBperiod/2.5f); // clock time are milliseconds and attach seed motor stepper controls
- cicloTbinCorso = 1;
- startCicloTB=0;
+ #if defined(pcSerial)
+ #if defined(checkLoop)
+ pc.printf("14a TBperiod: %f\n",TBperiod);
+ #endif
+ #endif
+ if (TBperiod >= (250.0f*2.5f)){
+ if (TBperiod != NULL){
+ motor->step_clock_mode_enable(StepperMotor::FWD);
+ TBticker.attach_us(&step_TBPulseOut,TBperiod/2.5f); // clock time are milliseconds and attach seed motor stepper controls
+ }
+ cicloTbinCorso = 1;
+ startCicloTB=0;
+ }
}
if ((loadDaCan==1)&&(loadDaCanInCorso==0)){
+ #if defined(pcSerial)
+ #if defined(checkLoop)
+ pc.printf("14b\n");
+ #endif
+ #endif
+ motor->step_clock_mode_enable(StepperMotor::FWD);
TBticker.attach_us(&step_TBPulseOut,1000.0f); // clock time are milliseconds and attach seed motor stepper controls
loadDaCanInCorso=1;
stopCicloTB=0;
}
if ((stopCicloTB==1)&&(TBactualPosition>5)){
+ #if defined(pcSerial)
+ #if defined(checkLoop)
+ pc.printf("14c\n");
+ #endif
+ #endif
TBticker.detach();
+ #if defined(pcSerial)
+ #if defined(loStop)
+ pc.printf("A2\n");
+ #endif
+ #endif
motor->soft_hiz();
cicloTbinCorso = 0;
stopCicloTB=0;
@@ -505,49 +596,12 @@
// Stepper driver init and set
TBmotorRst=0; // reset stepper driver
TBmotorDirecti=TBforward; // 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=1;
+ #if defined(pcSerial)
+ #if defined(checkLoop)
+ pc.printf("15\n");
+ #endif
+ #endif
}
//****************************************
void dcSetting(){
@@ -556,8 +610,12 @@
}
if (encoder==true){
DcEncoder.rise(&encoRise);
- //ElementPosition.fall(&encoRise);
}
+ #if defined(pcSerial)
+ #if defined(checkLoop)
+ pc.printf("16\n");
+ #endif
+ #endif
}
//*******************************************************
void allarmi(){
@@ -619,6 +677,11 @@
all_noSpeedSen=0;
all_no_Zeroing=0;
all_genericals=0;
+ #if defined(pcSerial)
+ #if defined(checkLoop)
+ pc.printf("17\n");
+ #endif
+ #endif
}
//*******************************************************
#if defined(speedMaster)
@@ -641,6 +704,11 @@
}
#endif
#endif
+ #if defined(pcSerial)
+ #if defined(checkLoop)
+ pc.printf("18\n");
+ #endif
+ #endif
}
#endif
//*******************************************************
@@ -708,6 +776,11 @@
checkState=0;
}
#endif
+ #if defined(pcSerial)
+ #if defined(checkLoop)
+ pc.printf("19\n");
+ #endif
+ #endif
}
//*******************************************************
void leggiCAN(){
@@ -906,7 +979,7 @@
uint16_t steps = (uint32_t) rxMsg.data[2]*0x00000100;
steps = steps + ((uint32_t)rxMsg.data[1]);
TBmotorSteps =steps;
- //stepSetting();
+ stepSetting();
cellsCountSet = rxMsg.data[3];
if ((flags&0x01)==0x01){
if (encoder==false){
@@ -960,7 +1033,11 @@
avviaSimula=1;
simOk=1;
#endif
-
+ #if defined(pcSerial)
+ #if defined(checkLoop)
+ pc.printf("20\n");
+ #endif
+ #endif
}
//*******************************************************
@@ -1019,6 +1096,11 @@
timeCurr.stop();
}
//}
+ #if defined(pcSerial)
+ #if defined(checkLoop)
+ pc.printf("21\n");
+ #endif
+ #endif
}
//*******************************************************
void DC_prepare(){
@@ -1038,19 +1120,43 @@
// fault reading
if (SDmotorInA==1){SD_faultA=1;}else{SD_faultA=0;}
if (SDmotorInB==1){SD_faultB=1;}else{SD_faultB=0;}
+ #if defined(pcSerial)
+ #if defined(checkLoopa)
+ pc.printf("22\n");
+ #endif
+ #endif
}
//*******************************************************
void startDelay(){
- int ritardo =0;
- ritardo = (int)((float)(dcActualDuty*800.0f));
- timeout.attach_us(&DC_CheckCurrent,ritardo);
+ if (currentCheckEnable==true){
+ int ritardo =0;
+ ritardo = (int)((float)(dcActualDuty*800.0f));
+ if (ritardo>250.0f){
+ timeout.attach_us(&DC_CheckCurrent,ritardo);
+ }
+ }
+ #if defined(pcSerial)
+ #if defined(checkLoop)
+ pc.printf("23\n");
+ #endif
+ #endif
}
//*******************************************************
void quincTrigon(){
quincClock=true;
+ #if defined(pcSerial)
+ #if defined(checkLoop)
+ pc.printf("24\n");
+ #endif
+ #endif
}
void quincTrigof(){
quincClock=false;
+ #if defined(pcSerial)
+ #if defined(checkLoop)
+ pc.printf("25\n");
+ #endif
+ #endif
}
//*******************************************************
void quinCalc(){
@@ -1195,12 +1301,22 @@
if ((percento) < (((double)quincLIMminus*-1.0f)/100.0f)){
percento=((double)quincLIMminus*-1.0f)/100.0f;
}
+ #if defined(pcSerial)
+ #if defined(checkLoop)
+ pc.printf("26\n");
+ #endif
+ #endif
}
// ----------------------------------------
#if defined(seedSensor)
void resetDelay(){
delaySeedCheck.reset();
delaySeedCheck.stop();
+ #if defined(pcSerial)
+ #if defined(checkLoop)
+ pc.printf("27\n");
+ #endif
+ #endif
}
#endif
// ------------------------------------------------------------------------------------------------------------------------------------------------------------------
@@ -1209,16 +1325,43 @@
//*******************************************************************************
int main(){
- //wait(1.0f);
- wd.Configure(2); //watchdog set at xx seconds
+ #if defined(pcSerial)
+ #if defined(resetCpu)
+ pc.printf("RESET\n");
+ #endif
+ #endif
+
+ wd.Configure(2.0); //watchdog set at xx seconds
stepSetting();
+ TBmotor_SW=1;
+ //----- Initialization
+ /* Initializing SPI bus. */
+ // dev_spi(mosi,miso,sclk)
+ // D11= PA7; D12= PA6; D13= PA5
+ DevSPI dev_spi(D11, D12, D13);
+
+ /* Initializing Motor Control Component. */
+ // powerstep01( flag, busy,stby, stck, cs, dev_spi)
+ // motor = new PowerStep01(D2, D4, D8, D9, D10, dev_spi); // linea standard per IHM03A1
+ motor = new PowerStep01(PA_8, PC_7, PC_4, PB_3, PB_6, dev_spi); // linea per scheda seminatrice V7
+ if (motor->init(&init) != COMPONENT_OK) {
+ exit(EXIT_FAILURE);
+ }
+
+ /* Attaching and enabling an interrupt handler. */
+ motor->attach_flag_irq(&my_flag_irq_handler);
+ motor->enable_flag_irq();
+ //motor->disable_flag_irq();
+
+ /* Attaching an error handler */
+ motor->attach_error_handler(&my_error_handler);
+
for (int a=0; a<5;a++){
mediaSpeed[a]=0;
}
-
// DC reset ad set
int decima = 100;
wait_ms(200);
@@ -1244,41 +1387,15 @@
metalTimer.start();
quincTime.start();
quincTimeSD.start();
- //intraEncoTimer.start();
//*******************************************************
// controls for check DC motor current
- pwmCheck.rise(&startDelay);
+ //pwmCheck.rise(&startDelay);
wait_ms(500);
- TBmotor_SW=1;
-//----- Initialization
- /* Initializing SPI bus. */
- // dev_spi(mosi,miso,sclk)
- // D11= PA7; D12= PA6; D13= PA5
- DevSPI dev_spi(D11, D12, D13);
-
- /* Initializing Motor Control Component. */
- // powerstep01( flag, busy,stby, stck, cs, dev_spi)
- // motor = new PowerStep01(D2, D4, D8, D9, D10, dev_spi); // linea standard per IHM03A1
- motor = new PowerStep01(PA_8, PC_7, PC_4, PB_3, PB_6, dev_spi); // linea per scheda seminatrice V7
- if (motor->init(&init) != COMPONENT_OK) {
- exit(EXIT_FAILURE);
- }
-
- /* Attaching and enabling an interrupt handler. */
- motor->attach_flag_irq(&my_flag_irq_handler);
- motor->enable_flag_irq();
- motor->disable_flag_irq();
-
- /* Attaching an 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);
@@ -1305,14 +1422,25 @@
TBmotorRst=1;
TBmotorDirecti=TBforward;
// definire il pin di clock che è PB_3
- motor->step_clock_mode_enable(StepperMotor::FWD);
+ motor->step_clock_mode_enable(StepperMotor::BWD);
// attiva l'out per il controllo dello stepper in stepClockMode
DigitalOut TBmotorStepOut(PB_3); // PowerStep01 Step Input
+ #if defined(pcSerial)
+ #if defined(checkLoop)
+ pc.printf("11f\n");
+ #endif
+ #endif
TBticker.attach_us(&step_TBPulseOut,500.0f); // clock time are seconds and attach seed motor stepper controls
- TATicker.attach(&inverti,2.0f);
+ TATicker.attach(&invertiLo,2.0f);
#else
// definire il pin di clock che è PB_3
motor->step_clock_mode_enable(StepperMotor::FWD);
+ #if defined(pcSerial)
+ #if defined(loStop)
+ pc.printf("A3\n");
+ #endif
+ #endif
+ motor->soft_hiz();
// attiva l'out per il controllo dello stepper in stepClockMode
DigitalOut TBmotorStepOut(PB_3); // PowerStep01 Step Input
#endif // end prova stepper
@@ -1326,15 +1454,9 @@
// MAIN LOOP
//**************************************************************************************************************
while (true){
- // ripetitore segnale di velocità
- #if defined(clockSpeedOut)
- clocca++;
- if (clocca >= 10){
- speedClock = !speedClock;
- clocca=0;
- }
- #endif
-
+ // ripetitore segnale di velocità. Il set a 1 è nella task ad interrupt
+ if (tractorSpeedRead==0){speedClock=0;}
+
// inversione segnali ingressi
#if !defined(simulaBanco)
seedWheelZeroPinInput = !seedWheelZeroPinInputRev;
@@ -1419,13 +1541,12 @@
if (inProva==0){
if ((startCycleSimulation==0)&&(enableSimula==0)){
- zeroRequestBuf=1;
- runRequestBuf=0;
- enableCycle=1;
+ runRequestBuf=1;//0;
}else{
- zeroRequestBuf=1;
- runRequestBuf=0;
- enableCycle=1;
+ runRequestBuf=1;//0;
+ }
+ if ((tractorSpeedRead==0)&&(speedFilter.read_ms()>3)){
+ oldTractorSpeedRead=0;
}
// ----------------------------------------
// filtra il segnale dei becchi per misurare il tempo di intervallo tra loro
@@ -1460,7 +1581,6 @@
if ((aspettaStart==0)&&(lowSpeed==1)){
beccoPronto=1;
}
- SDzeroCyclePulse=1;
lockStart=0;
double fase1=0.0f;
forzaFase=0;
@@ -1499,7 +1619,7 @@
#endif
#endif
if (timeIntraPick >= (memoIntraPick*2)){
- if ((aspettaStart==0)&&(zeroRequestBuf==0)){
+ if ((aspettaStart==0)){
if (firstStart==0){
all_pickSignal=1;
}
@@ -1518,14 +1638,14 @@
pulseRised2=1;
}
#if defined(speedMaster)
- if ((tractorSpeed_MtS_timed==0.0f)&&(zeroCycle==0)&&(zeroCycleEnd==1)){
- if ((firstStart==0)&&(simOk==0)&&(enableSimula==0)&&(zeroCycleEnd==1)){
+ if ((tractorSpeed_MtS_timed==0.0f)){
+ if ((firstStart==0)&&(simOk==0)&&(enableSimula==0)){
all_noSpeedSen=1;
}
}
double oldLastPr = (double)oldLastPulseRead*1.5f;
if((double)speedTimeOut.read_us()> (oldLastPr)){
- if ((firstStart==0)&&(simOk==0)&&(enableSimula==0)&&(zeroCycleEnd==1)){
+ if ((firstStart==0)&&(simOk==0)&&(enableSimula==0)){
all_speedError =1;
}
}
@@ -1533,6 +1653,14 @@
//*******************************************
// esegue calcolo clock per la generazione della posizione teorica
// la realtà in base al segnale di presenza del becco
+ if (speedOfSeedWheel < 0.002f){
+ #if defined(pcSerial)
+ #if defined(checkLoopb)
+ pc.printf("forza\n");
+ #endif
+ #endif
+ speedOfSeedWheel=0.001f;
+ }
realGiroSD = seedPerimeter / speedOfSeedWheel;
tempoBecco = (realGiroSD/360.0f)*16000.0f;
frequenzaReale = fixedStepGiroSD/realGiroSD;
@@ -1542,8 +1670,11 @@
TBrpm = seedWheelRPM*rapportoRuote; // 5.896 31,75
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
- //TBperiod=5.681818f*TBrpm; //prova dopo test con contagiri
- //TBperiod=2.0f*TBrpm; //prova dopo test con contagiri
+ #if defined(pcSerial)
+ #if defined(checkLoop)
+ pc.printf("sep: %f sposw: %f rgsd: %f tpb: %f fr: %f spr: %f swr: %f tbfr: %f \n",seedPerimeter,speedOfSeedWheel,realGiroSD,tempoBecco,frequenzaReale,semiPeriodoReale,seedWheelRPM,TBfrequency);
+ #endif
+ #endif
}
// ----------------------------------------
// check SD fase
@@ -1551,9 +1682,27 @@
forzaFase=0;
if (trigRepos==1){
SDactualPosition=0;
- if ((countCicli<30)&&(trigCicli==0)){countCicli++;trigCicli=1;}
- if(countCicli>=cicliAspettaStart){aspettaStart=0;}
- if ((lowSpeed==0)&&(aspettaStart==0)&&(lockStart==0)){syncroCheck=1;beccoPronto=0;}
+ if ((countCicli<30)&&(trigCicli==0)){
+ countCicli++;
+ trigCicli=1;
+ }
+ if(countCicli>=cicliAspettaStart){
+ aspettaStart=0;
+ #if defined(pcSerial)
+ #if defined(checkLoop)
+ pc.printf("NoAspetta\n");
+ #endif
+ #endif
+ }
+ if ((lowSpeed==0)&&(aspettaStart==0)&&(lockStart==0)){
+ syncroCheck=1;
+ beccoPronto=0;
+ #if defined(pcSerial)
+ #if defined(checkLoop)
+ pc.printf("BeccoNo\n");
+ #endif
+ #endif
+ }
if (trigTB==0){
inhibit=1;
trigSD=1;
@@ -1658,45 +1807,15 @@
}
// ----------------------------------------
// read and manage joystick
- // WARNING - ENABLE CYCLE IS SOFTWARE ALWAYS ACTIVE
- if (enableCycle==1){
- if(runRequestBuf==1){
- if (OldStartCycle!=runRequestBuf){
- if((startCycle==0)&&(zeroCycleEnd==1)){
- startCycle=1;
- OldStartCycle = runRequestBuf;
- oldZeroCycle=0;
- }
- }
- }else{
- startCycle=0;
- pntMedia=0;
- }
- if (azzeraDaCan==1){
- if (tractorSpeed_MtS_timed==0.0f){
- zeroRequestBuf=1;
- oldZeroCycle=0;
- }
- azzeraDaCan=0;
- }
- if (loadDaCan==1){
- if (tractorSpeed_MtS_timed==0.0f){
- ciclaTB();
- }
- }
- if ((zeroRequestBuf==1)){
- if (oldZeroCycle!=zeroRequestBuf){
- zeroCycle=1;
- zeroCycleEnd=0;
- SDzeroed=1;
- TBzeroed=0;
- zeroTrigger=0;
- oldZeroCycle = zeroRequestBuf;
- }
- }
- }else{
- startCycle=0;
- zeroCycle=0;
+ if (loadDaCan==1){
+ if (tractorSpeed_MtS_timed==0.0f){
+ #if defined(pcSerial)
+ #if defined(checkLoop)
+ pc.printf("daCAN\n");
+ #endif
+ #endif
+ ciclaTB();
+ }
}
//***************************************************************************************************
@@ -1763,6 +1882,7 @@
#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;
@@ -1811,21 +1931,14 @@
double k4=0.0f;
double k5=0.0f;
double k6=0.0f;
- /*if (tractorSpeed_MtS_timed <= minSeedSpeed){
- k3=1.030f;
- k4=5.103f;
- k5=10.00f;
- k6=20.50f;
- }else{*/
- #if defined(speedMaster)
- k3=0.010f;
- #else
- k3=0.050f;
- #endif
- k4=1.103f;
- k5=10.00f;
- k6=20.50f;
- //}
+ #if defined(speedMaster)
+ k3=0.010f;
+ #else
+ k3=0.050f;
+ #endif
+ k4=1.103f;
+ k5=10.00f;
+ k6=20.50f;
double L1 = 0.045f;
double L_1=-0.045f;
double L2 = 0.150f;
@@ -1932,7 +2045,14 @@
lockStart=1;
periodo = TBperiod;
motor->step_clock_mode_enable(StepperMotor::FWD);
- if (aspettaStart==0){cambiaTB(periodo);}
+ if (aspettaStart==0){
+ #if defined(pcSerial)
+ #if defined(checkLoop)
+ pc.printf("da sincro\n");
+ #endif
+ #endif
+ cambiaTB(periodo);
+ }
}
// controllo di stop
double memoIntraP = (double)memoIntraPick*1.8f;
@@ -1940,8 +2060,23 @@
syncroCheck=0;
aspettaStart=1;
countCicli=0;
+ #if defined(pcSerial)
+ #if defined(checkLoop)
+ pc.printf("AspettaSI\n");
+ #endif
+ #endif
if (TBzeroCyclePulse==1){
+ #if defined(pcSerial)
+ #if defined(checkLoop)
+ pc.printf("15f\n");
+ #endif
+ #endif
TBticker.detach();
+ #if defined(pcSerial)
+ #if defined(loStop)
+ pc.printf("A4\n");
+ #endif
+ #endif
motor->soft_hiz();
}
}
@@ -1963,7 +2098,14 @@
int tempoDiSincro = (int)((double)(ritardoMassimo - ((tempoBecco/2.0f)+((speedOfSeedWheel/maxWorkSpeed)*ritardoMassimo)))); //
if (tempoDiSincro <= 1){tempoDiSincro=1;}
if ((sincroTimer.read_ms()>= tempoDiSincro)){
- if (tractorSpeed_MtS_timed >= minWorkSpeed){startCicloTB=1;}
+ if (tractorSpeed_MtS_timed >= minWorkSpeed){
+ startCicloTB=1;
+ #if defined(pcSerial)
+ #if defined(checkLoop)
+ pc.printf("startTB\n");
+ #endif
+ #endif
+ }
beccoPronto=0;
}
}else{
@@ -1972,193 +2114,66 @@
beccoPronto=0;
}
}
+ #if defined(pcSerial)
+ #if defined(checkLoop)
+ pc.printf("lowSpeed\n");
+ #endif
+ #endif
ciclaTB();
}
//*************************************************************
}else{ // fine ciclo con velocita maggiore di 0
- SDwheelTimer.stop();
- SDwheelTimer.reset();
- #if defined(seedSensor)
- resetDelay();
- #endif
- checkSDrotation=0;
- oldFaseLavoro=0;
- aspettaStart=1;
- countCicli=0;
- startCycle=0;
- oldSeedWheelPeriod=0.0f;
- oldPeriodoTB=0.0f;
- correzione=0.0f;
- OLDpulseSpeedInterval=1000.01f;
- cicloTbinCorso=0;
- cntTbError=0;
- olddcActualDuty=0.0f;
- speedOfSeedWheel=0.0f;
if (cycleStopRequest==1){
- zeroDelay.reset();
- zeroDelay.start();
- runRequestBuf=0;
- zeroRequestBuf=1;
+ SDwheelTimer.stop();
+ SDwheelTimer.reset();
+ #if defined(seedSensor)
+ resetDelay();
+ #endif
+ checkSDrotation=0;
+ oldFaseLavoro=0;
+ aspettaStart=1;
+ countCicli=0;
+ oldSeedWheelPeriod=0.0f;
+ oldPeriodoTB=0.0f;
+ correzione=0.0f;
+ OLDpulseSpeedInterval=1000.01f;
+ cicloTbinCorso=0;
+ cntTbError=0;
+ olddcActualDuty=0.0f;
+ #if defined(pcSerial)
+ #if defined(checkLoopb)
+ pc.printf("forza\n");
+ #endif
+ #endif
+ speedOfSeedWheel=0.0f;
cycleStopRequest=0;
- SDzeroCyclePulse=0;
- TBzeroCyclePulse=0;
- zeroCycleEnd=0;
- zeroCycle=1;
- zeroTrigger=0;
- noSDzeroRequest=1;
+ DC_brake=1;
+ DC_prepare();
+ #if defined(pcSerial)
+ #if defined(checkLoop)
+ pc.printf("17h\n");
+ #endif
+ #endif
+ TBticker.detach();
+ #if defined(pcSerial)
+ #if defined(loStop)
+ pc.printf("A5\n");
+ #endif
+ #endif
+ motor->soft_hiz();
+ pntMedia=0;
+ #if defined(pcSerial)
+ #if defined(stopSignal)
+ pc.printf("stop\n");
+ #endif
+ #endif
}
}
- //*************************************************************************************************
- // ciclo di azzeramento motori
- if ((zeroCycleEnd==0)&&(zeroCycle==1)){//&&(zeroDelay.read_ms()>10000)){
- if (zeroTrigger==0){
- motor->step_clock_mode_enable(StepperMotor::FWD);
- TBticker.attach_us(&step_TBPulseOut,1000.0f); // clock time are seconds and attach seed motor stepper controls
- DC_forward=1;
- DC_brake=1;
- DC_prepare();
- frenata=0;
- zeroTrigger=1;
- ritardoStop.reset();
- ritardoComandoStop.reset();
- ritardoComandoStop.start();
- timeOutZeroSD.start();
- quincTimeSet.reset();
- }
- int tempoFrenata=300;
- if (((ritardoStop.read_ms()>tempoFrenata)&&(SDzeroDebounced==0))||(accensione==1)||(ritardoComandoStop.read_ms()>400)){
- accensione=0;
- avvicinamento=1;
- avvicinamentoOn.reset();
- avvicinamentoOn.start();
- SDmotorPWM.write(1.0f-0.32f); // duty cycle = 60% of power
- DC_forward=1;
- DC_brake=0;
- DC_prepare();
- ritardoComandoStop.reset();
- ritardoComandoStop.stop();
- }
- if (avvicinamento==1){
- if(avvicinamentoOn.read_ms()>300){
- SDmotorPWM.write(1.0f-0.7f);
- avvicinamentoOn.reset();
- avvicinamentoOff.reset();
- avvicinamentoOff.start();
- }
- if(avvicinamentoOff.read_ms()>100){
- SDmotorPWM.write(1.0f-0.32f);
- avvicinamentoOff.reset();
- avvicinamentoOff.stop();
- avvicinamentoOn.start();
- }
- }else{
- avvicinamentoOn.stop();
- avvicinamentoOff.stop();
- avvicinamentoOn.reset();
- avvicinamentoOff.reset();
- }
- if (frenata==0){
- if (SDzeroCyclePulse==1){
- SDticker.detach();
- frenata=1;
- quincTimeSet.reset();
- quincTimeSet.start();
- ritardoStop.start();
- //ritardoComandoStop.reset();
- //ritardoComandoStop.stop();
- }
- }else{
- #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;
- DC_prepare();
- SDzeroed=1;
- ritardoStop.reset();
- ritardoStop.stop();
- }
- #endif
- }
- // azzera tutto in time out
- if ((timeOutZeroSD.read_ms()>=10000)||(noSDzeroRequest==1)){
- if ((firstStart==0)&&(noSDzeroRequest==0)){
- all_no_Zeroing=1;
- }
- avvicinamento=0;
- DC_brake=1;
- DC_prepare();
- SDzeroed=1;
- ritardoStop.reset();
- ritardoStop.stop();
- avvicinamentoOn.stop();
- avvicinamentoOff.stop();
- avvicinamentoOn.reset();
- avvicinamentoOff.reset();
- ritardoComandoStop.reset();
- ritardoComandoStop.stop();
- timeOutZeroSD.stop();
- timeOutZeroSD.reset();
- //noSDzeroRequest=0;
- }
- if (TBzeroCyclePulse==1){
- TBticker.detach();
- motor->soft_hiz();
- TBzeroed=1;
- }
- if ((SDzeroed==1)&&(TBzeroed==1)){
- avvicinamentoOn.stop();
- avvicinamentoOff.stop();
- ritardoComandoStop.stop();
- ritardoStop.stop();
- zeroCycleEnd=1;
- zeroCycle=0;
- zeroTrigger=0;
- runRequestBuf=1;
- zeroRequestBuf=0;
- cycleStopRequest=0;
- SDzeroed=1;
- TBzeroed=0;
- timeOutZeroSD.stop();
- timeOutZeroSD.reset();
- DC_brake=1;
- DC_prepare();
- SDzeroed=1;
- noSDzeroRequest=0;
- }
- }
//*************************************************************************************************
- if (enableCycle==0){
- zeroTrigger=0;
- SDmotorPWM=0;
- SDmotorInA=0;
- SDmotorInB=0;
- }
- SDzeroCyclePulse=0;
TBzeroCyclePulse=0;
//*************************************************************************************************
- }
+ } //end inProva==0
wd.Service(); // kick the dog before the timeout
} // end while
} // end main