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:
- 39:6814c75dfa5c
- Parent:
- 38:79af1c65dd6f
- Child:
- 40:42eae86457dd
--- a/main.cpp Mon Apr 27 15:01:00 2020 +0000
+++ b/main.cpp Mon May 04 11:02:06 2020 +0000
@@ -210,7 +210,6 @@
//}
}
#endif
-//*******************************************************************************
//********************************************************************************************************************
// ------------------------------------------------------------------------------------------------------------------------------------------------------------------
// TASK SECTION
@@ -223,8 +222,8 @@
frequenzaReale = fixedStepGiroSD/realGiroSD;
semiPeriodoReale = (1000000.0f/frequenzaReale);
seedWheelRPM = (speedOfSeedWheel)*K_WheelRPM ; // calcola i giri al minuto della ruota di semina 7.37 31,75
+ TBrpm = seedWheelRPM*rapportoRuote; // 5.896 31,75
#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
@@ -383,6 +382,20 @@
#endif
*/
}
+//*******************************************************************************
+void attivaTb(double velocita){
+ TBmotorRst=0;
+ #if defined(pcSerial)
+ #if defined(velocity)
+ pc.printf("velocita: %f\n",velocita);
+ #endif
+ #endif
+ TBticker.attach_us(&step_TBPulseOut,velocita); // clock time are milliseconds and attach seed motor stepper controls
+}
+void stopTb(){
+ TBticker.detach();
+ TBmotorRst=1;
+}
//*******************************************************
void invertiLo()
{
@@ -470,7 +483,7 @@
#endif
}
//*******************************************************
-void cambiaTB(double perio)
+void cambiaTB(double perio, int idxc)
{
#if defined(runner)
// update TB frequency
@@ -493,7 +506,7 @@
}
#else
// update TB frequency
- double limite=500.0f;
+ double limite=400.0f;
double TBper=0.0f;
double scala =2.0f;
if (aspettaStart==0) {
@@ -504,9 +517,9 @@
if (oldPeriodoTB!=TBper) {
if (TBper >= (limite/2.0f)) {
#if defined(pcSerial)
- #if defined(checkLoop)
+ #if defined(checkLoopc)
pc.printf("11a\n");
- pc.printf("11a TBper: %f \n",TBper);
+ pc.printf("11a TBper: %f perio: %f idx: %d\n",TBper,perio,idxc);
#endif
#endif
if (TBper != NULL) {
@@ -519,7 +532,9 @@
#endif
#endif
#endif
- TBticker.attach_us(&step_TBPulseOut,TBper); // clock time are milliseconds and attach seed motor stepper controls
+ //TBmotorRst=0;
+ //TBticker.attach_us(&step_TBPulseOut,TBper); // clock time are milliseconds and attach seed motor stepper controls
+ attivaTb(TBper);
}
} else {
#if defined(pcSerial)
@@ -527,7 +542,9 @@
pc.printf("11b\n");
#endif
#endif
- TBticker.detach();
+ //TBticker.detach();
+ //TBmotorRst=1;
+ stopTb();
#if defined(pcSerial)
#if defined(loStop)
pc.printf("A1\n");
@@ -584,6 +601,12 @@
double posError =0.0f;
double posSD=((double)SDactualPosition)/KcorT;
posError = posSD - (double)TBactualPosition;
+ #if defined(pcSerial)
+ #if defined(calcoli)
+ pc.printf("posSD: %f SDactual: %d TBactual: %d posErr: %f \n",posSD,SDactualPosition,TBactualPosition,posError);
+ #endif
+ #endif
+
// interviene sulla velocità di TB per raggiungere la corretta posizione relativa
if((lowSpeed==0)&&(aspettaStart==0)) {
double lowLim=-50.0f;
@@ -638,12 +661,19 @@
#endif
#endif
#else
- ePpos = periodo /(1.0f+ ((posError/divide)));
+ double variante = posError/100.0f;
+ //if (variante < -0.399f){variante=-0.3999;}
+ ePpos = periodo /(1.0f+ variante);
+ #if defined(pcSerial)
+ #if defined(calcoli)
+ pc.printf("variante: %f poserror: %f periodo: %f eppos: %f \n",variante,posError,periodo,ePpos);
+ #endif
+ #endif
#endif
- if (ePpos>0.0f) {
- cambiaTB(ePpos);
+ if ((ePpos>0.0f)&&(ePpos!=NULL)) {
+ cambiaTB(ePpos,1);
} else {
- cambiaTB(periodo);///2.0f);
+ cambiaTB(periodo,2);///2.0f);
}
}
}
@@ -709,7 +739,9 @@
#endif
#endif
#endif
- TBticker.attach_us(&step_TBPulseOut,TBperiod/2.0f); // clock time are milliseconds and attach seed motor stepper controls
+ //TBmotorRst=0;
+ //TBticker.attach_us(&step_TBPulseOut,TBperiod/2.0f); // clock time are milliseconds and attach seed motor stepper controls
+ attivaTb(TBperiod/2.0f);
}
}
#endif
@@ -736,7 +768,9 @@
motor->step_clock_mode_enable(StepperMotor::FWD);
#endif
#endif
- TBticker.attach_us(&step_TBPulseOut,1000.0f); // clock time are milliseconds and attach seed motor stepper controls
+ //TBmotorRst=0;
+ //TBticker.attach_us(&step_TBPulseOut,1000.0f); // clock time are milliseconds and attach seed motor stepper controls
+ attivaTb(1000.0f);
#endif
loadDaCanInCorso=1;
stopCicloTB=0;
@@ -748,7 +782,9 @@
#endif
#endif
#if !defined(runner)
- TBticker.detach();
+ //TBticker.detach();
+ //TBmotorRst=1;
+ stopTb();
#endif
#if defined(pcSerial)
#if defined(loStop)
@@ -1304,11 +1340,15 @@
} else {
tankLevelEnable=false;
}
- if ((flags&0x04)==0x04) {
- seedSensorEnable=true;
- } else {
+ #if !defined(oldStepperDriver)
+ if ((flags&0x04)==0x04) {
+ seedSensorEnable=true;
+ } else {
+ seedSensorEnable=false;
+ }
+ #else
seedSensorEnable=false;
- }
+ #endif
if ((flags&0x08)==0x08) {
drumSelect=true; // usare per selezione del tamburo =0 meccanico =1 PNEUMATICO
} else {
@@ -1884,13 +1924,20 @@
if (simOk==1){
if (simStepper==1){
oldSimStepper=true;
- simStepSpeed= ((double)speedStepp*180.45f)/50.0f;
+ #if defined(runner)
+ simStepSpeed= ((double)speedStepp*180.45f)/50.0f;
+ #endif
+ #if defined(oldStepperDriver)
+ simStepSpeed= 1.0f/(((double)speedStepp/250.0f)/100.0f);
+ #endif
if (oldSimStepSpeed!=simStepSpeed){
#if defined(runner)
motor->run(StepperMotor::FWD,simStepSpeed);
#endif
#if defined(oldStepperDriver)
- TBticker.attach_us(&step_TBPulseOut,simStepSpeed/2.0f); // clock time are milliseconds and attach seed motor stepper controls
+ //TBmotorRst=0;
+ //TBticker.attach_us(&step_TBPulseOut,simStepSpeed/2.0f); // clock time are milliseconds and attach seed motor stepper controls
+ attivaTb(simStepSpeed/2.0f);
#endif
oldSimStepSpeed=simStepSpeed;
}
@@ -1901,7 +1948,9 @@
motor->soft_hiz();
#endif
#if defined(oldStepperDriver)
- TBticker.detach();
+ //TBticker.detach();
+ //TBmotorRst=1;
+ stopTb();
#endif
oldSimStepSpeed=0.0f;
}
@@ -1913,7 +1962,9 @@
motor->soft_hiz();
#endif
#if defined(oldStepperDriver)
- TBticker.detach();
+ //TBticker.detach();
+ //TBmotorRst=1;
+ stopTb();
#endif
oldSimStepSpeed=0.0f;
}
@@ -2003,6 +2054,11 @@
if (timeIntraPick >= (memoIntraPick*2)) {
if ((aspettaStart==0)&&(alarmEnable==true)) {
if (firstStart==0) {
+ #if defined(pcSerial)
+ #if defined(tempoTb)
+ pc.printf("intra: %f memo: %f\n" , timeIntraPick, memoIntraPick);
+ #endif
+ #endif
all_pickSignal=1;
}
}
@@ -2224,6 +2280,11 @@
if ((tractorSpeed_MtS_timed>0.0f)&&(alarmEnable==true)){
if (((double)TBactualPosition > ((TBgiroStep/cellsNumber)*3.0f))||(cntTbError>5)) {
if (firstStart==0) {
+ #if defined(pcSerial)
+ #if defined(posit)
+ pc.printf("actual: %d giro: %f cells: %f cnt: %d", TBactualPosition,TBgiroStep, cellsNumber,cntTbError);
+ #endif
+ #endif
all_noStepRota=1;
#if defined(seedSensor)
resetDelay();
@@ -2364,7 +2425,7 @@
dutyTeorico = tabComan[0];
}
#if defined(oldStepperDriver)
- for (int ii = 0; ii<16; ii++) { // era ii<16
+ for (int ii = 0; ii<tabeling; ii++) { // era ii<16
#endif
#if defined(runner)
for (int ii = 0; ii<tabeling; ii++) { // era ii<16
@@ -2582,7 +2643,7 @@
pc.printf("da sincro\n");
#endif
#endif
- cambiaTB(periodo);
+ cambiaTB(periodo,3);
}
}
// controllo di stop
@@ -2598,7 +2659,9 @@
#endif
if (TBzeroCyclePulse==1) {
#if !defined(runner)
- TBticker.detach();
+ //TBticker.detach();
+ //TBmotorRst=1;
+ stopTb();
#endif
#if !defined(oldStepperDriver)
#if defined(runner)
@@ -2678,7 +2741,9 @@
#endif
#endif
#if !defined(runner)
- TBticker.detach();
+ //TBticker.detach();
+ //TBmotorRst=1;
+ stopTb();
#endif
#if defined(pcSerial)
#if defined(loStop)
@@ -2691,7 +2756,9 @@
motor->set_home();
#endif
#if defined(oldStepperDriver)
- TBticker.detach();
+ //TBticker.detach();
+ //TBmotorRst=1;
+ stopTb();
#endif
}
cntSpeedError=0;