Forigo
/
FORIGO_Modula_V7_3_VdcStep_DICEMBRE2020
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed X_NUCLEO_IHM03A1_for
Diff: main.cpp
- Revision:
- 14:e2b5efa06c41
- Parent:
- 13:d1030d4e51a8
- Child:
- 16:786bb20a6759
--- a/main.cpp Mon Mar 25 15:42:04 2019 +0000
+++ b/main.cpp Sun Mar 31 14:51:36 2019 +0000
@@ -1045,18 +1045,19 @@
}
}
- if (tractorSpeed_MtS_timed <= 0.01f){
+ //if (tractorSpeed_MtS_timed <= 0.01f){
if (rxMsg.id==RX_Settings) {
- if (tractorSpeed_MtS_timed==0.0f) {
+ //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]/10000.0f); // deep of seeding
+ deepOfSeed=(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)
aggiornaParametri();
- }
+ //}
}
+ if (tractorSpeed_MtS_timed <= 0.01f){
if (rxMsg.id==RX_AngoloPh) {
if (tractorSpeed_MtS_timed==0.0f) {
#if defined(M1)
@@ -1362,12 +1363,12 @@
if (speedFromMaster>0.0f) {
if (enableSimula==0) {
tractorSpeed_MtS_timed = speedFromMaster + percento;
- if (tractorSpeed_MtS_timed > (oldLocalTractorSpeed*1.15f)){
+ /*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;
- }
+ }*/
}
}
}
@@ -1413,12 +1414,12 @@
if (speedFromMaster>0.0f) {
if (enableSimula==0) {
tractorSpeed_MtS_timed = speedFromMaster + percento;
- if (tractorSpeed_MtS_timed > (oldLocalTractorSpeed*1.15f)){
+ /*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;
- }
+ }*/
}
}
}
@@ -1915,7 +1916,7 @@
// ciclo conteggio celle per carico manuale
if (loadDaCanInCorso==1) {
cntCellsForLoad++;
- if (cntCellsForLoad >= 5) {
+ if (cntCellsForLoad >= 10) {
stopCicloTB=1;
cntCellsForLoad=0;
}
@@ -2121,22 +2122,38 @@
double deltaD=0.0f;
double setV=0.0f;
double teoriaC=0.0f;
-
- if ((tractorSpeed_MtS_timed>0.0)&&(tractorSpeed_MtS_timed<tabSpeed[0])) {
+ double speedCorrectionMachine=0.0f;
+ #if defined(speedMaster)
+ speedCorrectionMachine=(seedWheelDiameter/(seedWheelDiameter-(deepOfSeed*2.0f)))*tractorSpeed_MtS_timed;
+ #else
+ speedCorrectionMachine=tractorSpeed_MtS_timed;
+ #endif
+ //if ((tractorSpeed_MtS_timed>0.0)&&(tractorSpeed_MtS_timed<tabSpeed[0])) {
+ if ((speedCorrectionMachine>0.0)&&(speedCorrectionMachine<tabSpeed[0])) {
dutyTeorico = tabComan[0];
}
for (int ii = 0; ii<16; ii++) {
- if ((tractorSpeed_MtS_timed>=tabSpeed[ii])&&(tractorSpeed_MtS_timed<tabSpeed[ii+1])) {
+ 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];
deltaD=tabComan[ii+1]-tabComan[ii];
- setV = tractorSpeed_MtS_timed-tabSpeed[ii];
+ //setV = tractorSpeed_MtS_timed-tabSpeed[ii];
+ setV = speedCorrectionMachine-tabSpeed[ii];
teoriaC=(setV/deltaV)*deltaD;
dutyTeorico = tabComan[ii]+teoriaC; // era ii+1 al 23/03/19
+ #if defined(pcSerial)
+ #if defined(boost1)
+ pc.printf("tractor: %f tabspeed1: %f tabspeed2: %f\n",tractorSpeed_MtS_timed, tabSpeed[ii],tabSpeed[ii+1]);
+ pc.printf("deltaV: %f deltaD: %f setV: %f teoriaC %f \n",deltaV,deltaD,setV,teoriaC);
+ #endif
+ #endif
}
}
- if (tractorSpeed_MtS_timed > tabSpeed[16]) {
- dutyTeorico=tractorSpeed_MtS_timed/maxWorkSpeed;
+ //if (tractorSpeed_MtS_timed > tabSpeed[16]) {
+ // dutyTeorico=tractorSpeed_MtS_timed/maxWorkSpeed;
+ //}
+ if (speedCorrectionMachine > tabSpeed[16]) {
+ dutyTeorico=speedCorrectionMachine/maxWorkSpeed;
}
#if !defined(speedMaster)
quinCalc();
@@ -2199,9 +2216,15 @@
correzione = correzione + memoCorrezione;
if (correzione > (1.0f - dutyTeorico)) {
correzione = (1.0f - dutyTeorico);
+ #if defined(pcSerial)
+ pc.printf("limite\n");
+ #endif
}
if ((correzione < 0.0f)&&(dutyTeorico+correzione<0.0f)) {
correzione = -1.0f*dutyTeorico;
+ #if defined(pcSerial)
+ pc.printf("limite\n");
+ #endif
}
}
pulseRised1=0;
@@ -2224,6 +2247,7 @@
#endif
}
// introduce il controllo di corrente
+ double variazione=0.0f;
if (correggiCorrente==1){
if (SD_CurrentScaled < 1.6f){
boostDcOut +=0.01f;
@@ -2233,17 +2257,18 @@
}
if (boostDcOut >= 0.2f){
boostDcOut=0.2f;
- all_genericals=1;
+ //all_genericals=1;
}
if (boostDcOut <=-0.2f){
boostDcOut=-0.2f;
- all_genericals=1;
+ //all_genericals=1;
}
+ variazione=boostDcOut;
correggiCorrente=0;
}
if (currentCheckEnable==true){
- correzione += boostDcOut;
- boostDcOut=0.0f;
+ correzione += variazione;
+ variazione=0.0f;
}
DC_brake=0;
DC_forward=1;
@@ -2274,6 +2299,8 @@
if (dcActualDuty > 0.95f) {
dcActualDuty = 0.95f;
}
+ if (dcActualDuty > (dutyTeorico *1.05f)){dcActualDuty=dutyTeorico*1.05f;}
+ if (dcActualDuty < (dutyTeorico *0.85f)){dcActualDuty=dutyTeorico*0.85f;}
if (olddcActualDuty!=dcActualDuty) {
SDmotorPWM.write(1.0f-dcActualDuty);
olddcActualDuty=dcActualDuty;