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:
- 17:9629eb019892
- Parent:
- 16:786bb20a6759
- Child:
- 18:7c978f69cc51
--- a/main.cpp Sun Mar 31 15:11:53 2019 +0000
+++ b/main.cpp Fri Apr 05 07:52:39 2019 +0000
@@ -223,7 +223,7 @@
void aggioVelocita()
{
realGiroSD = seedPerimeter / speedOfSeedWheel;
- tempoBecco = realGiroSD*444.444f; //(realGiroSD/360.0f)*16000.0f;
+ tempoBecco = realGiroSD*44.4444f; //(realGiroSD/360.0f)*16000.0f;
frequenzaReale = fixedStepGiroSD/realGiroSD;
semiPeriodoReale = (1000000.0f/frequenzaReale);
seedWheelRPM = (speedOfSeedWheel)*K_WheelRPM ; // calcola i giri al minuto della ruota di semina 7.37 31,75
@@ -232,7 +232,11 @@
#if defined(Zucca)
TBperiod=5.2f*TBrpm*2.0f; //prova dopo test con contagiri
#else
- TBperiod=5.2f*TBrpm; //prova dopo test con contagiri
+ if (cellsNumber<8.0f){
+ TBperiod=4.8f*TBrpm; //prova dopo test con contagiri
+ }else{
+ TBperiod=5.2f*TBrpm; //prova dopo test con contagiri
+ }
#endif
#else
TBfrequency = (TBrpm*K_TBfrequency); // 130Hz a 0,29Mts 1397,00 a 1,25mt/s con 15 becchi e 15 celle
@@ -426,7 +430,7 @@
seedPerimeter = Pi * (seedWheelDiameter-(deepOfSeed*2.0f)); // perimeter of seed wheel
intraPickDistance = seedPerimeter/pickNumber;
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
+ //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
SDsectorStep = fixedStepGiroSD / pickNumber;
TBsectorStep = (TBmotorSteps*TBreductionRatio)/cellsNumber;
@@ -581,9 +585,9 @@
divide= 100.0f;
}
if (posError>higLim) {
- //posError=higLim;
- posError=0.0f;
- motor->soft_hiz();
+ posError=higLim;
+ //posError=0.0f;
+ //motor->soft_hiz();
}
if (posError<lowLim) {
posError=lowLim;
@@ -1050,7 +1054,7 @@
//if (tractorSpeed_MtS_timed==0.0f) {
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]/1000.0f); // deep of seeding
+ deepOfSeed=((double)rxMsg.data[2]/1000.0f); // deep of seeding
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)
@@ -1330,7 +1334,7 @@
if ((sincroQui==1)&&(quincStart==1)) {
if (countPicks==1) { //decelera
scostamento = oldQuincTimeSD;
- if (scostamento < (tempoBecchiPerQuinc*0.75f)) {
+ if ((scostamento < (tempoBecchiPerQuinc*0.85f))) {
double scostPerc = (scostamento/tempoBecchiPerQuinc);
percento -= ((double)quincPIDminus/100.0f)*(scostPerc);
#if defined(pcSerial)
@@ -1343,7 +1347,7 @@
}
if (countPicks==2) { //accelera
scostamento = (double)quincTime.read_ms();
- if (scostamento < (tempoBecchiPerQuinc*0.75f)) {
+ if (scostamento < (tempoBecchiPerQuinc*0.85f)) {
double scostPerc = (scostamento/tempoBecchiPerQuinc);
percento += ((double)quincPIDplus/100.0f)*(scostPerc);
#if defined(pcSerial)
@@ -1379,7 +1383,7 @@
if (canDataCheckEnable==true) {
if (canDataCheck==1) { // sincro da comunicazione can del valore di posizione del tamburo master
canDataCheck=0;
- double parametro = SDsectorStep/2.0f;
+ double parametro = SDsectorStep/(double)quincSector;
double differenza=0.0f;
#if defined(mezzo)
if (quinconceActive==1) {
@@ -1390,23 +1394,14 @@
#else
differenza = (double)mast2_Sinc - (double)prePosSD;
#endif
+ if (abs(differenza)<=50.0f){differenza=0.0f;}
if ((differenza > 0.0f)&&(differenza < parametro)) {
double diffPerc = differenza / parametro;
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);
- #endif
- #endif
}
if ((differenza < 0.0f)&&(abs(differenza) < parametro)) {
double diffPerc = (double)differenza / parametro;
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
}
// questo e il secondo
#if !defined(speedMaster)
@@ -1453,6 +1448,22 @@
}
#endif
+void aggiornati(){
+ #if defined(runner)
+ legPos.detach();
+ TBoldPosition= (uint32_t) motor->get_position();
+ legPos.attach(&step_Reading,0.002f);
+ #if defined(pcSerial)
+ #if defined(TBperS)
+ pc.printf("TBoldPos: %d\n",TBoldPosition);
+ #endif
+ #endif
+
+ #else
+ TBactualPosition=0;
+ #endif
+}
+
// ------------------------------------------------------------------------------------------------------------------------------------------------------------------
// MAIN SECTION
// ---------------------------------------------------------------------------------------------------------------------------------------------------------------
@@ -1575,9 +1586,10 @@
TBmotorDirecti=TBforward;
// definire il pin di clock che è PB_3
#if defined(Zucca)
- motor->run(StepperMotor::BWD,100.0f);
+ motor->run(StepperMotor::BWD,100.0f);
//motor->step_clock_mode_enable(StepperMotor::FWD);
#else
+ motor->run(StepperMotor::BWD,5.0f);
//motor->step_clock_mode_enable(StepperMotor::BWD);
#endif
// attiva l'out per il controllo dello stepper in stepClockMode
@@ -1741,6 +1753,10 @@
if (quinconceActive==0) {
posForQuinc=500;
}
+ if (checkSDrotation==0) {
+ checkSDrotation=1;
+ SDwheelTimer.start();
+ }
#endif
if (quincCnt<10) {
quincCnt++;
@@ -1783,10 +1799,12 @@
pc.printf(" SPEED: %f \n",tractorSpeed_MtS_timed);
#endif
#endif
- if (timeIntraPick >= (memoIntraPick*2)) {
- if ((aspettaStart==0)) {
- if (firstStart==0) {
- all_pickSignal=1;
+ if (tractorSpeed_MtS_timed>0.0f){
+ if (timeIntraPick >= (memoIntraPick*2)) {
+ if ((aspettaStart==0)) {
+ if (firstStart==0) {
+ all_pickSignal=1;
+ }
}
}
}
@@ -1811,10 +1829,12 @@
}
}
}
- double oldLastPr = (double)oldLastPulseRead*1.8f;
- if((double)speedTimeOut.read_us()> (oldLastPr)) {
- if ((firstStart==0)&&(simOk==0)&&(enableSimula==0)) {
- all_speedError =1;
+ if (tractorSpeed_MtS_timed>0.2f){
+ double oldLastPr = (double)oldLastPulseRead*1.8f;
+ if((double)speedTimeOut.read_us()> (oldLastPr)) {
+ if ((firstStart==0)&&(simOk==0)&&(enableSimula==0)) {
+ all_speedError =1;
+ }
}
}
#endif
@@ -1843,20 +1863,10 @@
}
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;
@@ -1961,44 +1971,55 @@
}
if ((double)TBactualPosition > ((TBgiroStep/cellsNumber)*1.8f)&&(erroreTamburo==0)) {
if (firstStart==0) {
- if (cntTbError>2) {
- all_cellSignal=1;
- #if defined(seedSensor)
- resetDelay();
- #endif
+ if (tractorSpeed_MtS_timed>0.5f){
+ if (cntTbError>5) {
+ all_cellSignal=1;
+ #if defined(seedSensor)
+ resetDelay();
+ #endif
+ }
}
}
if (erroreTamburo==0) {
- erroreTamburo=1;
- TBmotorDirecti=TBreverse; // rotazione inversa
- #if defined(runner)
- #if defined(Zucca)
- motor->run(StepperMotor::FWD);
+ if (cellsCountSet>1){
+ erroreTamburo=1;
+ TBmotorDirecti=TBreverse; // rotazione inversa
+ #if defined(runner)
+ #if defined(Zucca)
+ motor->run(StepperMotor::FWD);
+ #else
+ motor->run(StepperMotor::BWD);
+ #endif
#else
- motor->run(StepperMotor::BWD);
+ #if defined(Zucca)
+ motor->step_clock_mode_enable(StepperMotor::FWD);
+ #else
+ motor->step_clock_mode_enable(StepperMotor::BWD);
+ #endif
#endif
- #else
- #if defined(Zucca)
- motor->step_clock_mode_enable(StepperMotor::FWD);
- #else
- motor->step_clock_mode_enable(StepperMotor::BWD);
- #endif
- #endif
+ }
cntCellsForReload=0;
- cntTbError++;
+ if (tractorSpeed_MtS_timed>0.0f){
+ cntTbError++;
+ aggiornati();
+ }else{
+ cntTbError=0;
+ }
#if defined(seedSensor)
resetDelay();
#endif
}
}
- if (((double)TBactualPosition > ((TBgiroStep/cellsNumber)*3.0f))||(cntTbError>4)) {
- if (firstStart==0) {
- all_noStepRota=1;
- #if defined(seedSensor)
- resetDelay();
- #endif
+ if (tractorSpeed_MtS_timed>0.0f){
+ if (((double)TBactualPosition > ((TBgiroStep/cellsNumber)*3.0f))||(cntTbError>5)) {
+ if (firstStart==0) {
+ all_noStepRota=1;
+ #if defined(seedSensor)
+ resetDelay();
+ #endif
+ }
+ cntTbError=0;
}
- cntTbError=0;
}
// ----------------------------------------
// read and manage joystick
@@ -2037,10 +2058,10 @@
}
oldLocalTractorSpeed = tractorSpeed_MtS_timed;
}
- if (checkSDrotation==0) {
+ /*if (checkSDrotation==0) {
checkSDrotation=1;
SDwheelTimer.start();
- }
+ }*/
}
}
speedTimeOut.reset();
@@ -2104,7 +2125,7 @@
tempoTraBecchi_mS = (tempoGiroSD / 25.0f)*1000.0f; // tempo tra due impulsi dai becchi in millisecondi ( circa 157mS a 4,5Km/h)
}
} else {
- tempoTraBecchi_mS = (tempoGiroSD / (SDreductionRatio*25.5f))*1000.0f; // tempo tra due impulsi dai becchi in millisecondi ( circa 157mS a 4,5Km/h)
+ tempoTraBecchi_mS = (tempoGiroSD / (SDreductionRatio*dcPulseTurn))*1000.0f; // tempo tra due impulsi dai becchi in millisecondi ( circa 157mS a 4,5Km/h)
#if defined(pcSerial)
#if defined(Qnce)
pc.printf("tempoGiroSD: %f SDreductionRatio: %f tempoBecchi:%f\n",tempoGiroSD,SDreductionRatio,tempoTraBecchi_mS);
@@ -2123,16 +2144,16 @@
double setV=0.0f;
double teoriaC=0.0f;
double speedCorrectionMachine=0.0f;
- #if defined(speedMaster)
+ //#if defined(speedMaster)
speedCorrectionMachine=(seedWheelDiameter/(seedWheelDiameter-(deepOfSeed*2.0f)))*tractorSpeed_MtS_timed;
- #else
- speedCorrectionMachine=tractorSpeed_MtS_timed;
- #endif
+ //#else
+ // speedCorrectionMachine=tractorSpeed_MtS_timed;
+ //#endif
//if ((tractorSpeed_MtS_timed>0.0)&&(tractorSpeed_MtS_timed<tabSpeed[0])) {
- if ((speedCorrectionMachine>0.0)&&(speedCorrectionMachine<tabSpeed[0])) {
+ if ((speedCorrectionMachine>0.0f)&&(speedCorrectionMachine<tabSpeed[0])) {
dutyTeorico = tabComan[0];
}
- for (int ii = 0; ii<6; ii++) { // era ii<16
+ for (int ii = 0; ii<tabeling; ii++) { // era ii<16
if ((speedCorrectionMachine>=tabSpeed[ii])&&(speedCorrectionMachine<tabSpeed[ii+1])) {
// esegue l'interpolazione dei valori stimati di duty in base alla velocità
deltaV=tabSpeed[ii+1]-tabSpeed[ii];
@@ -2152,7 +2173,7 @@
//if (tractorSpeed_MtS_timed > tabSpeed[16]) {
// dutyTeorico=tractorSpeed_MtS_timed/maxWorkSpeed;
//}
- if (speedCorrectionMachine > tabSpeed[16]) {
+ if (speedCorrectionMachine > tabSpeed[tabeling]) {
dutyTeorico=speedCorrectionMachine/maxWorkSpeed;
}
#if !defined(speedMaster)
@@ -2299,22 +2320,24 @@
if (dcActualDuty > 0.95f) {
dcActualDuty = 0.95f;
}
- if (dcActualDuty > (dutyTeorico *1.05f)){dcActualDuty=dutyTeorico*1.05f;}
+ if (dcActualDuty > (dutyTeorico *1.15f)){dcActualDuty=dutyTeorico*1.15f;}
if (dcActualDuty < (dutyTeorico *0.85f)){dcActualDuty=dutyTeorico*0.85f;}
if (olddcActualDuty!=dcActualDuty) {
SDmotorPWM.write(1.0f-dcActualDuty);
olddcActualDuty=dcActualDuty;
}
// allarme
- if (SDwheelTimer.read_ms()>4000) {
- if (firstStart==0) {
- all_noDcRotati=1;
+ if (tractorSpeed_MtS_timed>0.0f){
+ if (SDwheelTimer.read_ms()>4000) {
+ if (firstStart==0) {
+ all_noDcRotati=1;
+ }
+ #if defined(pcSerial)
+ #if defined(VediAllarmi)
+ pc.printf("allarme no DC rotation");
+ #endif
+ #endif
}
- #if defined(pcSerial)
- #if defined(VediAllarmi)
- pc.printf("allarme no DC rotation");
- #endif
- #endif
}
//***************************************************************************************************************
@@ -2353,19 +2376,9 @@
#endif
#endif
if (TBzeroCyclePulse==1) {
- #if defined(pcSerial)
- #if defined(checkLoop)
- pc.printf("15f\n");
- #endif
- #endif
#if !defined(runner)
TBticker.detach();
#endif
- #if defined(pcSerial)
- #if defined(loStop)
- pc.printf("A4\n");
- #endif
- #endif
motor->soft_hiz();
}
}
@@ -2391,11 +2404,6 @@
if ((sincroTimer.read_ms()>= tempoDiSincro)) {
if (tractorSpeed_MtS_timed >= minWorkSpeed) {
startCicloTB=1;
- #if defined(pcSerial)
- #if defined(checkLoop)
- pc.printf("startTB\n");
- #endif
- #endif
}
beccoPronto=0;
}
@@ -2407,11 +2415,6 @@
beccoPronto=0;
}
}
- #if defined(pcSerial)
- #if defined(checkLoop)
- pc.printf("lowSpeed\n");
- #endif
- #endif
ciclaTB();
}
//*************************************************************