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:
- 10:9e70619e97ab
- Parent:
- 9:7f02256f6e8f
- Child:
- 12:b0fc1d313813
--- a/main.cpp Mon Mar 11 17:13:17 2019 +0000
+++ b/main.cpp Fri Mar 15 11:09:37 2019 +0000
@@ -123,47 +123,47 @@
motor->isrFlag = TRUE;
/* Get the value of the status register. */
unsigned int statusRegister = motor->get_status();
-#if defined(pcSerial)
- pc.printf(" WARNING: \"FLAG\" interrupt triggered.\r\n");
-#endif
+ #if defined(pcSerial)
+ pc.printf(" WARNING: \"FLAG\" interrupt triggered.\r\n");
+ #endif
/* Check SW_F flag: if not set, the SW input is opened */
if ((statusRegister & POWERSTEP01_STATUS_SW_F ) != 0) {
-#if defined(pcSerial)
+ #if defined(pcSerial)
pc.printf(" SW closed (connected to ground).\r\n");
-#endif
+ #endif
}
/* Check SW_EN bit */
if ((statusRegister & POWERSTEP01_STATUS_SW_EVN) == POWERSTEP01_STATUS_SW_EVN) {
-#if defined(pcSerial)
+ #if defined(pcSerial)
pc.printf(" SW turn_on event.\r\n");
-#endif
+ #endif
}
/* Check Command Error flag: if set, the command received by SPI can't be */
/* performed. This occurs for instance when a move command is sent to the */
/* Powerstep01 while it is already running */
if ((statusRegister & POWERSTEP01_STATUS_CMD_ERROR) == POWERSTEP01_STATUS_CMD_ERROR) {
-#if defined(pcSerial)
+ #if defined(pcSerial)
pc.printf(" Non-performable command detected.\r\n");
-#endif
+ #endif
}
/* Check UVLO flag: if not set, there is an undervoltage lock-out */
if ((statusRegister & POWERSTEP01_STATUS_UVLO)==0) {
-#if defined(pcSerial)
+ #if defined(pcSerial)
pc.printf(" undervoltage lock-out.\r\n");
-#endif
+ #endif
}
/* Check thermal STATUS flags: if set, the thermal status is not normal */
if ((statusRegister & POWERSTEP01_STATUS_TH_STATUS)!=0) {
//thermal status: 1: Warning, 2: Bridge shutdown, 3: Device shutdown
-#if defined(pcSerial)
+ #if defined(pcSerial)
pc.printf(" Thermal status: %d.\r\n", (statusRegister & POWERSTEP01_STATUS_TH_STATUS)>>11);
-#endif
+ #endif
}
/* Check OCD flag: if not set, there is an overcurrent detection */
if ((statusRegister & POWERSTEP01_STATUS_OCD)==0) {
-#if defined(pcSerial)
+ #if defined(pcSerial)
pc.printf(" Overcurrent detection.\r\n");
-#endif
+ #endif
}
/* Reset ISR flag. */
motor->isrFlag = FALSE;
@@ -179,9 +179,9 @@
void my_error_handler(uint16_t error)
{
/* Printing to the console. */
-#if defined(pcSerial)
- pc.printf("Error %d detected\r\n\n", error);
-#endif
+ #if defined(pcSerial)
+ pc.printf("Error %d detected\r\n\n", error);
+ #endif
/* Infinite loop */
//while (true) {
@@ -223,35 +223,32 @@
void aggioVelocita()
{
realGiroSD = seedPerimeter / speedOfSeedWheel;
- tempoBecco = (realGiroSD/360.0f)*16000.0f;
+ tempoBecco = realGiroSD*444.444f; //(realGiroSD/360.0f)*16000.0f;
frequenzaReale = fixedStepGiroSD/realGiroSD;
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
- 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
#if defined(runner)
+ TBrpm = seedWheelRPM*rapportoRuote; // 5.896 31,75
#if defined(Zucca)
TBperiod=5.2f*TBrpm*2.0f; //prova dopo test con contagiri
#else
TBperiod=5.2f*TBrpm; //prova dopo test con contagiri
#endif
- //5.681818f
#else
+ 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
#endif
}
//************************************************************************
// rise of seed speed 25 pulse sensor
-void sd25Fall()
-{
+void sd25Fall(){
timeHole=metalTimer.read_ms();
- int memo_TimeHole= (memoTimeHole + timeHole)/ (int)2;
- memoTimeHole = timeHole;
+ double memo_TimeHole= (memoTimeHole + (double)timeHole)/ 2.0f;
+ memoTimeHole = (double)timeHole;
metalTimer.reset();
if (speedFromPick==0) {
- speedOfSeedWheel=((seedPerimeter/25.0f)/(double)memo_TimeHole)*1000.0f; //mtS
+ speedOfSeedWheel=((seedPerimeter/25.0f)/memo_TimeHole)*1000.0f; //mtS
}
#if defined(pcSerial)
#if defined(checkLoop)
@@ -259,14 +256,15 @@
#endif
#endif
}
+//************************************************************************
// rise of seed speed motor encoder
void encoRise(){
timeHole=metalTimer.read_us();
- int memo_TimeHole= (memoTimeHole + timeHole)/ (int)2;
- memoTimeHole = timeHole;
+ double memo_TimeHole= (memoTimeHole + (double)timeHole)/ 2.0f;
+ memoTimeHole = (double)timeHole;
metalTimer.reset();
if (encoder==true) {
- speedOfSeedWheel=((seedPerimeter/((SDreductionRatio*25.5f)))/(double)memo_TimeHole)*1000000.0f; //mtS
+ speedOfSeedWheel=((seedPerimeter/((SDreductionRatio*25.5f)))/memo_TimeHole)*1000000.0f; //mtS
pulseRised2=1;
}
#if defined(pcSerial)
@@ -313,6 +311,7 @@
}
speedClock=1;
speedFilter.reset();
+ cntSpeedError=0;
#if defined(pcSerial)
#if defined(checkLoop)
pc.printf("5\n");
@@ -330,11 +329,11 @@
}
mediaSpeed[0]=lastPd;
OLDpulseSpeedInterval=pulseSpeedInterval;
-#if defined(pcSerial)
-#if defined(checkLoop)
- pc.printf("6\n");
-#endif
-#endif
+ #if defined(pcSerial)
+ #if defined(checkLoop)
+ pc.printf("6\n");
+ #endif
+ #endif
}
//*******************************************************
@@ -345,14 +344,14 @@
{
SDactualPosition++;
prePosSD++;
-#if defined(speedMaster)
- posForQuinc++;
-#endif
-#if defined(pcSerial)
-#if defined(checkLoop)
- pc.printf("7\n");
-#endif
-#endif
+ #if defined(speedMaster)
+ posForQuinc++;
+ #endif
+ #if defined(pcSerial)
+ #if defined(checkLoop)
+ pc.printf("7\n");
+ #endif
+ #endif
}
//*******************************************************
void step_TBPulseOut()
@@ -363,11 +362,11 @@
TBactualPosition++;
}
}
-#if defined(pcSerial)
-#if defined(stepTamb)
- pc.printf("step\n");
-#endif
-#endif
+ #if defined(pcSerial)
+ #if defined(stepTamb)
+ pc.printf("step\n");
+ #endif
+ #endif
/*
#if defined(pcSerial)
#if defined(checkLoop)
@@ -552,39 +551,6 @@
TBdeltaStep=(fixedStepGiroSD/pickNumber)-(stepGrado*avvioGradi);
TBfaseStep = (stepGrado*angoloFase);
*/
-//#if defined(Zucca)
-/* if ((tractorSpeed_MtS_timed>0.01f)) {
- if (inhibit==0) {
- double posError =0.0f;
- double posSD=((double)SDactualPosition)/KcorT;
- posError = posSD - (double)TBactualPosition;
- // interviene sulla velocità di TB per raggiungere la corretta posizione relativa
- if((lowSpeed==0)&&(aspettaStart==0)) {
- //if (posError>50.0f){posError=50.0f;}
- //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 {
- cambiaTB(periodo);//2.0f;
- }
- #if defined(pcSerial)
- #if defined(TBperS)
- pc.printf("TBpos: %f SDpos: %f Err: %f Correggi: %f\n",(double)TBactualPosition,posSD,posError,ePpos);
- #endif
- #endif
- }
- }
- }
- }
- */
-//#else
if ((tractorSpeed_MtS_timed>0.01f)) {
if (inhibit==0) {
double posError =0.0f;
@@ -637,12 +603,11 @@
}
}
}
-//#endif
-#if defined(pcSerial)
-#if defined(checkLoopa)
- pc.printf("12\n");
-#endif
-#endif
+ #if defined(pcSerial)
+ #if defined(checkLoopa)
+ pc.printf("12\n");
+ #endif
+ #endif
}
//*******************************************************
void videoUpdate()
@@ -656,20 +621,20 @@
totalSpeed += speedForDisplay[aa];
}
totalSpeed = totalSpeed / 5.0f;
-#if defined(pcSerial)
-#if defined(SDreset)
- pc.printf("Fase: %d",fase);
- pc.printf(" PrePosSD: %d",prePosSD);
- pc.printf(" PosSD: %d",SDactualPosition);
- pc.printf(" speed: %f",tractorSpeed_MtS_timed);
- pc.printf(" Trigger: %d \n", trigRepos);
-#endif
-#endif
-#if defined(pcSerial)
-#if defined(checkLoop)
- pc.printf("13\n");
-#endif
-#endif
+ #if defined(pcSerial)
+ #if defined(SDreset)
+ pc.printf("Fase: %d",fase);
+ pc.printf(" PrePosSD: %d",prePosSD);
+ pc.printf(" PosSD: %d",SDactualPosition);
+ pc.printf(" speed: %f",tractorSpeed_MtS_timed);
+ pc.printf(" Trigger: %d \n", trigRepos);
+ #endif
+ #endif
+ #if defined(pcSerial)
+ #if defined(checkLoop)
+ pc.printf("13\n");
+ #endif
+ #endif
}
//*******************************************************
void ciclaTB()
@@ -802,44 +767,44 @@
alarmLowRegister = alarmLowRegister1;
}
-#if defined(pcSerial)
-#if defined(VediAllarmi)
- if (all_pickSignal==1) {
- pc.printf("AllarmeBecchi\n");
- }
- if (all_cellSignal==1) {
- pc.printf("AllarmeCelle\n");
- }
- if (all_lowBattery==1) {
- pc.printf("AllarmeBassaCorrente\n");
- }
- if (all_overCurrDC==1) {
- pc.printf("AllarmeAltaCorrente\n");
- }
- if (all_stopSistem==1) {
- pc.printf("AllarmeStop\n");
- }
- if (all_noDcRotati==1) {
- pc.printf("AllarmeDCnoRotation\n");
- }
- if (all_noStepRota==1) {
- pc.printf("AllarmeNoStepRotation\n");
- }
- if (all_speedError==1) {
- pc.printf("AllarmeSpeedError\n");
- }
- if (all_noSpeedSen==1) {
- pc.printf("AllarmeNoSpeedSensor\n");
- }
- if (all_no_Zeroing==1) {
- pc.printf("AllarmeNoZero\n");
- }
- if (all_genericals==1) {
- pc.printf("AllarmeGenerico\n");
- }
- pc.printf("Code: 0x%x%x\n",alarmHighRegister,alarmLowRegister);
-#endif
-#endif
+ #if defined(pcSerial)
+ #if defined(VediAllarmi)
+ if (all_pickSignal==1) {
+ pc.printf("AllarmeBecchi\n");
+ }
+ if (all_cellSignal==1) {
+ pc.printf("AllarmeCelle\n");
+ }
+ if (all_lowBattery==1) {
+ pc.printf("AllarmeBassaCorrente\n");
+ }
+ if (all_overCurrDC==1) {
+ pc.printf("AllarmeAltaCorrente\n");
+ }
+ if (all_stopSistem==1) {
+ pc.printf("AllarmeStop\n");
+ }
+ if (all_noDcRotati==1) {
+ pc.printf("AllarmeDCnoRotation\n");
+ }
+ if (all_noStepRota==1) {
+ pc.printf("AllarmeNoStepRotation\n");
+ }
+ if (all_speedError==1) {
+ pc.printf("AllarmeSpeedError\n");
+ }
+ if (all_noSpeedSen==1) {
+ pc.printf("AllarmeNoSpeedSensor\n");
+ }
+ if (all_no_Zeroing==1) {
+ pc.printf("AllarmeNoZero\n");
+ }
+ if (all_genericals==1) {
+ pc.printf("AllarmeGenerico\n");
+ }
+ pc.printf("Code: 0x%x%x\n",alarmHighRegister,alarmLowRegister);
+ #endif
+ #endif
all_semiFiniti=0;
all_pickSignal=0;
all_cellSignal=0;
@@ -855,11 +820,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(pcSerial)
+ #if defined(checkLoop)
+ pc.printf("17\n");
+ #endif
+ #endif
}
//*******************************************************
#if defined(speedMaster)
@@ -928,19 +893,20 @@
val1[3]=val1[3]+(TBzeroPinInput*0x02);
val1[3]=val1[3]+(DcEncoder*0x04);
val1[3]=val1[3]+(seedWheelZeroPinInput*0x08);
-#if defined(simulaPerCan)
- if (buttonUser==0) {
- val1[1]=0x02;
- } else {
- val1[1]=0x00;
- }
- val1[3]=valore;
- valore+=1;
- if(valore>50) {
- valore=0;
- }
- tractorSpeed_MtS_timed=valore;
-#endif
+ #if defined(simulaPerCan)
+ if (buttonUser==0) {
+ val1[1]=0x02;
+ } else {
+ val1[1]=0x00;
+ }
+ val1[3]=valore;
+ valore+=1;
+ if(valore>50) {
+ valore=0;
+ }
+ tractorSpeed_MtS_timed=valore;
+ oldLocalTractorSpeed=valore;
+ #endif
allarmi();
valore = totalSpeed*36.0f; // tractorSpeed_MtS_timed*36.0f;
val1[0]=valore;
@@ -950,16 +916,16 @@
val1[4]=resetComandi;
val1[5]=cellsCounterLow;
val1[6]=cellsCounterHig;
-#if defined(canbusActive)
- if(can1.write(CANMessage(TX_ID, *&val1,8))) {
- checkState=0;
- }
-#endif
-#if defined(pcSerial)
-#if defined(checkLoop)
- pc.printf("19\n");
-#endif
-#endif
+ #if defined(canbusActive)
+ if(can1.write(CANMessage(TX_ID, *&val1,8))) {
+ checkState=0;
+ }
+ #endif
+ #if defined(pcSerial)
+ #if defined(checkLoop)
+ pc.printf("19\n");
+ #endif
+ #endif
}
//*******************************************************
void leggiCAN()
@@ -1212,18 +1178,18 @@
}
}
}
-#endif
-#if defined(overWriteCanSimulation)
- enableSimula=1;
- speedSimula=25;
- avviaSimula=1;
- simOk=1;
-#endif
-#if defined(pcSerial)
-#if defined(checkLoop)
- pc.printf("20\n");
-#endif
-#endif
+ #endif
+ #if defined(overWriteCanSimulation)
+ enableSimula=1;
+ speedSimula=25;
+ avviaSimula=1;
+ simOk=1;
+ #endif
+ #if defined(pcSerial)
+ #if defined(checkLoop)
+ pc.printf("20\n");
+ #endif
+ #endif
}
//*******************************************************
@@ -1253,13 +1219,13 @@
}
float SD_CurrentAnalogica=sommaTutto/20.0f;
SD_CurrentScaled = floor(( (SD_CurrentAnalogica - SD_CurrentStart)*3.3f) / (SD_CurrentFactor/1000.0f)*10)/10;
-#if defined(pcSerial)
-#if defined(correnteAssorbita)
- pc.printf("CorrenteStart: %f ",SD_CurrentStart);
- pc.printf(" CorrenteScaled: %f ",SD_CurrentScaled);
- pc.printf(" CorrenteMedia: %f \n",SD_CurrentAnalogica);
-#endif
-#endif
+ #if defined(pcSerial)
+ #if defined(correnteAssorbita)
+ pc.printf("CorrenteStart: %f ",SD_CurrentStart);
+ pc.printf(" CorrenteScaled: %f ",SD_CurrentScaled);
+ pc.printf(" CorrenteMedia: %f \n",SD_CurrentAnalogica);
+ #endif
+ #endif
reduceCurrent=false;
incrementCurrent=false;
/*
@@ -1273,9 +1239,9 @@
if (SD_CurrentScaled > 10.0f) {
timeCurr.start();
if (timeCurr.read() > 5.0f) {
-#if defined(canbusActive)
+ #if defined(canbusActive)
all_overCurrDC=1;
-#endif
+ #endif
reduceCurrent=true;
timeCurr.reset();
}
@@ -1283,11 +1249,11 @@
timeCurr.stop();
}
//}
-#if defined(pcSerial)
-#if defined(checkLoop)
- pc.printf("21\n");
-#endif
-#endif
+ #if defined(pcSerial)
+ #if defined(checkLoop)
+ pc.printf("21\n");
+ #endif
+ #endif
}
//*******************************************************
void DC_prepare()
@@ -1316,11 +1282,11 @@
} else {
SD_faultB=0;
}
-#if defined(pcSerial)
-#if defined(checkLoopa)
- pc.printf("22\n");
-#endif
-#endif
+ #if defined(pcSerial)
+ #if defined(checkLoopa)
+ pc.printf("22\n");
+ #endif
+ #endif
}
//*******************************************************
void startDelay()
@@ -1332,67 +1298,65 @@
timeout.attach_us(&DC_CheckCurrent,ritardo);
}
}
-#if defined(pcSerial)
-#if defined(checkLoop)
- pc.printf("23\n");
-#endif
-#endif
+ #if defined(pcSerial)
+ #if defined(checkLoop)
+ pc.printf("23\n");
+ #endif
+ #endif
}
//*******************************************************
-void quincTrigon()
-{
+void quincTrigon(){
quincClock=true;
-#if defined(pcSerial)
-#if defined(checkLoop)
- pc.printf("24\n");
-#endif
-#endif
+ #if defined(pcSerial)
+ #if defined(checkLoop)
+ pc.printf("24\n");
+ #endif
+ #endif
}
-void quincTrigof()
-{
+void quincTrigof(){
quincClock=false;
-#if defined(pcSerial)
-#if defined(checkLoop)
- pc.printf("25\n");
-#endif
-#endif
+ #if defined(pcSerial)
+ #if defined(checkLoop)
+ pc.printf("25\n");
+ #endif
+ #endif
}
//*******************************************************
void quinCalc()
{
// riceve l'impulso di sincro dal master e fa partire il timer di verifica dell'errore
-#if !defined(mezzo)
- if ((quincClock==true)&&(oldQuincIn==0)) {
- oldQuincIn=1;
- if (quincStart==0) {
- oldQuincTimeSD = (double) quincTimeSD.read_ms();
- quincTime.reset();
- quincTimeSD.reset();
- quincStart=1;
+ #if !defined(mezzo)
+ if ((quincClock==true)&&(oldQuincIn==0)) {
+ oldQuincIn=1;
+ if (quincStart==0) {
+ oldQuincTimeSD = (double) quincTimeSD.read_ms();
+ quincTime.reset();
+ quincTimeSD.reset();
+ quincStart=1;
+ }
}
- }
- if(quincClock==false) {
- oldQuincIn=0;
- }
-#else
- if ((((quinconceActive==0)&&(quincClock==true))||((quinconceActive==1)&&(quincClock==false)))&&(oldQuincIn==0)) {
- oldQuincIn=1;
- if (quincStart==0) {
- oldQuincTimeSD = (double) quincTimeSD.read_ms();
- quincTime.reset();
- quincStart=1;
- }
- }
- if (quinconceActive==0) {
- if (quincClock==false) {
+ if(quincClock==false) {
oldQuincIn=0;
}
- } else {
- if (quincClock==true) {
- oldQuincIn=0;
+ #else
+ if ((((quinconceActive==0)&&(quincClock==true))||((quinconceActive==1)&&(quincClock==false)))&&(oldQuincIn==0)) {
+ oldQuincIn=1;
+ if (quincStart==0) {
+ oldQuincTimeSD = (double) quincTimeSD.read_ms();
+ quincTime.reset();
+ quincStart=1;
+ }
}
- }
-#endif
+ if (quinconceActive==0) {
+ if (quincClock==false) {
+ oldQuincIn=0;
+ }
+ } else {
+ if (quincClock==true) {
+ oldQuincIn=0;
+ }
+ }
+ #endif
//****************************************************************************************
if (quincCnt>=4) {
if (countPicks==0) {
@@ -1441,6 +1405,12 @@
if (speedFromMaster>0.0f) {
if (enableSimula==0) {
tractorSpeed_MtS_timed = speedFromMaster + percento;
+ if (tractorSpeed_MtS_timed > (oldLocalTractorSpeed*1.15f){
+ tractorSpeed_MtS_timed = oldLocalTractorSpeed*1.15f;
+ }
+ if (tractorSpeed_MtS_timed < (oldLocalTractorSpeed/1.15f){
+ tractorSpeed_MtS_timed = oldLocalTractorSpeed/1.15f;
+ }
}
}
}
@@ -1451,7 +1421,7 @@
if (canDataCheckEnable==true) {
if (canDataCheck==1) { // sincro da comunicazione can del valore di posizione del tamburo master
canDataCheck=0;
- double parametro = SDsectorStep/3.0f;
+ double parametro = SDsectorStep/2.0f;
double differenza=0.0f;
#if defined(mezzo)
if (quinconceActive==1) {
@@ -1486,6 +1456,12 @@
if (speedFromMaster>0.0f) {
if (enableSimula==0) {
tractorSpeed_MtS_timed = speedFromMaster + percento;
+ if (tractorSpeed_MtS_timed > (oldLocalTractorSpeed*1.15f){
+ tractorSpeed_MtS_timed = oldLocalTractorSpeed*1.15f;
+ }
+ if (tractorSpeed_MtS_timed < (oldLocalTractorSpeed/1.15f){
+ tractorSpeed_MtS_timed = oldLocalTractorSpeed/1.15f;
+ }
}
}
}
@@ -1549,7 +1525,8 @@
// dev_spi(mosi,miso,sclk)
// D11= PA7; D12= PA6; D13= PA5
DevSPI dev_spi(D11, D12, D13);
-
+ dev_spi.frequency(5000000);
+
/* 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
@@ -1610,8 +1587,6 @@
if (inProva==0) {
tractorSpeedRead.rise(&tractorReadSpeed);
-
-
#if !defined(speedMaster)
quinconceIn.rise(&quincTrigon);
quinconceIn.fall(&quincTrigof);
@@ -1639,19 +1614,20 @@
TBmotorDirecti=TBforward;
// definire il pin di clock che è PB_3
#if defined(Zucca)
- motor->step_clock_mode_enable(StepperMotor::FWD);
+ motor->run(StepperMotor::BWD,100.0f);
+ //motor->step_clock_mode_enable(StepperMotor::FWD);
#else
- motor->step_clock_mode_enable(StepperMotor::BWD);
+ //motor->step_clock_mode_enable(StepperMotor::BWD);
#endif
// attiva l'out per il controllo dello stepper in stepClockMode
- DigitalOut TBmotorStepOut(PB_3); // PowerStep01 Step Input
+ //DigitalOut TBmotorStepOut(PB_3); // PowerStep01 Step Input
#if defined(pcSerial)
#if defined(checkLoop)
pc.printf("11f\n");
#endif
#endif
- TBticker.attach(&step_TBPulseOut,0.0005f); // clock time are seconds and attach seed motor stepper controls
- TATicker.attach(&invertiLo,3.0f);
+ //TBticker.attach(&step_TBPulseOut,0.0005f); // clock time are seconds and attach seed motor stepper controls
+ //TATicker.attach(&invertiLo,3.0f);
#else
// definire il pin di clock che è PB_3
motor->set_home();
@@ -1820,12 +1796,10 @@
if(speedForCorrection >= speedOfSeedWheel) {
fase1=TBdeltaStep;
} else {
- //fase1=(TBdeltaStep)-(((speedOfSeedWheel-speedForCorrection)/maxWorkSpeed)*(TBfaseStep));
fase1=(TBdeltaStep)-(((speedOfSeedWheel)/maxWorkSpeed)*(TBfaseStep));
}
if (fase1 > limite) {
fase1 -= limite; // se la fase calcolata supera gli step del settore riporta il valore nell'arco precedente (es. fase 1 800, limite 750, risulta 50)
- //forzaFase=1;
}
if ((fase1 > (limite -20.0f))&&(fase1<(limite +5.0f))) {
fase1 = limite -20.0f; // se la fase è molto vicina al limite interpone uno spazio minimo di lavoro del sincronismo
@@ -1857,11 +1831,11 @@
memoIntraPick = timeIntraPick;
if ((speedFromPick==1)&&(encoder==false)) {
speedOfSeedWheel=((seedPerimeter / pickNumber)/timeIntraPick)*1000.0f;
- #if defined(pcSerial)
- #if defined(Qnca)
- pc.printf("perim: %f pickN: %f sped: %f\n", seedPerimeter, pickNumber,speedOfSeedWheel);
+ #if defined(pcSerial)
+ #if defined(Qnca)
+ pc.printf("perim: %f pickN: %f sped: %f\n", seedPerimeter, pickNumber,speedOfSeedWheel);
+ #endif
#endif
- #endif
}
if (encoder==false) {
pulseRised2=1;
@@ -1869,10 +1843,13 @@
#if defined(speedMaster)
if ((tractorSpeed_MtS_timed==0.0f)) {
if ((firstStart==0)&&(simOk==0)&&(enableSimula==0)) {
- all_noSpeedSen=1;
+ cntSpeedError++;
+ if (cntSpeedError >= 3){
+ all_noSpeedSen=1;
+ }
}
}
- double oldLastPr = (double)oldLastPulseRead*1.5f;
+ double oldLastPr = (double)oldLastPulseRead*1.8f;
if((double)speedTimeOut.read_us()> (oldLastPr)) {
if ((firstStart==0)&&(simOk==0)&&(enableSimula==0)) {
all_speedError =1;
@@ -1883,30 +1860,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");
+ #if defined(pcSerial)
+ #if defined(checkLoopb)
+ pc.printf("forza\n");
+ #endif
#endif
- #endif
speedOfSeedWheel=0.001f;
}
aggioVelocita();
- /*
- realGiroSD = seedPerimeter / speedOfSeedWheel;
- tempoBecco = (realGiroSD/360.0f)*16000.0f;
- frequenzaReale = fixedStepGiroSD/realGiroSD;
- 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
- 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
- #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
@@ -1920,20 +1881,20 @@
}
if(countCicli>=cicliAspettaStart) {
aspettaStart=0;
-#if defined(pcSerial)
-#if defined(checkLoop)
- pc.printf("NoAspetta\n");
-#endif
-#endif
+ #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 defined(pcSerial)
+ #if defined(checkLoop)
+ pc.printf("BeccoNo\n");
+ #endif
+ #endif
}
if (trigTB==0) {
inhibit=1;
@@ -2107,6 +2068,7 @@
if ((pulseSpeedInterval>=0.0f)) { //minIntervalPulse)&&(pulseSpeedInterval<maxInterval)){
if((quincCnt<3)||(speedFromMaster==0.0f)||(enableSimula==1)) {
tractorSpeed_MtS_timed = ((pulseDistance / pulseSpeedInterval)); // tractor speed (unit= Mt/S) from pulse time interval
+ oldLocalTractorSpeed = tractorSpeed_MtS_timed;
}
if (checkSDrotation==0) {
checkSDrotation=1;
@@ -2119,9 +2081,10 @@
double oldLastPr = (double)oldLastPulseRead*1.7f;
if((double)speedTimeOut.read_us()> (oldLastPr)) {
tractorSpeed_MtS_timed = 0.0f;
-#if defined(seedSensor)
- resetDelay();
-#endif
+ oldLocalTractorSpeed=0.0f;
+ #if defined(seedSensor)
+ resetDelay();
+ #endif
pntMedia=0;
speedTimeOut.reset();
enableSpeed=0;
@@ -2129,16 +2092,16 @@
}
}
-#if defined(seedSensor)
- if (seedSensorEnable==true) {
- if (delaySeedCheck.read_ms()>100) {
- if (seedSee==0) {
- all_noSeedOnCe=1;
+ #if defined(seedSensor)
+ if (seedSensorEnable==true) {
+ if (delaySeedCheck.read_ms()>100) {
+ if (seedSee==0) {
+ all_noSeedOnCe=1;
+ }
+ resetDelay();
}
- resetDelay();
}
- }
-#endif
+ #endif
// esegue il controllo di velocità minima
/*if ((double)speedTimer.read_ms()>=maxInterval){
tractorSpeed_MtS_timed = 0.0f;
@@ -2154,6 +2117,7 @@
if (enableSimula==1) {
if(simOk==0) {
tractorSpeed_MtS_timed=0.0f;
+ oldLocalTractorSpeed=0.0f;
}
}
if ((tractorSpeed_MtS_timed>0.01f)) {
@@ -2180,9 +2144,8 @@
#endif
#endif
#if !defined(speedMaster)
- double pippo=0.0f;
- pippo = seedPerimeter / speedFromMaster;
- tempoBecchiPerQuinc = (pippo / pickNumber)*1000.0f;
+ double tempoGiroSDfomMaster = seedPerimeter / speedFromMaster;
+ tempoBecchiPerQuinc = (tempoGiroSDfomMaster / pickNumber)*1000.0f;
#endif
}
//*******************************************