Francesco Pistone / Mbed 2 deprecated FORIGO_Modula_V4_R5_DoubleEnc_2019

new_2019

Dependencies:   mbed CANMsg

main.cpp@4:8c64ea251890, 2019-01-31 (annotated)

Committer:
francescopistone
Date:
Thu Jan 31 07:32:31 2019 +0000
Branch:
BOX_ARGENTO_2019
Revision:
4:8c64ea251890
Parent:
3:36da019e6bb6 
BOX_ARGENTO_2019

Who changed what in which revision?

UserRevisionLine numberNew contents of line
nerit 3:36da019e6bb6 1 //********************************************************************************************************************
nerit 3:36da019e6bb6 2 //********************************************************************************************************************
nerit 3:36da019e6bb6 3 // FIRMWARE SEMINATRICE MODULA
nerit 3:36da019e6bb6 4 // VERSIONE PER SCHEDA DI CONTROLLO CON DRIVER INTEGRATI
nerit 3:36da019e6bb6 5 // V4 - ATTENZIONE - LA VERSIONE V4 HA IL DRIVER STEPPER LV8727
nerit 3:36da019e6bb6 6 // IL MOTORE DC E' GESTITO CON IL DRIVER VNH3SP30-E E CON LA LETTURA
nerit 3:36da019e6bb6 7 // DELLA CORRENTE ASSORBITA TRAMITE IL CONVERTITORE MLX91210-CAS102 CON 50A FONDOSCALA
nerit 3:36da019e6bb6 8 // CHE FORNISCE UNA TENSIONE DI USCITA PARI A 40mV/A
nerit 3:36da019e6bb6 9 // FIRST RELEASE OF BOARD DEC 2017
nerit 3:36da019e6bb6 10 // FIRST RELEASE OF FIRMWARE JAN 2018
nerit 3:36da019e6bb6 11 //
nerit 3:36da019e6bb6 12 // THIS RELEASE: 29 MAJ 2018
nerit 3:36da019e6bb6 13 //
nerit 3:36da019e6bb6 14 // APPLICATION: MODULA CON DISTRIBUTORE ZUCCA OPPURE RISO E PUO' FUNZIONARE ANCHE CON SENSORE A 25 FORI SUL DISCO O
nerit 3:36da019e6bb6 15 // ENCODER MOTORE SETTANDO GLI APPOSITI FLAGS
nerit 3:36da019e6bb6 16 //
nerit 3:36da019e6bb6 17 // 29 05 2018 - INSERITO SECONDO ENCODER VIRTUALE PER LA GESTIONE DEL SINCRONISMO TRA TAMBURO E RUOTA DI SEMINA
nerit 3:36da019e6bb6 18 // IN PRATICA IL PRIMO ENCODER è SINCRONO CON IL SEGNALE DEI BECCHI E VIENE AZZERATO DA QUESTI, MENTRE
nerit 3:36da019e6bb6 19 // IL SECONDO E' INCREMENTATO IN SINCRONO CON IL PRIMO MA AZZERATO DALLA FASE. IL SUO VALORE E' POI DIVISO
nerit 3:36da019e6bb6 20 // PER IL RAPPORTO RUOTE E LA CORREZIONE AGISCE SULLA VELOCITA' DEL TAMBURO PER MANTENERE LA FASE DEL SECONDO
nerit 3:36da019e6bb6 21 // ENCODER
nerit 3:36da019e6bb6 22 // 05 06 2018 - INSERITO IL CONTROLLO DI GESTIONE DEL QUINCONCE SENZA ENCODER
nerit 3:36da019e6bb6 23 //
nerit 3:36da019e6bb6 24 //********************************************************************************************************************
nerit 3:36da019e6bb6 25 //********************************************************************************************************************
nerit 3:36da019e6bb6 26
nerit 3:36da019e6bb6 27 #include "main.hpp"
nerit 3:36da019e6bb6 28 #include "timeandtick.hpp"
nerit 3:36da019e6bb6 29 #include "canbus.hpp"
nerit 3:36da019e6bb6 30 #include "iodefinition.hpp"
nerit 3:36da019e6bb6 31 #include "parameters.hpp"
nerit 3:36da019e6bb6 32 #include "variables.hpp"
nerit 3:36da019e6bb6 33 //********************************************************************************************************************
nerit 3:36da019e6bb6 34 //********************************************************************************************************************
nerit 3:36da019e6bb6 35 // ------------------------------------------------------------------------------------------------------------------------------------------------------------------
nerit 3:36da019e6bb6 36 // TASK SECTION
nerit 3:36da019e6bb6 37 // ------------------------------------------------------------------------------------------------------------------------------------------------------------------
nerit 3:36da019e6bb6 38 //************************************************************************
nerit 3:36da019e6bb6 39 // rise of seed speed 25 pulse sensor
nerit 3:36da019e6bb6 40 void sd25Fall(){
nerit 3:36da019e6bb6 41 timeHole=metalTimer.read_ms();
nerit 3:36da019e6bb6 42 int memo_TimeHole= (memoTimeHole + timeHole)/ (int)2;
nerit 3:36da019e6bb6 43 memoTimeHole = timeHole;
nerit 3:36da019e6bb6 44 metalTimer.reset();
nerit 3:36da019e6bb6 45 if (speedFromPick==0){
nerit 3:36da019e6bb6 46 speedOfSeedWheel=((seedPerimeter/25.0f)/(double)memo_TimeHole)*1000.0f; //mtS
nerit 3:36da019e6bb6 47 }
nerit 3:36da019e6bb6 48 }
nerit 3:36da019e6bb6 49 //**************************************************
nerit 3:36da019e6bb6 50 // generate speed clock when speed is simulated from Tritecnica display
nerit 3:36da019e6bb6 51 void speedSimulationClock(){
nerit 3:36da019e6bb6 52 lastPulseRead=speedTimer.read_us();
nerit 3:36da019e6bb6 53 oldLastPulseRead=lastPulseRead;
nerit 3:36da019e6bb6 54 speedTimer.reset();
nerit 3:36da019e6bb6 55 pulseRised=1;
nerit 3:36da019e6bb6 56 }
nerit 3:36da019e6bb6 57 //*******************************************************
nerit 3:36da019e6bb6 58 // interrupt task for tractor speed reading
nerit 3:36da019e6bb6 59 //*******************************************************
nerit 3:36da019e6bb6 60 void tractorReadSpeed(){
nerit 3:36da019e6bb6 61 if ((oldTractorSpeedRead==0)){
nerit 3:36da019e6bb6 62 lastPulseRead=speedTimer.read_us();
nerit 3:36da019e6bb6 63 oldLastPulseRead=lastPulseRead;
nerit 3:36da019e6bb6 64 speedTimer.reset();
nerit 3:36da019e6bb6 65 pulseRised=1;
nerit 3:36da019e6bb6 66 oldTractorSpeedRead=1;
nerit 3:36da019e6bb6 67 }
nerit 3:36da019e6bb6 68 speedFilter.reset();
nerit 3:36da019e6bb6 69 speedClock=1;
nerit 3:36da019e6bb6 70 }
nerit 3:36da019e6bb6 71 //*******************************************************
nerit 3:36da019e6bb6 72 void speedMediaCalc(){
nerit 3:36da019e6bb6 73 double lastPd=(double) lastPulseRead/1000.0f;
nerit 3:36da019e6bb6 74 pulseSpeedInterval = (mediaSpeed[0]+lastPd)/2.0f;
nerit 3:36da019e6bb6 75 if (enableSimula==1){
nerit 3:36da019e6bb6 76 double TMT = (double)(speedSimula) * 100.0f /3600.0f;
nerit 3:36da019e6bb6 77 pulseSpeedInterval = pulseDistance / TMT;
nerit 3:36da019e6bb6 78 }
nerit 3:36da019e6bb6 79 mediaSpeed[0]=lastPd;
nerit 3:36da019e6bb6 80 OLDpulseSpeedInterval=pulseSpeedInterval;
nerit 3:36da019e6bb6 81 }
hudakz 1:6f8ffb2c2dd7 82
nerit 3:36da019e6bb6 83 //*******************************************************
nerit 3:36da019e6bb6 84 // clocked task for manage virtual encoder of seed wheel i/o
nerit 3:36da019e6bb6 85 //*******************************************************
nerit 3:36da019e6bb6 86 //*******************************************************
nerit 3:36da019e6bb6 87 void step_SDPulseOut(){
nerit 3:36da019e6bb6 88 SDactualPosition++;
nerit 3:36da019e6bb6 89 prePosSD++;
nerit 3:36da019e6bb6 90 }
nerit 3:36da019e6bb6 91 //*******************************************************
nerit 3:36da019e6bb6 92 void step_TBPulseOut(){
nerit 3:36da019e6bb6 93 TBmotorStepOut=!TBmotorStepOut;
nerit 3:36da019e6bb6 94 if (TBmotorStepOut==0){
nerit 3:36da019e6bb6 95 TBactualPosition++;
nerit 3:36da019e6bb6 96 }
nerit 3:36da019e6bb6 97 }
nerit 3:36da019e6bb6 98 //*******************************************************
nerit 3:36da019e6bb6 99 // aggiornamento parametri di lavoro fissi e da Tritecnica
nerit 3:36da019e6bb6 100 void aggiornaParametri(){
nerit 3:36da019e6bb6 101 speedPerimeter = Pi * speedWheelDiameter ; // perimeter of speed wheel
nerit 3:36da019e6bb6 102 pulseDistance = (speedPerimeter / speedWheelPulse)*1000.0f; // linear space between speed wheel pulse
nerit 3:36da019e6bb6 103 seedPerimeter = Pi * (seedWheelDiameter-(deepOfSeed*2.0f)); // perimeter of seed wheel
nerit 3:36da019e6bb6 104 intraPickDistance = seedPerimeter/pickNumber;
nerit 3:36da019e6bb6 105 K_WheelRPM = 60.0f/seedPerimeter; // calcola il K per i giri al minuto della ruota di semina
nerit 3:36da019e6bb6 106 K_WhellFrequency = (seedWheelMotorSteps*SDreductionRatio)/60.0f; // calcola il K per la frequenza di comando del motore di semina
nerit 3:36da019e6bb6 107 rapportoRuote = pickNumber/cellsNumber; // calcola il rapporto tra il numero di becchi ed il numero di celle
nerit 3:36da019e6bb6 108 K_TBfrequency = (TBmotorSteps*TBreductionRatio)/240.0f;
nerit 3:36da019e6bb6 109 K_percentuale = TBmotorSteps*TBreductionRatio;
nerit 3:36da019e6bb6 110 SDsectorStep = (double)fixedStepGiroSD / (double)pickNumber;
nerit 3:36da019e6bb6 111 TBsectorStep = (TBmotorSteps*TBreductionRatio)/cellsNumber;
nerit 3:36da019e6bb6 112 KcorT = (SDsectorStep/TBsectorStep);///2.0f;
nerit 3:36da019e6bb6 113 angoloFase=angoloPh;
nerit 3:36da019e6bb6 114 avvioGradi=angoloAv;
nerit 3:36da019e6bb6 115 stepGrado=fixedStepGiroSD/360.0f;
nerit 3:36da019e6bb6 116 TBdeltaStep=(fixedStepGiroSD/pickNumber)+(stepGrado*avvioGradi);
nerit 3:36da019e6bb6 117 TBfaseStep = (stepGrado*angoloFase);
nerit 3:36da019e6bb6 118 K_WheelRPM = 60.0f/seedPerimeter; // calcola il K per i giri al minuto della ruota di semina 25.4 25,40
nerit 3:36da019e6bb6 119 if (speedFromPick==1) {
nerit 3:36da019e6bb6 120 intraPickDistance = seedPerimeter/pickNumber;
nerit 3:36da019e6bb6 121 }else{
nerit 3:36da019e6bb6 122 intraPickDistance = seedPerimeter/25.0f; // 25 è il numero di fori presenti nel disco di semina
nerit 3:36da019e6bb6 123 }
nerit 3:36da019e6bb6 124 if (anticipoMax > ((360.0f/pickNumber)-(avvioGradi+16.0f))){
nerit 3:36da019e6bb6 125 anticipoMax=(360.0f/pickNumber)-(avvioGradi+16.0f);
nerit 3:36da019e6bb6 126 }
nerit 3:36da019e6bb6 127 }
nerit 3:36da019e6bb6 128 //*******************************************************
nerit 3:36da019e6bb6 129 void cambiaTB(double perio){
nerit 3:36da019e6bb6 130 // update TB frequency
nerit 3:36da019e6bb6 131 double TBper=0.0f;
nerit 3:36da019e6bb6 132 if (aspettaStart==0){
nerit 3:36da019e6bb6 133 if (perio<250.0f){perio=500.0f;}
nerit 3:36da019e6bb6 134 double scala =0.0f;
nerit 3:36da019e6bb6 135 if (lowSpeed==1){
nerit 3:36da019e6bb6 136 scala =2.0f;
nerit 3:36da019e6bb6 137 }else{
nerit 3:36da019e6bb6 138 scala =1.8f;
nerit 3:36da019e6bb6 139 }
nerit 3:36da019e6bb6 140 TBper=perio/scala;
nerit 3:36da019e6bb6 141 if (oldPeriodoTB!=TBper){
nerit 3:36da019e6bb6 142 if (TBper >= 250.0f){
nerit 3:36da019e6bb6 143 TBticker.attach_us(&step_TBPulseOut,TBper); // clock time are milliseconds and attach seed motor stepper controls
nerit 3:36da019e6bb6 144 }else{
nerit 3:36da019e6bb6 145 TBticker.detach();
nerit 3:36da019e6bb6 146 }
nerit 3:36da019e6bb6 147 oldPeriodoTB=TBper;
nerit 3:36da019e6bb6 148 }
nerit 3:36da019e6bb6 149 }
nerit 3:36da019e6bb6 150 }
nerit 3:36da019e6bb6 151 //*******************************************************
nerit 3:36da019e6bb6 152 void seedCorrect(){
nerit 3:36da019e6bb6 153 /*
nerit 3:36da019e6bb6 154 posError determina la posizione relativa di TB rispetto ad SD
nerit 3:36da019e6bb6 155 la reale posizione di SD viene modificata in funzione della velocità per
nerit 3:36da019e6bb6 156 traslare la posizione relativa di TB. All'aumentare della velocità la posizione
nerit 3:36da019e6bb6 157 di SD viene incrementata così che TB acceleri per raggiungerla in modo da rilasciare il seme prima
nerit 3:36da019e6bb6 158 La taratura del sistema avviene determinando prima il valore di angoloFase alla minima velocità,
nerit 3:36da019e6bb6 159 poi, alla massima velocità, dovrebbe spostarsi la posizione relativa con una variabile proporzionale alla velocità, ma c'è un però.
nerit 3:36da019e6bb6 160 Il problema è che il momento di avvio determina una correzione dell'angolo di partenza del tamburo
nerit 3:36da019e6bb6 161 angolo che viene rideterminato ogni volta che il sensore becchi legge un transito.
nerit 3:36da019e6bb6 162 Di fatto c'è una concorrenza tra l'angolo di avvio determinato e la correzione di posizione relativa
nerit 3:36da019e6bb6 163 del tamburo. E' molto probabile che convenga modificare solo la posizione relativa e non anche l'angolo di avvio
nerit 3:36da019e6bb6 164 Ancora di più se viene eliminata la parte gestita da ciclata.
nerit 3:36da019e6bb6 165 In questo modo dovrebbe esserci solo un andamento in accelerazione di TB che viene poi eventualmente decelerato
nerit 3:36da019e6bb6 166 dal passaggio sul sensore di TB. Funzione corretta perchè il sincronismo tra i sensori genera l'inibizione della correzione
nerit 3:36da019e6bb6 167 di fase di TB. In pratica il ciclo viene resettato al passaggio sul sensore di SD che riporta a 0 la posizione di SD.
nerit 3:36da019e6bb6 168 Appena il sensore di TB viene impegnato allora viene abilitato il controllo di fase del tamburo.
nerit 3:36da019e6bb6 169 Questo si traduce nel fatto che il controllo di posizione viene gestito solo all'interno di uno slot di semina in modo che
nerit 3:36da019e6bb6 170 il tamburo non risenta della condizione di reset della posizione di SD mentre lui è ancora nella fase precedente. Si fermerebbe.
nerit 3:36da019e6bb6 171
nerit 3:36da019e6bb6 172 // La considerazione finale è che mantenendo l'angolo di avvio fisso e regolato sulla bassa velocità, intervenendo solo sulla correzione
nerit 3:36da019e6bb6 173 // di posizione in questa routine, dovrebbe essere possibile seminare correttamente a tutte le velocità regolando solo 2 parametri.
nerit 3:36da019e6bb6 174 */
nerit 3:36da019e6bb6 175 /*
nerit 3:36da019e6bb6 176 SDsectorStep = (double)fixedStepGiroSD / (double)pickNumber;
nerit 3:36da019e6bb6 177 TBsectorStep = (TBmotorSteps*TBreductionRatio)/cellsNumber;
nerit 3:36da019e6bb6 178 KcorT = (SDsectorStep/TBsectorStep);
nerit 3:36da019e6bb6 179 angoloFase=angoloPh;
nerit 3:36da019e6bb6 180 stepGrado=fixedStepGiroSD/360.0f;
nerit 3:36da019e6bb6 181 avvioGradi = costante da terminale tritecnica
nerit 3:36da019e6bb6 182 TBdeltaStep=(fixedStepGiroSD/pickNumber)-(stepGrado*avvioGradi);
nerit 3:36da019e6bb6 183 TBfaseStep = (stepGrado*angoloFase);
nerit 3:36da019e6bb6 184 anticipoMax = valore in gradi del massimo anticipo proporzionale alla velocità della ruota di semina
nerit 3:36da019e6bb6 185 */
nerit 3:36da019e6bb6 186
nerit 3:36da019e6bb6 187 if ((tractorSpeed_MtS_timed>0.01f)){
nerit 3:36da019e6bb6 188 if (inhibit==0){
nerit 3:36da019e6bb6 189 double posError =0.0f;
nerit 3:36da019e6bb6 190 double posSD=((double)SDactualPosition)/KcorT;
nerit 3:36da019e6bb6 191 posError = posSD - (double)TBactualPosition;
nerit 3:36da019e6bb6 192 // interviene sulla velocità di TB per raggiungere la corretta posizione relativa
nerit 3:36da019e6bb6 193 if((lowSpeed==0)&&(aspettaStart==0)){
nerit 3:36da019e6bb6 194 if (posError>50.0f){posError=50.0f;}
nerit 3:36da019e6bb6 195 if (posError<-50.0f){posError=-50.0f;}
nerit 3:36da019e6bb6 196 if ((posError >=1.0f)||(posError<=-1.0f)){
nerit 3:36da019e6bb6 197 ePpos = periodo /(1.0f+ ((posError/100.0f)));
nerit 3:36da019e6bb6 198 cambiaTB(ePpos);
nerit 3:36da019e6bb6 199 }
nerit 3:36da019e6bb6 200 }
nerit 3:36da019e6bb6 201 }
nerit 3:36da019e6bb6 202 #if defined(pcSerial)
nerit 3:36da019e6bb6 203 #if defined(SDreset)
nerit 3:36da019e6bb6 204 pc.printf("Fase: %d",fase);
nerit 3:36da019e6bb6 205 pc.printf(" PrePosSD: %d",prePosSD);
nerit 3:36da019e6bb6 206 pc.printf(" PosSD: %d",SDactualPosition);
nerit 3:36da019e6bb6 207 pc.printf(" speed: %f",tractorSpeed_MtS_timed);
nerit 3:36da019e6bb6 208 pc.printf(" Trigger: %d \n", trigRepos);
nerit 3:36da019e6bb6 209 #endif
nerit 3:36da019e6bb6 210 #endif
nerit 3:36da019e6bb6 211 }
nerit 3:36da019e6bb6 212 }
nerit 3:36da019e6bb6 213 //*******************************************************
nerit 3:36da019e6bb6 214 void videoUpdate(){
nerit 3:36da019e6bb6 215 for(int aa=0;aa<4;aa++){speedForDisplay[aa]=speedForDisplay[aa+1];}
nerit 3:36da019e6bb6 216 speedForDisplay[4]=tractorSpeed_MtS_timed;
nerit 3:36da019e6bb6 217 totalSpeed=0.0f;
nerit 3:36da019e6bb6 218 for (int aa=0; aa<5; aa++){totalSpeed += speedForDisplay[aa];}
nerit 3:36da019e6bb6 219 totalSpeed = totalSpeed / 5.0f;
nerit 3:36da019e6bb6 220 }
nerit 3:36da019e6bb6 221 //*******************************************************
nerit 3:36da019e6bb6 222 void ciclaTB(){
nerit 3:36da019e6bb6 223 if ((startCicloTB==1)&&(cicloTbinCorso==0)){
nerit 3:36da019e6bb6 224 TBticker.attach_us(&step_TBPulseOut,TBperiod/2.5f); // clock time are milliseconds and attach seed motor stepper controls
nerit 3:36da019e6bb6 225 cicloTbinCorso = 1;
nerit 3:36da019e6bb6 226 startCicloTB=0;
nerit 3:36da019e6bb6 227 }
nerit 3:36da019e6bb6 228 if ((loadDaCan==1)&&(loadDaCanInCorso==0)){
nerit 3:36da019e6bb6 229 TBticker.attach_us(&step_TBPulseOut,1000.0f); // clock time are milliseconds and attach seed motor stepper controls
nerit 3:36da019e6bb6 230 loadDaCanInCorso=1;
nerit 3:36da019e6bb6 231 stopCicloTB=0;
nerit 3:36da019e6bb6 232 }
nerit 3:36da019e6bb6 233 if ((stopCicloTB==1)&&(TBactualPosition>5)){
nerit 3:36da019e6bb6 234 TBticker.detach();
nerit 3:36da019e6bb6 235 cicloTbinCorso = 0;
nerit 3:36da019e6bb6 236 stopCicloTB=0;
nerit 3:36da019e6bb6 237 loadDaCanInCorso=0;
nerit 3:36da019e6bb6 238 loadDaCan=0;
nerit 3:36da019e6bb6 239 }
nerit 3:36da019e6bb6 240 }
nerit 3:36da019e6bb6 241 //*******************************************************
nerit 3:36da019e6bb6 242 void allarmi(){
nerit 3:36da019e6bb6 243 uint8_t alarmLowRegister1=0x00;
nerit 3:36da019e6bb6 244 alarmLowRegister=0x00;
nerit 3:36da019e6bb6 245 alarmHighRegister=0x80;
nerit 3:36da019e6bb6 246
nerit 3:36da019e6bb6 247 //alarmLowRegister=alarmLowRegister+(all_semiFiniti*0x01); // manca il sensore
nerit 3:36da019e6bb6 248 alarmLowRegister=alarmLowRegister+(all_pickSignal*0x02); // fatto
nerit 3:36da019e6bb6 249 alarmLowRegister=alarmLowRegister+(all_cellSignal*0x04); // fatto
nerit 3:36da019e6bb6 250 //alarmLowRegister=alarmLowRegister+(all_lowBattery*0x08); // non gestito
nerit 3:36da019e6bb6 251 //alarmLowRegister=alarmLowRegister+(all_overCurrDC*0x10);
nerit 3:36da019e6bb6 252 alarmLowRegister=alarmLowRegister+(all_stopSistem*0x20); // verificarne la necessità
nerit 3:36da019e6bb6 253 //alarmLowRegister=alarmLowRegister+(all_upElements*0x40); // manca il sensore
nerit 3:36da019e6bb6 254 //alarmLowRegister=alarmLowRegister+(all_noSeedOnCe*0x80); // manca il sensore
nerit 3:36da019e6bb6 255
nerit 3:36da019e6bb6 256 //alarmLowRegister1=alarmLowRegister1+(all_cfgnErrors*0x01); // da scrivere
nerit 3:36da019e6bb6 257 alarmLowRegister1=alarmLowRegister1+(all_noDcRotati*0x02); // fatto
nerit 3:36da019e6bb6 258 alarmLowRegister1=alarmLowRegister1+(all_noStepRota*0x04); // fatto
nerit 3:36da019e6bb6 259 alarmLowRegister1=alarmLowRegister1+(all_speedError*0x08); // fatto
nerit 3:36da019e6bb6 260 alarmLowRegister1=alarmLowRegister1+(all_noSpeedSen*0x10); // fatto
nerit 3:36da019e6bb6 261 alarmLowRegister1=alarmLowRegister1+(all_no_Zeroing*0x20); // fatto
nerit 3:36da019e6bb6 262 alarmLowRegister1=alarmLowRegister1+(all_genericals*0x40);
nerit 3:36da019e6bb6 263 if (alarmLowRegister1 > 0){
nerit 3:36da019e6bb6 264 alarmHighRegister = 0x81;
nerit 3:36da019e6bb6 265 alarmLowRegister = alarmLowRegister1;
nerit 3:36da019e6bb6 266 }
hudakz 0:1b9561cd1c36 267
nerit 3:36da019e6bb6 268 #if defined(pcSerial)
nerit 3:36da019e6bb6 269 #if defined(VediAllarmi)
nerit 3:36da019e6bb6 270 if (all_pickSignal==1){pc.printf("AllarmeBecchi\n");}
nerit 3:36da019e6bb6 271 if (all_cellSignal==1){pc.printf("AllarmeCelle\n");}
nerit 3:36da019e6bb6 272 if (all_stopSistem==1){pc.printf("AllarmeStop\n");}
nerit 3:36da019e6bb6 273 if (all_noDcRotati==1){pc.printf("AllarmeDCnoRotation\n");}
nerit 3:36da019e6bb6 274 if (all_noStepRota==1){pc.printf("AllarmeNoStepRotation\n");}
nerit 3:36da019e6bb6 275 if (all_speedError==1){pc.printf("AllarmeSpeedError\n");}
nerit 3:36da019e6bb6 276 if (all_noSpeedSen==1){pc.printf("AllarmeNoSpeedSensor\n");}
nerit 3:36da019e6bb6 277 if (all_no_Zeroing==1){pc.printf("AllarmeNoZero\n");}
nerit 3:36da019e6bb6 278 if (all_genericals==1){pc.printf("AllarmeGenerico\n");}
nerit 3:36da019e6bb6 279 pc.printf("Code: 0x%x%x\n",alarmHighRegister,alarmLowRegister);
nerit 3:36da019e6bb6 280 #endif
nerit 3:36da019e6bb6 281 #endif
nerit 3:36da019e6bb6 282 all_semiFiniti=0;
nerit 3:36da019e6bb6 283 all_pickSignal=0;
nerit 3:36da019e6bb6 284 all_cellSignal=0;
nerit 3:36da019e6bb6 285 all_lowBattery=0;
nerit 3:36da019e6bb6 286 all_overCurrDC=0;
nerit 3:36da019e6bb6 287 all_stopSistem=0;
nerit 3:36da019e6bb6 288 all_upElements=0;
nerit 3:36da019e6bb6 289 all_noSeedOnCe=0;
nerit 3:36da019e6bb6 290 all_cfgnErrors=0;
nerit 3:36da019e6bb6 291 all_noDcRotati=0;
nerit 3:36da019e6bb6 292 all_noStepRota=0;
nerit 3:36da019e6bb6 293 all_speedError=0;
nerit 3:36da019e6bb6 294 all_noSpeedSen=0;
nerit 3:36da019e6bb6 295 all_no_Zeroing=0;
nerit 3:36da019e6bb6 296 all_genericals=0;
nerit 3:36da019e6bb6 297 }
nerit 3:36da019e6bb6 298 //*******************************************************
nerit 3:36da019e6bb6 299 void upDateSincro(){
nerit 3:36da019e6bb6 300 char val1[8]={0,0,0,0,0,0,0,0};
nerit 3:36da019e6bb6 301 val1[0]=(prePosSD /0x00FF0000)&0x000000FF;
nerit 3:36da019e6bb6 302 val1[1]=(prePosSD /0x0000FF00)&0x000000FF;
nerit 3:36da019e6bb6 303 val1[2]=(prePosSD /0x000000FF)&0x000000FF;
nerit 3:36da019e6bb6 304 val1[3]=prePosSD & 0x000000FF;
nerit 3:36da019e6bb6 305 #if defined(canbusActive)
nerit 3:36da019e6bb6 306 #if defined(speedMaster)
nerit 3:36da019e6bb6 307 if(can1.write(CANMessage(TX_SI, *&val1,8))){
nerit 3:36da019e6bb6 308 checkState=0;
nerit 3:36da019e6bb6 309 }
nerit 3:36da019e6bb6 310 #endif
nerit 3:36da019e6bb6 311 #endif
nerit 3:36da019e6bb6 312 }
nerit 3:36da019e6bb6 313 //*******************************************************
nerit 3:36da019e6bb6 314 void upDateSpeed(){
nerit 3:36da019e6bb6 315 /*
nerit 3:36da019e6bb6 316 aggiorna dati OPUSA3
nerit 3:36da019e6bb6 317 val1[0] contiene il dato di velocità
nerit 3:36da019e6bb6 318 val1[1] contiene il byte basso della tabella allarmi
nerit 3:36da019e6bb6 319 uint8_t all_semiFiniti = 0; // semi finiti (generato dal relativo sensore)
nerit 3:36da019e6bb6 320 uint8_t all_pickSignal = 0; // errore segnale becchi (generato dal tempo tra un becco ed il successivo)
nerit 3:36da019e6bb6 321 uint8_t all_cellSignal = 0; // errore segnale celle (generato dal sensore tamburo )
nerit 3:36da019e6bb6 322 uint8_t all_lowBattery = 0; // allarme batteria (valore interno di tritecnica)
nerit 3:36da019e6bb6 323 uint8_t all_overCurrDC = 0; // sovracorrente motore DC (generato dal sensore di corrente)
nerit 3:36da019e6bb6 324 uint8_t all_stopSistem = 0; // sistema in stop (a tempo con ruota ferma)
nerit 3:36da019e6bb6 325 uint8_t all_upElements = 0; // elementi sollevati (generato dal relativo sensore)
nerit 3:36da019e6bb6 326 uint8_t all_noSeedOnCe = 0; // fallanza di semina (manca il seme nella cella)
nerit 3:36da019e6bb6 327 uint8_t all_cfgnErrors = 0; // errore di configurazione (incongruenza dei parametri impostati)
nerit 3:36da019e6bb6 328 uint8_t all_noDcRotati = 0; // ruota di semina bloccata (arriva la velocità ma non i becchi)
nerit 3:36da019e6bb6 329 uint8_t all_noStepRota = 0; // tamburo di semina bloccato (comando il tamburo ma non ricevo il sensore)
nerit 3:36da019e6bb6 330 uint8_t all_speedError = 0; // errore sensore velocità (tempo impulsi non costante)
nerit 3:36da019e6bb6 331 uint8_t all_noSpeedSen = 0; // mancanza segnale di velocità (sento i becchi ma non la velocita)
nerit 3:36da019e6bb6 332 uint8_t all_no_Zeroing = 0; // mancato azzeramento sistema (generato dal timeout del comando motore DC)
nerit 3:36da019e6bb6 333 uint8_t all_genericals = 0; // allarme generico
nerit 3:36da019e6bb6 334 val1[2] contiene il byte alto della tabella di allarmi
nerit 3:36da019e6bb6 335 val1[3] contiene i segnali per la diagnostica
nerit 3:36da019e6bb6 336 bit 0= sensore ruota fonica
nerit 3:36da019e6bb6 337 bit 1= sensore celle
nerit 3:36da019e6bb6 338 bit 2= sensore posizione
nerit 3:36da019e6bb6 339 bit 3= sensore becchi
nerit 3:36da019e6bb6 340 bit 4= motore DC
nerit 3:36da019e6bb6 341 bit 5= controller
nerit 3:36da019e6bb6 342 bit 6= motore stepper
nerit 3:36da019e6bb6 343 */
nerit 3:36da019e6bb6 344 char val1[8]={0,0,0,0,0,0,0,0};
hudakz 0:1b9561cd1c36 345
nerit 3:36da019e6bb6 346 val1[3]=0;
nerit 3:36da019e6bb6 347 val1[3]=val1[3]+(tractorSpeedRead*0x01);
nerit 3:36da019e6bb6 348 val1[3]=val1[3]+(TBzeroPinInputRev*0x02);
nerit 3:36da019e6bb6 349 val1[3]=val1[3]+(ElementPosition*0x04);
nerit 3:36da019e6bb6 350 val1[3]=val1[3]+(seedWheelZeroPinInputRev*0x08);
nerit 3:36da019e6bb6 351 #if defined(simulaPerCan)
nerit 3:36da019e6bb6 352 if (buttonUser==0){
nerit 3:36da019e6bb6 353 val1[1]=0x02;
nerit 3:36da019e6bb6 354 }else{
nerit 3:36da019e6bb6 355 val1[1]=0x00;
nerit 3:36da019e6bb6 356 }
nerit 3:36da019e6bb6 357 val1[3]=valore;
nerit 3:36da019e6bb6 358 valore+=1;
nerit 3:36da019e6bb6 359 if(valore>50){
nerit 3:36da019e6bb6 360 valore=0;
nerit 3:36da019e6bb6 361 }
nerit 3:36da019e6bb6 362 tractorSpeed_MtS_timed=valore;
nerit 3:36da019e6bb6 363 #endif
nerit 3:36da019e6bb6 364 allarmi();
nerit 3:36da019e6bb6 365 valore = totalSpeed*36.0f; // tractorSpeed_MtS_timed*36.0f;
nerit 3:36da019e6bb6 366 val1[0]=valore;
nerit 3:36da019e6bb6 367 val1[1]=alarmHighRegister; // registro alto allarmi. Parte sempre da 0x80
nerit 3:36da019e6bb6 368 val1[2]=alarmLowRegister; // registro basso allarmi
nerit 3:36da019e6bb6 369 //val1[3]=val1[3];// registro di stato degli ingressi
nerit 3:36da019e6bb6 370 val1[4]=resetComandi;
nerit 3:36da019e6bb6 371 val1[5]=cellsCounterLow;
nerit 3:36da019e6bb6 372 val1[6]=cellsCounterHig;
nerit 3:36da019e6bb6 373 #if defined(canbusActive)
nerit 3:36da019e6bb6 374 if(can1.write(CANMessage(TX_ID, *&val1,8))){
nerit 3:36da019e6bb6 375 checkState=0;
nerit 3:36da019e6bb6 376 }
nerit 3:36da019e6bb6 377 #endif
nerit 3:36da019e6bb6 378 }
nerit 3:36da019e6bb6 379 //*******************************************************
nerit 3:36da019e6bb6 380 void leggiCAN(){
nerit 3:36da019e6bb6 381 #if defined(canbusActive)
nerit 3:36da019e6bb6 382 if(can1.read(rxMsg)) {
nerit 3:36da019e6bb6 383 if (firstStart==1){
nerit 3:36da019e6bb6 384 #if defined(speedMaster)
nerit 3:36da019e6bb6 385 sincUpdate.attach(&upDateSincro,0.01f);
nerit 3:36da019e6bb6 386 #endif
nerit 3:36da019e6bb6 387 canUpdate.attach(&upDateSpeed,0.11f);
nerit 3:36da019e6bb6 388 firstStart=0;
nerit 3:36da019e6bb6 389 }
nerit 3:36da019e6bb6 390
nerit 3:36da019e6bb6 391 if (rxMsg.id==RX_ID){
nerit 3:36da019e6bb6 392 #if defined(pcSerial)
nerit 3:36da019e6bb6 393 #if defined(canDataReceived)
nerit 3:36da019e6bb6 394 pc.printf("Messaggio ricevuto\n");
nerit 3:36da019e6bb6 395 #endif
nerit 3:36da019e6bb6 396 #endif
nerit 3:36da019e6bb6 397 }
nerit 3:36da019e6bb6 398 if (rxMsg.id==RX_Broadcast){
nerit 3:36da019e6bb6 399 #if defined(pcSerial)
nerit 3:36da019e6bb6 400 #if defined(canDataReceived)
nerit 3:36da019e6bb6 401 pc.printf("BroadCast ricevuto\n");
nerit 3:36da019e6bb6 402 #endif
nerit 3:36da019e6bb6 403 #endif
nerit 3:36da019e6bb6 404 enableSimula= rxMsg.data[0];
nerit 3:36da019e6bb6 405 speedSimula = rxMsg.data[1];
nerit 3:36da019e6bb6 406 avviaSimula = rxMsg.data[2];
nerit 3:36da019e6bb6 407 selezionato = rxMsg.data[3];
nerit 3:36da019e6bb6 408 comandiDaCan = rxMsg.data[4];
nerit 3:36da019e6bb6 409 #if defined(pcSerial)
nerit 3:36da019e6bb6 410 #if defined(canDataReceived)
nerit 3:36da019e6bb6 411 pc.printf("Comandi: %x\n",comandiDaCan);
nerit 3:36da019e6bb6 412 #endif
nerit 3:36da019e6bb6 413 #endif
nerit 3:36da019e6bb6 414 switch (comandiDaCan){
nerit 3:36da019e6bb6 415 case 1:
nerit 3:36da019e6bb6 416 case 3:
nerit 3:36da019e6bb6 417 azzeraDaCan=1;
nerit 3:36da019e6bb6 418 resetComandi=0x01;
nerit 3:36da019e6bb6 419 comandiDaCan=0;
nerit 3:36da019e6bb6 420 break;
nerit 3:36da019e6bb6 421 case 2:
nerit 3:36da019e6bb6 422 loadDaCan=1;
nerit 3:36da019e6bb6 423 resetComandi=0x02;
nerit 3:36da019e6bb6 424 comandiDaCan=0;
nerit 3:36da019e6bb6 425 break;
nerit 3:36da019e6bb6 426 default:
nerit 3:36da019e6bb6 427 comandiDaCan=0;
nerit 3:36da019e6bb6 428 resetComandi=0xFF;
nerit 3:36da019e6bb6 429 break;
nerit 3:36da019e6bb6 430 }
nerit 3:36da019e6bb6 431 #if defined(pcSerial)
nerit 3:36da019e6bb6 432 #if defined(canDataReceived)
nerit 3:36da019e6bb6 433 pc.printf("Comandi: %x\n",resetComandi);
nerit 3:36da019e6bb6 434 #endif
nerit 3:36da019e6bb6 435 #endif
nerit 3:36da019e6bb6 436 if (speedSimula>45){
nerit 3:36da019e6bb6 437 speedSimula=45;
nerit 3:36da019e6bb6 438 }
nerit 3:36da019e6bb6 439 // modulo 1
nerit 3:36da019e6bb6 440 if (RX_ID==0x100){
nerit 3:36da019e6bb6 441 if ((selezionato&0x01)==0x01){
nerit 3:36da019e6bb6 442 simOk=1;
nerit 3:36da019e6bb6 443 }else{
nerit 3:36da019e6bb6 444 simOk=0;
nerit 3:36da019e6bb6 445 }
nerit 3:36da019e6bb6 446 }
nerit 3:36da019e6bb6 447 // modulo 2
nerit 3:36da019e6bb6 448 if (RX_ID==0x102){
nerit 3:36da019e6bb6 449 if ((selezionato&0x02)==0x02){
nerit 3:36da019e6bb6 450 simOk=1;
nerit 3:36da019e6bb6 451 }else{
nerit 3:36da019e6bb6 452 simOk=0;
nerit 3:36da019e6bb6 453 }
nerit 3:36da019e6bb6 454 }
nerit 3:36da019e6bb6 455 // modulo 3
nerit 3:36da019e6bb6 456 if (RX_ID==0x104){
nerit 3:36da019e6bb6 457 if ((selezionato&0x04)==0x04){
nerit 3:36da019e6bb6 458 simOk=1;
nerit 3:36da019e6bb6 459 }else{
nerit 3:36da019e6bb6 460 simOk=0;
nerit 3:36da019e6bb6 461 }
nerit 3:36da019e6bb6 462 }
nerit 3:36da019e6bb6 463 // modulo 4
nerit 3:36da019e6bb6 464 if (RX_ID==0x106){
nerit 3:36da019e6bb6 465 if ((selezionato&0x08)==0x08){
nerit 3:36da019e6bb6 466 simOk=1;
nerit 3:36da019e6bb6 467 }else{
nerit 3:36da019e6bb6 468 simOk=0;
nerit 3:36da019e6bb6 469 }
nerit 3:36da019e6bb6 470 }
nerit 3:36da019e6bb6 471 // modulo 5
nerit 3:36da019e6bb6 472 if (RX_ID==0x108){
nerit 3:36da019e6bb6 473 if ((selezionato&0x10)==0x10){
nerit 3:36da019e6bb6 474 simOk=1;
nerit 3:36da019e6bb6 475 }else{
nerit 3:36da019e6bb6 476 simOk=0;
nerit 3:36da019e6bb6 477 }
nerit 3:36da019e6bb6 478 }
nerit 3:36da019e6bb6 479
nerit 3:36da019e6bb6 480 }
nerit 3:36da019e6bb6 481 if (rxMsg.id==RX_Settings){
nerit 3:36da019e6bb6 482 if (tractorSpeed_MtS_timed==0.0f){
nerit 3:36da019e6bb6 483 pickNumber = rxMsg.data[0]; // numero di becchi installati sulla ruota di semina
nerit 3:36da019e6bb6 484 cellsNumber = rxMsg.data[1]; // numero di celle del tamburo
nerit 3:36da019e6bb6 485 deepOfSeed=(rxMsg.data[2]/10000.0f); // deep of seeding
nerit 3:36da019e6bb6 486 seedWheelDiameter = ((rxMsg.data[4]*0xFF)+rxMsg.data[3])/10000.0f; // seed wheel diameter setting
nerit 3:36da019e6bb6 487 speedWheelDiameter = ((rxMsg.data[6]*0xFF)+rxMsg.data[5])/10000.0f; // variable for tractor speed calculation (need to be set from UI) ( Unit= meters )
nerit 3:36da019e6bb6 488 speedWheelPulse = rxMsg.data[7]; // variable which define the number of pulse each turn of tractor speed wheel (need to be set from UI)
nerit 3:36da019e6bb6 489 aggiornaParametri();
nerit 3:36da019e6bb6 490 #if defined(pcSerial)
nerit 3:36da019e6bb6 491 #if defined(canDataReceived)
nerit 3:36da019e6bb6 492 pc.printf("Seed wheel diameter %f \n",seedWheelDiameter);
nerit 3:36da019e6bb6 493 pc.printf("Speed wheel diameter %f \n",speedWheelDiameter);
nerit 3:36da019e6bb6 494 #endif
nerit 3:36da019e6bb6 495 #endif
nerit 3:36da019e6bb6 496 }
nerit 3:36da019e6bb6 497 }
nerit 3:36da019e6bb6 498 if (rxMsg.id==RX_AngoloPh){
nerit 3:36da019e6bb6 499 if (tractorSpeed_MtS_timed==0.0f){
nerit 3:36da019e6bb6 500 #if defined(M1)
nerit 3:36da019e6bb6 501 angoloPh = (double) rxMsg.data[0] ;
nerit 3:36da019e6bb6 502 aggiornaParametri();
nerit 3:36da019e6bb6 503 #endif
nerit 3:36da019e6bb6 504 #if defined(M2)
nerit 3:36da019e6bb6 505 angoloPh = (double) rxMsg.data[1] ;
nerit 3:36da019e6bb6 506 aggiornaParametri();
nerit 3:36da019e6bb6 507 #endif
nerit 3:36da019e6bb6 508 #if defined(M3)
nerit 3:36da019e6bb6 509 angoloPh = (double) rxMsg.data[2] ;
nerit 3:36da019e6bb6 510 aggiornaParametri();
nerit 3:36da019e6bb6 511 #endif
nerit 3:36da019e6bb6 512 #if defined(M4)
nerit 3:36da019e6bb6 513 angoloPh = (double) rxMsg.data[3] ;
nerit 3:36da019e6bb6 514 aggiornaParametri();
nerit 3:36da019e6bb6 515 #endif
nerit 3:36da019e6bb6 516 #if defined(M5)
nerit 3:36da019e6bb6 517 angoloPh = (double) rxMsg.data[4] ;
nerit 3:36da019e6bb6 518 aggiornaParametri();
nerit 3:36da019e6bb6 519 #endif
nerit 3:36da019e6bb6 520 #if defined(M6)
nerit 3:36da019e6bb6 521 angoloPh = (double) rxMsg.data[5] ;
nerit 3:36da019e6bb6 522 aggiornaParametri();
nerit 3:36da019e6bb6 523 #endif
nerit 3:36da019e6bb6 524 }
nerit 3:36da019e6bb6 525 }
nerit 3:36da019e6bb6 526 if (rxMsg.id==RX_AngoloAv){
nerit 3:36da019e6bb6 527 if (tractorSpeed_MtS_timed==0.0f){
nerit 3:36da019e6bb6 528 #if defined(M1)
nerit 3:36da019e6bb6 529 angoloAv = (double) rxMsg.data[0] ;
nerit 3:36da019e6bb6 530 aggiornaParametri();
nerit 3:36da019e6bb6 531 #endif
nerit 3:36da019e6bb6 532 #if defined(M2)
nerit 3:36da019e6bb6 533 angoloAv = (double) rxMsg.data[1] ;
nerit 3:36da019e6bb6 534 aggiornaParametri();
nerit 3:36da019e6bb6 535 #endif
nerit 3:36da019e6bb6 536 #if defined(M3)
nerit 3:36da019e6bb6 537 angoloAv = (double) rxMsg.data[2] ;
nerit 3:36da019e6bb6 538 aggiornaParametri();
nerit 3:36da019e6bb6 539 #endif
nerit 3:36da019e6bb6 540 #if defined(M4)
nerit 3:36da019e6bb6 541 angoloAv = (double) rxMsg.data[3] ;
nerit 3:36da019e6bb6 542 aggiornaParametri();
nerit 3:36da019e6bb6 543 #endif
nerit 3:36da019e6bb6 544 #if defined(M5)
nerit 3:36da019e6bb6 545 angoloAv = (double) rxMsg.data[4] ;
nerit 3:36da019e6bb6 546 aggiornaParametri();
nerit 3:36da019e6bb6 547 #endif
nerit 3:36da019e6bb6 548 #if defined(M6)
nerit 3:36da019e6bb6 549 angoloAv = (double) rxMsg.data[5] ;
nerit 3:36da019e6bb6 550 aggiornaParametri();
nerit 3:36da019e6bb6 551 #endif
nerit 3:36da019e6bb6 552 }
nerit 3:36da019e6bb6 553 }
nerit 3:36da019e6bb6 554 if (rxMsg.id==RX_Quinconce){
nerit 3:36da019e6bb6 555 if (tractorSpeed_MtS_timed==0.0f){
nerit 3:36da019e6bb6 556 quinconceActive = (uint8_t) rxMsg.data[0];
nerit 3:36da019e6bb6 557 aggiornaParametri();
nerit 3:36da019e6bb6 558 }
nerit 3:36da019e6bb6 559 }
nerit 3:36da019e6bb6 560 if (rxMsg.id==RX_QuincSinc){
nerit 3:36da019e6bb6 561 masterSinc = (uint32_t) rxMsg.data[0] * 0x00FF0000;
nerit 3:36da019e6bb6 562 masterSinc = masterSinc + ((uint32_t) rxMsg.data[1] * 0x0000FF00);
nerit 3:36da019e6bb6 563 masterSinc = masterSinc + ((uint32_t) rxMsg.data[2] * 0x000000FF);
nerit 3:36da019e6bb6 564 masterSinc = masterSinc + ((uint32_t) rxMsg.data[3]);
nerit 3:36da019e6bb6 565 quincVerify=1;
nerit 3:36da019e6bb6 566 //pc.printf("pippo");
nerit 3:36da019e6bb6 567 }
nerit 3:36da019e6bb6 568 }
nerit 3:36da019e6bb6 569 #endif
nerit 3:36da019e6bb6 570 #if defined(overWriteCanSimulation)
nerit 3:36da019e6bb6 571 enableSimula=1;
nerit 3:36da019e6bb6 572 speedSimula=25;
nerit 3:36da019e6bb6 573 avviaSimula=1;
nerit 3:36da019e6bb6 574 simOk=1;
nerit 3:36da019e6bb6 575 #endif
nerit 3:36da019e6bb6 576
hudakz 0:1b9561cd1c36 577 }
hudakz 0:1b9561cd1c36 578
nerit 3:36da019e6bb6 579 //*******************************************************
nerit 3:36da019e6bb6 580 void DC_CheckCurrent(){
nerit 3:36da019e6bb6 581
nerit 3:36da019e6bb6 582 // TODO: tabella di riferimento assorbimenti alle varie velocità al fine di gestire
nerit 3:36da019e6bb6 583 // gli allarmi e le correzioni di velocità
nerit 3:36da019e6bb6 584
nerit 3:36da019e6bb6 585 //float SD_analogMatrix[10]={0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f};
nerit 3:36da019e6bb6 586 //int SD_analogIndex[10]={0,0,0,0,0,0,0,0,0,0};
nerit 3:36da019e6bb6 587 // Analog reading
nerit 3:36da019e6bb6 588 //number = floor(number * 100) / 100;
nerit 3:36da019e6bb6 589 //if (pwmCheck==1){
nerit 3:36da019e6bb6 590 timeout.detach();
nerit 3:36da019e6bb6 591 for (int ii=1;ii<20;ii++){
nerit 3:36da019e6bb6 592 SD_analogMatrix[ii]=SD_analogMatrix[ii+1];
nerit 3:36da019e6bb6 593 }
nerit 3:36da019e6bb6 594 SD_CurrentAnalog = floor(SDcurrent.read()*100)/100; // valore in ingresso compreso tra 0.0 e 1.0
nerit 3:36da019e6bb6 595 SD_analogMatrix[20]=SD_CurrentAnalog;
nerit 3:36da019e6bb6 596 if (SDmotorPWM==0.0f){
nerit 3:36da019e6bb6 597 SD_CurrentStart=SD_CurrentAnalog;
nerit 3:36da019e6bb6 598 }
nerit 3:36da019e6bb6 599 float sommaTutto=0.0f;
nerit 3:36da019e6bb6 600 for (int ii=1;ii<21;ii++){
nerit 3:36da019e6bb6 601 sommaTutto=sommaTutto+SD_analogMatrix[ii];
nerit 3:36da019e6bb6 602 }
nerit 3:36da019e6bb6 603 float SD_CurrentAnalogica=sommaTutto/20.0f;
nerit 3:36da019e6bb6 604 SD_CurrentScaled = floor((((SD_CurrentAnalogica) - (SD_CurrentStart))*3.3f)/(SD_CurrentFactor/1000.0f)*10)/10;
nerit 3:36da019e6bb6 605 #if defined(pcSerial)
nerit 3:36da019e6bb6 606 #if defined(correnteAssorbita)
nerit 3:36da019e6bb6 607 pc.printf("Corrente: %f \n",SD_CurrentAnalogica);
nerit 3:36da019e6bb6 608 #endif
nerit 3:36da019e6bb6 609 #endif
nerit 3:36da019e6bb6 610 if (SD_CurrentAnalogica < -10.0f){
nerit 3:36da019e6bb6 611 #if defined(canbusActive)
nerit 3:36da019e6bb6 612 int SD_AllarmeUndervoltage=1; // da implementare nel CAN
nerit 3:36da019e6bb6 613 #endif
nerit 3:36da019e6bb6 614 }
nerit 3:36da019e6bb6 615 if (SD_CurrentAnalogica > 10.0f){
nerit 3:36da019e6bb6 616 #if defined(canbusActive)
nerit 3:36da019e6bb6 617 all_overCurrDC=1;
nerit 3:36da019e6bb6 618 #endif
nerit 3:36da019e6bb6 619 }
nerit 3:36da019e6bb6 620 //}
nerit 3:36da019e6bb6 621 }
nerit 3:36da019e6bb6 622 //*******************************************************
nerit 3:36da019e6bb6 623 void DC_prepare(){
nerit 3:36da019e6bb6 624 // direction or brake preparation
nerit 3:36da019e6bb6 625 if (DC_brake==1){SDmotorInA=1;SDmotorInB=1;}else{if (DC_forward==1){SDmotorInA=1;SDmotorInB=0;}else{SDmotorInA=0;SDmotorInB=1;}}
nerit 3:36da019e6bb6 626 // fault reading
nerit 3:36da019e6bb6 627 if (SDmotorInA==1){SD_faultA=1;}else{SD_faultA=0;}
nerit 3:36da019e6bb6 628 if (SDmotorInB==1){SD_faultB=1;}else{SD_faultB=0;}
nerit 3:36da019e6bb6 629 }
nerit 3:36da019e6bb6 630 //*******************************************************
nerit 3:36da019e6bb6 631 void startDelay(){
nerit 3:36da019e6bb6 632 int ritardo =0;
nerit 3:36da019e6bb6 633 ritardo = (int)((float)(dcActualDuty*800.0f));
nerit 3:36da019e6bb6 634 timeout.attach_us(&DC_CheckCurrent,ritardo);
nerit 3:36da019e6bb6 635 }
nerit 3:36da019e6bb6 636 //*******************************************************
nerit 3:36da019e6bb6 637 void quinCalc(double parametro){
nerit 3:36da019e6bb6 638 double riferimento=tempoTraBecchi_mS/parametro;
nerit 3:36da019e6bb6 639 // riceve l'impulso di sincro dal master e fa partire il timer di verifica dell'errore
nerit 3:36da019e6bb6 640 if ((quinconceIn==1)&&(quincStart==0)&&(oldQuincIn==0)){
nerit 3:36da019e6bb6 641 #if defined(pcSerial)
nerit 3:36da019e6bb6 642 #if defined(Qnc)
nerit 3:36da019e6bb6 643 pc.printf(" quinconceIn: %d", masterSinc);
nerit 3:36da019e6bb6 644 #endif
nerit 3:36da019e6bb6 645 #endif
nerit 3:36da019e6bb6 646 oldQuincIn=1;
nerit 3:36da019e6bb6 647 quincTime.reset();
nerit 3:36da019e6bb6 648 quincStart=1;
nerit 3:36da019e6bb6 649 }
nerit 3:36da019e6bb6 650 if( quinconceIn==0){
nerit 3:36da019e6bb6 651 oldQuincIn=0;
nerit 3:36da019e6bb6 652 }
nerit 3:36da019e6bb6 653 // riceve l'impulso di passaggio del becco locale e determina il valore dell'errore
nerit 3:36da019e6bb6 654 if (quinconceActive==1){
nerit 3:36da019e6bb6 655 if ((quincStart==1)&&(SDzeroDebounced==1)){
nerit 3:36da019e6bb6 656 memoDuty=dcActualDuty;
nerit 3:36da019e6bb6 657 accelera=0;
nerit 3:36da019e6bb6 658 decelera=0;
nerit 3:36da019e6bb6 659 quincStop=0;
nerit 3:36da019e6bb6 660 quincStart=0;
nerit 3:36da019e6bb6 661 percento=0.0f;
nerit 3:36da019e6bb6 662 scostamento = (double)quincTime.read_ms();
nerit 3:36da019e6bb6 663 quincError = 0.0f;
nerit 3:36da019e6bb6 664 if ((scostamento > riferimento)||(scostamento==0.0f)){
nerit 3:36da019e6bb6 665 decelera=1;
nerit 3:36da019e6bb6 666 accelera=0;
nerit 3:36da019e6bb6 667 quincError = scostamento - riferimento;
nerit 3:36da019e6bb6 668 percento = 1.0f - (abs(quincError)/riferimento);
hudakz 0:1b9561cd1c36 669 }
nerit 3:36da019e6bb6 670 if ((scostamento < riferimento)&&(scostamento >0.0f)){
nerit 3:36da019e6bb6 671 decelera=0;
nerit 3:36da019e6bb6 672 accelera=1;
nerit 3:36da019e6bb6 673 quincError = riferimento - scostamento;
nerit 3:36da019e6bb6 674 percento = 1.0f + (abs(quincError)/riferimento);
nerit 3:36da019e6bb6 675 }
nerit 3:36da019e6bb6 676 if ((decelera==1)&&(quincStop==0)){
nerit 3:36da019e6bb6 677 quincStopTimer.reset();
nerit 3:36da019e6bb6 678 quincStopTimer.start();
nerit 3:36da019e6bb6 679 quincStop=1;
nerit 3:36da019e6bb6 680 propoQuinc = (speedOfSeedWheel/1.25f)*quincError;
nerit 3:36da019e6bb6 681 }
nerit 3:36da019e6bb6 682 if (abs(percento) > 1.05f){percento=1.05f;}
nerit 3:36da019e6bb6 683 if (abs(percento) < 0.75f){percento=0.75f;}
hudakz 0:1b9561cd1c36 684 }
nerit 3:36da019e6bb6 685 dcActualDuty = memoDuty * abs(percento);
nerit 3:36da019e6bb6 686 }
nerit 3:36da019e6bb6 687 if ((quinconceActive==0)&&(quincEnable==1)){
nerit 3:36da019e6bb6 688 // da encoder master
nerit 3:36da019e6bb6 689 if (quincVerify==1){
nerit 3:36da019e6bb6 690 memoDuty=dcActualDuty;
nerit 3:36da019e6bb6 691 quincVerify=0;
nerit 3:36da019e6bb6 692 accelera=0;
nerit 3:36da019e6bb6 693 decelera=0;
nerit 3:36da019e6bb6 694 percento=0.0f;
nerit 3:36da019e6bb6 695 quincError = 0.0f;
nerit 3:36da019e6bb6 696 if (masterSinc > prePosSD){
nerit 3:36da019e6bb6 697 decelera=0;
nerit 3:36da019e6bb6 698 accelera=1;
nerit 3:36da019e6bb6 699 quincError = masterSinc - prePosSD;
nerit 3:36da019e6bb6 700 percento = 1.0f + (abs(quincError)/SDsectorStep);
hudakz 0:1b9561cd1c36 701 }
nerit 3:36da019e6bb6 702 if (masterSinc < prePosSD){
nerit 3:36da019e6bb6 703 decelera=1;
nerit 3:36da019e6bb6 704 accelera=0;
nerit 3:36da019e6bb6 705 quincError = prePosSD -masterSinc;
nerit 3:36da019e6bb6 706 percento = 1.0f - (abs(quincError)/riferimento);
nerit 3:36da019e6bb6 707 }
nerit 3:36da019e6bb6 708 // se quinconce richiesto deve andare al valore di riferimento
nerit 3:36da019e6bb6 709 if (abs(percento) > 1.55f){percento=1.55f;}
nerit 3:36da019e6bb6 710 if (abs(percento) < 0.55f){percento=0.55f;}
nerit 3:36da019e6bb6 711 #if defined(pcSerial)
nerit 3:36da019e6bb6 712 #if defined(Qnc)
nerit 3:36da019e6bb6 713 pc.printf(" masterSinc: %d", masterSinc);
nerit 3:36da019e6bb6 714 pc.printf(" prePosSD: %d",prePosSD);
nerit 3:36da019e6bb6 715 pc.printf(" percento: %f\n",percento);
nerit 3:36da019e6bb6 716 #endif
nerit 3:36da019e6bb6 717 #endif
nerit 3:36da019e6bb6 718 dcActualDuty = memoDuty * abs(percento);
hudakz 0:1b9561cd1c36 719 }
hudakz 0:1b9561cd1c36 720 }
hudakz 0:1b9561cd1c36 721 }
nerit 3:36da019e6bb6 722 // ------------------------------------------------------------------------------------------------------------------------------------------------------------------
nerit 3:36da019e6bb6 723 // MAIN SECTION
nerit 3:36da019e6bb6 724 // ------------------------------------------------------------------------------------------------------------------------------------------------------------------
nerit 3:36da019e6bb6 725
nerit 3:36da019e6bb6 726 int main()
nerit 3:36da019e6bb6 727 {
nerit 3:36da019e6bb6 728 for (int a=0; a<5;a++){
nerit 3:36da019e6bb6 729 mediaSpeed[a]=0;
nerit 3:36da019e6bb6 730 }
nerit 3:36da019e6bb6 731
nerit 3:36da019e6bb6 732 // Stepper driver init and set
nerit 3:36da019e6bb6 733 TBmotorRst=0; // reset stepper driver
nerit 3:36da019e6bb6 734 TBmotorDirecti=0; // reset stepper direction
nerit 3:36da019e6bb6 735 // M1 M2 M3 RESOLUTION
nerit 3:36da019e6bb6 736 // 0 0 0 1/2
nerit 3:36da019e6bb6 737 // 1 0 0 1/8
nerit 3:36da019e6bb6 738 // 0 1 0 1/16
nerit 3:36da019e6bb6 739 // 1 1 0 1/32
nerit 3:36da019e6bb6 740 // 0 0 1 1/64
nerit 3:36da019e6bb6 741 // 1 0 1 1/128
nerit 3:36da019e6bb6 742 // 0 1 1 1/10
nerit 3:36da019e6bb6 743 // 1 1 1 1/20
nerit 3:36da019e6bb6 744 if (TBmotorSteps==400){
nerit 3:36da019e6bb6 745 TBmotor_M1=0;
nerit 3:36da019e6bb6 746 TBmotor_M2=0;
nerit 3:36da019e6bb6 747 TBmotor_M3=0;
nerit 3:36da019e6bb6 748 }else if (TBmotorSteps==1600){
nerit 3:36da019e6bb6 749 TBmotor_M1=1;
nerit 3:36da019e6bb6 750 TBmotor_M2=0;
nerit 3:36da019e6bb6 751 TBmotor_M3=0;
nerit 3:36da019e6bb6 752 }else if (TBmotorSteps==3200){
nerit 3:36da019e6bb6 753 TBmotor_M1=0;
nerit 3:36da019e6bb6 754 TBmotor_M2=1;
nerit 3:36da019e6bb6 755 TBmotor_M3=0;
nerit 3:36da019e6bb6 756 }else if (TBmotorSteps==6400){
nerit 3:36da019e6bb6 757 TBmotor_M1=1;
nerit 3:36da019e6bb6 758 TBmotor_M2=1;
nerit 3:36da019e6bb6 759 TBmotor_M3=0;
nerit 3:36da019e6bb6 760 }else if (TBmotorSteps==12800){
nerit 3:36da019e6bb6 761 TBmotor_M1=0;
nerit 3:36da019e6bb6 762 TBmotor_M2=0;
nerit 3:36da019e6bb6 763 TBmotor_M3=1;
nerit 3:36da019e6bb6 764 }else if (TBmotorSteps==25600){
nerit 3:36da019e6bb6 765 TBmotor_M1=1;
nerit 3:36da019e6bb6 766 TBmotor_M2=0;
nerit 3:36da019e6bb6 767 TBmotor_M3=1;
nerit 3:36da019e6bb6 768 }else if (TBmotorSteps==2000){
nerit 3:36da019e6bb6 769 TBmotor_M1=0;
nerit 3:36da019e6bb6 770 TBmotor_M2=1;
nerit 3:36da019e6bb6 771 TBmotor_M3=1;
nerit 3:36da019e6bb6 772 }else if (TBmotorSteps==4000){
nerit 3:36da019e6bb6 773 TBmotor_M1=1;
nerit 3:36da019e6bb6 774 TBmotor_M2=1;
nerit 3:36da019e6bb6 775 TBmotor_M3=1;
nerit 3:36da019e6bb6 776 }
nerit 3:36da019e6bb6 777 TBmotorRst=1;
nerit 3:36da019e6bb6 778
nerit 3:36da019e6bb6 779 // DC reset ad set
nerit 3:36da019e6bb6 780 int decima = 100;
nerit 3:36da019e6bb6 781 wait_ms(200);
nerit 3:36da019e6bb6 782 SD_CurrentStart=floor(SDcurrent.read()*decima)/decima;
nerit 3:36da019e6bb6 783 wait_ms(2);
nerit 3:36da019e6bb6 784 SD_CurrentStart+=floor(SDcurrent.read()*decima)/decima;
nerit 3:36da019e6bb6 785 wait_ms(1);
nerit 3:36da019e6bb6 786 SD_CurrentStart+=floor(SDcurrent.read()*decima)/decima;
nerit 3:36da019e6bb6 787 wait_ms(3);
nerit 3:36da019e6bb6 788 SD_CurrentStart+=floor(SDcurrent.read()*decima)/decima;
nerit 3:36da019e6bb6 789 SD_CurrentStart=(floor((SD_CurrentStart/4.0f)*decima)/decima)-0.01f;
nerit 3:36da019e6bb6 790 wait_ms(100);
nerit 3:36da019e6bb6 791 DC_prepare();
nerit 3:36da019e6bb6 792
nerit 3:36da019e6bb6 793 speedTimer.start(); // speed pulse timer
nerit 3:36da019e6bb6 794 intraPickTimer.start();
nerit 3:36da019e6bb6 795 speedTimeOut.start();
nerit 3:36da019e6bb6 796 speedFilter.start();
nerit 3:36da019e6bb6 797 seedFilter.start();
nerit 3:36da019e6bb6 798 TBfilter.start();
nerit 3:36da019e6bb6 799 sincroTimer.start();
nerit 3:36da019e6bb6 800 rotationTimeOut.start();
nerit 3:36da019e6bb6 801 metalTimer.start();
nerit 3:36da019e6bb6 802 quincTime.start();
nerit 3:36da019e6bb6 803
nerit 3:36da019e6bb6 804 //*******************************************************
nerit 3:36da019e6bb6 805 // controls for check DC motor current
nerit 3:36da019e6bb6 806 //currentCheck.attach(&DC_CheckCurrent, 0.10f);
nerit 3:36da019e6bb6 807 pwmCheck.rise(&startDelay);
nerit 3:36da019e6bb6 808 //tbCorrection.attach(&correction,0.1f;
nerit 3:36da019e6bb6 809
nerit 3:36da019e6bb6 810 if (inProva==0){
nerit 3:36da019e6bb6 811 tractorSpeedRead.rise(&tractorReadSpeed);
nerit 3:36da019e6bb6 812 tftUpdate.attach(&videoUpdate,0.50f);
nerit 3:36da019e6bb6 813 seedCorrection.attach(&seedCorrect,0.005f); // con 16 becchi a 4,5Kmh ci sono 37mS tra un becco e l'altro, quindi 8 correzioni di tb
nerit 3:36da019e6bb6 814 if (speedFromPick==0){
nerit 3:36da019e6bb6 815 ElementPosition.rise(&sd25Fall);
nerit 3:36da019e6bb6 816 }
nerit 3:36da019e6bb6 817 }else{
nerit 3:36da019e6bb6 818 tftUpdate.attach(&videoUpdate,0.125f);
nerit 3:36da019e6bb6 819 }
nerit 3:36da019e6bb6 820
nerit 3:36da019e6bb6 821 aggiornaParametri();
nerit 3:36da019e6bb6 822
nerit 3:36da019e6bb6 823 SDmotorPWM.period_us(periodoSD); // frequency 1KHz pilotaggio motore DC
nerit 3:36da019e6bb6 824 SDmotorPWM.write(0.0f); // duty cycle = stop
nerit 3:36da019e6bb6 825 TBmotorDirecti=0; // tb motor direction set
nerit 3:36da019e6bb6 826
nerit 3:36da019e6bb6 827 #if defined(provaStepper)
nerit 3:36da019e6bb6 828 TBmotorRst=1;
nerit 3:36da019e6bb6 829 TBticker.attach_us(&step_TBPulseOut,3000.0f); // clock time are seconds and attach seed motor stepper controls
nerit 3:36da019e6bb6 830 #endif // end prova stepper
nerit 3:36da019e6bb6 831
nerit 3:36da019e6bb6 832 #if defined(canbusActive)
nerit 3:36da019e6bb6 833 can1.attach(&leggiCAN, CAN::RxIrq);
nerit 3:36da019e6bb6 834 #endif
nerit 3:36da019e6bb6 835
nerit 3:36da019e6bb6 836 //**************************************************************************************************************
nerit 3:36da019e6bb6 837 // MAIN LOOP
nerit 3:36da019e6bb6 838 //**************************************************************************************************************
nerit 3:36da019e6bb6 839 while (true){
nerit 3:36da019e6bb6 840
nerit 3:36da019e6bb6 841 // ripetitore segnale di velocità
nerit 3:36da019e6bb6 842 if (tractorSpeedRead==0){speedClock=0;}
nerit 3:36da019e6bb6 843
nerit 3:36da019e6bb6 844 // inversione segnali ingressi
nerit 3:36da019e6bb6 845 TBzeroPinInput = !TBzeroPinInputRev;
nerit 3:36da019e6bb6 846 seedWheelZeroPinInput = !seedWheelZeroPinInputRev;
nerit 3:36da019e6bb6 847
nerit 3:36da019e6bb6 848 // se quinconce attivo ed unita' master invia segnale di sincro
nerit 3:36da019e6bb6 849 #if defined(speedMaster)
nerit 3:36da019e6bb6 850 if (seedWheelZeroPinInput==1){
nerit 3:36da019e6bb6 851 quinconceOut=1;
nerit 3:36da019e6bb6 852 }else{
nerit 3:36da019e6bb6 853 quinconceOut=0;
nerit 3:36da019e6bb6 854 }
nerit 3:36da019e6bb6 855 #endif
nerit 3:36da019e6bb6 856
nerit 3:36da019e6bb6 857 #if defined(overWriteCanSimulation)
nerit 3:36da019e6bb6 858 leggiCAN();
nerit 3:36da019e6bb6 859 #endif
nerit 3:36da019e6bb6 860
nerit 3:36da019e6bb6 861 // simulazione velocita
nerit 3:36da019e6bb6 862 if (enableSimula==1){
nerit 3:36da019e6bb6 863 double TMT = 0.0f;
nerit 3:36da019e6bb6 864 if (speedSimula > 0){
nerit 3:36da019e6bb6 865 TMT = (double)(speedSimula) * 100.0f /3600.0f;
nerit 3:36da019e6bb6 866 pulseSpeedInterval = pulseDistance / TMT;
nerit 3:36da019e6bb6 867 }else{
nerit 3:36da019e6bb6 868 pulseSpeedInterval = 10000.0f;
nerit 3:36da019e6bb6 869 }
nerit 3:36da019e6bb6 870 if (avviaSimula==1){
nerit 3:36da019e6bb6 871 if(oldSimulaSpeed!=pulseSpeedInterval){
nerit 3:36da019e6bb6 872 spedSimclock.attach_us(&speedSimulationClock,pulseSpeedInterval);
nerit 3:36da019e6bb6 873 oldSimulaSpeed=pulseSpeedInterval;
nerit 3:36da019e6bb6 874 }
nerit 3:36da019e6bb6 875 }else{
nerit 3:36da019e6bb6 876 oldSimulaSpeed=10000.0f;
nerit 3:36da019e6bb6 877 spedSimclock.detach();
nerit 3:36da019e6bb6 878 }
nerit 3:36da019e6bb6 879 }else{
nerit 3:36da019e6bb6 880 spedSimclock.detach();
nerit 3:36da019e6bb6 881 }
nerit 3:36da019e6bb6 882
nerit 3:36da019e6bb6 883 //*******************************************************
nerit 3:36da019e6bb6 884 // determina se sono in bassa velocità per il controllo di TB
nerit 3:36da019e6bb6 885 if (speedOfSeedWheel<=minSeedSpeed){
nerit 3:36da019e6bb6 886 if (lowSpeedRequired==0){
nerit 3:36da019e6bb6 887 ritardaLowSpeed.reset();
nerit 3:36da019e6bb6 888 ritardaLowSpeed.start();
nerit 3:36da019e6bb6 889 }
nerit 3:36da019e6bb6 890 lowSpeedRequired=1;
nerit 3:36da019e6bb6 891 }else{
nerit 3:36da019e6bb6 892 if (lowSpeedRequired==1){
nerit 3:36da019e6bb6 893 lowSpeedRequired=0;
nerit 3:36da019e6bb6 894 ritardaLowSpeed.reset();
nerit 3:36da019e6bb6 895 ritardaLowSpeed.stop();
nerit 3:36da019e6bb6 896 }
nerit 3:36da019e6bb6 897 }
nerit 3:36da019e6bb6 898
nerit 3:36da019e6bb6 899 if (ritardaLowSpeed.read_ms()> 2000){
nerit 3:36da019e6bb6 900 lowSpeed=1;
nerit 3:36da019e6bb6 901 }else{
nerit 3:36da019e6bb6 902 lowSpeed=0;
nerit 3:36da019e6bb6 903 }
nerit 3:36da019e6bb6 904
nerit 3:36da019e6bb6 905
nerit 3:36da019e6bb6 906 //**************************************************************************************************************
nerit 3:36da019e6bb6 907 //**************************************************************************************************************
nerit 3:36da019e6bb6 908 // LOGICAL CONTROLS
nerit 3:36da019e6bb6 909 //**************************************************************************************************************
nerit 3:36da019e6bb6 910 //**************************************************************************************************************
nerit 3:36da019e6bb6 911
nerit 3:36da019e6bb6 912 if (inProva==0){
nerit 3:36da019e6bb6 913 if ((startCycleSimulation==0)&&(enableSimula==0)){zeroRequestBuf=1;runRequestBuf=0;enableCycle=1;
nerit 3:36da019e6bb6 914 }else{zeroRequestBuf=1;runRequestBuf=0;enableCycle=1;}
nerit 3:36da019e6bb6 915 if ((tractorSpeedRead==0)&&(speedFilter.read_ms()>3)){oldTractorSpeedRead=0;}
nerit 3:36da019e6bb6 916 // ----------------------------------------
nerit 3:36da019e6bb6 917 // filtra il segnale dei becchi per misurare il tempo di intervallo tra loro
nerit 3:36da019e6bb6 918 // ----------------------------------------
nerit 3:36da019e6bb6 919 if ((seedWheelZeroPinInput==0)&&(seedFilter.read_ms()>=4)){
nerit 3:36da019e6bb6 920 oldSeedWheelZeroPinInput=0;
nerit 3:36da019e6bb6 921 SDzeroDebounced=0;
nerit 3:36da019e6bb6 922 }
nerit 3:36da019e6bb6 923 if ((seedWheelZeroPinInput==1)&&(oldSeedWheelZeroPinInput==0)){
nerit 3:36da019e6bb6 924 timeIntraPick = (double)intraPickTimer.read_ms();
nerit 3:36da019e6bb6 925 prePosSD=500; // preposizionamento SD
nerit 3:36da019e6bb6 926 intraPickTimer.reset();
nerit 3:36da019e6bb6 927 rotationTimeOut.reset();
nerit 3:36da019e6bb6 928 seedFilter.reset();
nerit 3:36da019e6bb6 929 sincroTimer.reset();
nerit 3:36da019e6bb6 930 oldSeedWheelZeroPinInput=1;
nerit 3:36da019e6bb6 931 SDzeroDebounced=1;
nerit 3:36da019e6bb6 932 SDwheelTimer.reset();
nerit 3:36da019e6bb6 933 if (quincCnt<10){
nerit 3:36da019e6bb6 934 quincCnt++;
nerit 3:36da019e6bb6 935 }
nerit 3:36da019e6bb6 936 if (quincCnt >3){
nerit 3:36da019e6bb6 937 quincEnable=1;
nerit 3:36da019e6bb6 938 }else{
nerit 3:36da019e6bb6 939 quincEnable=0;
nerit 3:36da019e6bb6 940 }
nerit 3:36da019e6bb6 941 if ((aspettaStart==0)&&(lowSpeed==1)){
nerit 3:36da019e6bb6 942 beccoPronto=1;
nerit 3:36da019e6bb6 943 }
nerit 3:36da019e6bb6 944 SDzeroCyclePulse=1;
nerit 3:36da019e6bb6 945 lockStart=0;
nerit 3:36da019e6bb6 946 double fase1=0.0f;
nerit 3:36da019e6bb6 947 forzaFase=0;
nerit 3:36da019e6bb6 948 double limite=fixedStepGiroSD/pickNumber;
nerit 3:36da019e6bb6 949 if (pickCounterLow< 0xFF){
nerit 3:36da019e6bb6 950 pickCounterLow++;
nerit 3:36da019e6bb6 951 }else{
nerit 3:36da019e6bb6 952 pickCounterHig++;
nerit 3:36da019e6bb6 953 pickCounterLow=0;
nerit 3:36da019e6bb6 954 }
nerit 3:36da019e6bb6 955 if (tamburoStandard==0){
nerit 3:36da019e6bb6 956 fase1=TBdeltaStep;
nerit 3:36da019e6bb6 957 }else{
nerit 3:36da019e6bb6 958 if(speedForCorrection >= speedOfSeedWheel){
nerit 3:36da019e6bb6 959 fase1=TBdeltaStep;
nerit 3:36da019e6bb6 960 }else{
nerit 3:36da019e6bb6 961 //fase1=(TBdeltaStep)-(((speedOfSeedWheel-speedForCorrection)/1.25f)*(TBfaseStep));
nerit 3:36da019e6bb6 962 fase1=(TBdeltaStep)-(((speedOfSeedWheel)/1.25f)*(TBfaseStep));
nerit 3:36da019e6bb6 963 }
nerit 3:36da019e6bb6 964 if (fase1 > limite){
nerit 3:36da019e6bb6 965 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)
nerit 3:36da019e6bb6 966 //forzaFase=1;
nerit 3:36da019e6bb6 967 }
nerit 3:36da019e6bb6 968 if ((fase1 > (limite -20.0f))&&(fase1<(limite +5.0f))){
nerit 3:36da019e6bb6 969 fase1 = limite -20.0f; // se la fase è molto vicina al limite interpone uno spazio minimo di lavoro del sincronismo
nerit 3:36da019e6bb6 970 forzaFase=1;
nerit 3:36da019e6bb6 971 }
nerit 3:36da019e6bb6 972 trigRepos=1;
nerit 3:36da019e6bb6 973 }
nerit 3:36da019e6bb6 974 fase = (uint32_t)fase1+500;
nerit 3:36da019e6bb6 975 #if defined(pcSerial)
nerit 3:36da019e6bb6 976 #if defined(inCorre)
nerit 3:36da019e6bb6 977 pc.printf(" limite %f", limite);
nerit 3:36da019e6bb6 978 pc.printf(" delta %f", TBdeltaStep);
nerit 3:36da019e6bb6 979 pc.printf(" faseStep %f", TBfaseStep);
nerit 3:36da019e6bb6 980 pc.printf(" fase %d",fase);
nerit 3:36da019e6bb6 981 pc.printf(" forzaFase %d",forzaFase);
nerit 3:36da019e6bb6 982 pc.printf(" trigRepos %d", trigRepos);
nerit 3:36da019e6bb6 983 pc.printf(" ActualSD: %d",SDactualPosition);
nerit 3:36da019e6bb6 984 pc.printf(" SpeedWheel: %f",speedOfSeedWheel);
nerit 3:36da019e6bb6 985 pc.printf(" SPEED: %f \n",tractorSpeed_MtS_timed);
nerit 3:36da019e6bb6 986 #endif
nerit 3:36da019e6bb6 987 #endif
nerit 3:36da019e6bb6 988 if (timeIntraPick >= (memoIntraPick*2)){
nerit 3:36da019e6bb6 989 if ((aspettaStart==0)&&(zeroRequestBuf==0)){
nerit 3:36da019e6bb6 990 if (firstStart==0){
nerit 3:36da019e6bb6 991 all_pickSignal=1;
nerit 3:36da019e6bb6 992 }
nerit 3:36da019e6bb6 993 }
nerit 3:36da019e6bb6 994 }
nerit 3:36da019e6bb6 995 memoIntraPick = timeIntraPick;
nerit 3:36da019e6bb6 996 if (speedFromPick==1){
nerit 3:36da019e6bb6 997 speedOfSeedWheel=((seedPerimeter / pickNumber)/timeIntraPick)*1000.0f;
nerit 3:36da019e6bb6 998 }
nerit 3:36da019e6bb6 999 if ((tractorSpeed_MtS_timed==0.0f)&&(zeroCycle==0)&&(zeroCycleEnd==1)){
nerit 3:36da019e6bb6 1000 if ((firstStart==0)&&(simOk==0)&&(enableSimula==0)&&(zeroCycleEnd==1)){
nerit 3:36da019e6bb6 1001 all_noSpeedSen=1;
nerit 3:36da019e6bb6 1002 }
nerit 3:36da019e6bb6 1003 #if defined(pcSerial)
nerit 3:36da019e6bb6 1004 #if defined(canDataReceived)
nerit 3:36da019e6bb6 1005 pc.printf("allarme no speed sensor");
nerit 3:36da019e6bb6 1006 #endif
nerit 3:36da019e6bb6 1007 #endif
nerit 3:36da019e6bb6 1008 }
nerit 3:36da019e6bb6 1009 pulseRised2=1;
nerit 3:36da019e6bb6 1010 #if defined(speedMaster)
nerit 3:36da019e6bb6 1011 double oldLastPr = (double)oldLastPulseRead*1.2f;
nerit 3:36da019e6bb6 1012 if((double)speedTimeOut.read_us()> (oldLastPr)){
nerit 3:36da019e6bb6 1013 if ((firstStart==0)&&(simOk==0)&&(enableSimula==0)&&(zeroCycleEnd==1)){
nerit 3:36da019e6bb6 1014 all_speedError =1;
nerit 3:36da019e6bb6 1015 }
nerit 3:36da019e6bb6 1016 #if defined(pcSerial)
nerit 3:36da019e6bb6 1017 #if defined(canDataReceived)
nerit 3:36da019e6bb6 1018 pc.printf("allarme errore speed sensor");
nerit 3:36da019e6bb6 1019 #endif
nerit 3:36da019e6bb6 1020 #endif
nerit 3:36da019e6bb6 1021
nerit 3:36da019e6bb6 1022 }
nerit 3:36da019e6bb6 1023 #endif
nerit 3:36da019e6bb6 1024 //*******************************************
nerit 3:36da019e6bb6 1025 // segue calcolo clock per la generazione della posizione teorica
nerit 3:36da019e6bb6 1026 // la realtà in base al segnale di presenza del becco
nerit 3:36da019e6bb6 1027 realSpeed = speedOfSeedWheel;
nerit 3:36da019e6bb6 1028 realGiroSD = seedPerimeter / realSpeed;
nerit 3:36da019e6bb6 1029 tempoBecco = (realGiroSD/360.0f)*16000.0f;
nerit 3:36da019e6bb6 1030 frequenzaReale = fixedStepGiroSD/realGiroSD;
nerit 3:36da019e6bb6 1031 semiPeriodoReale = (1000000.0f/frequenzaReale);
nerit 3:36da019e6bb6 1032 tempoTraBecchi_mS = 0.0f;
nerit 3:36da019e6bb6 1033 seedWheelRPM = (speedOfSeedWheel)*K_WheelRPM ; // calcola i giri al minuto della ruota di semina 7.37 31,75
nerit 3:36da019e6bb6 1034 rapportoRuote = pickNumber/cellsNumber; // 0.67 calcola il rapporto tra il numero di becchi ed il numero di celle 0.8 1,00
nerit 3:36da019e6bb6 1035 TBrpm = seedWheelRPM*rapportoRuote; // 5.896 31,75
nerit 3:36da019e6bb6 1036 TBgiroStep = TBmotorSteps*TBreductionRatio;
nerit 3:36da019e6bb6 1037 K_TBfrequency = TBgiroStep/60.0f; // 1600 * 1.65625f /60 = 44 44,00
nerit 3:36da019e6bb6 1038 TBfrequency = (TBrpm*K_TBfrequency); // 130Hz a 0,29Mts 1397,00 a 1,25mt/s con 15 becchi e 15 celle
nerit 3:36da019e6bb6 1039 TBperiod=1000000.0f/TBfrequency; // 715uS
nerit 3:36da019e6bb6 1040 }
nerit 3:36da019e6bb6 1041 // ----------------------------------------
nerit 3:36da019e6bb6 1042 // check SD fase
nerit 3:36da019e6bb6 1043 if ((prePosSD >= fase)||(forzaFase==1)){//&&(prePosSD < (fase +30))){
nerit 3:36da019e6bb6 1044 forzaFase=0;
nerit 3:36da019e6bb6 1045 if (trigRepos==1){
nerit 3:36da019e6bb6 1046 SDactualPosition=0;
nerit 3:36da019e6bb6 1047 if ((countCicli<30)&&(trigCicli==0)){countCicli++;trigCicli=1;}
nerit 3:36da019e6bb6 1048 if(countCicli>=cicliAspettaStart){aspettaStart=0;}
nerit 3:36da019e6bb6 1049 if ((lowSpeed==0)&&(aspettaStart==0)&&(lockStart==0)){syncroCheck=1;beccoPronto=0;}
nerit 3:36da019e6bb6 1050 if (trigTB==0){inhibit=1;trigSD=1;}else{inhibit=0;trigTB=0;trigSD=0;}
nerit 3:36da019e6bb6 1051 trigRepos=0;
nerit 3:36da019e6bb6 1052 }
nerit 3:36da019e6bb6 1053 }else{
nerit 3:36da019e6bb6 1054 trigCicli=0;
nerit 3:36da019e6bb6 1055 }
nerit 3:36da019e6bb6 1056 /*
nerit 3:36da019e6bb6 1057 if ((SDactualPosition >= fase) && (SDactualPosition < (fase +(uint32_t)30))){
nerit 3:36da019e6bb6 1058 if ((countCicli<30)&&(trigCicli==0)){countCicli++;trigCicli=1;}
nerit 3:36da019e6bb6 1059 if(countCicli>=cicliAspettaStart){aspettaStart=0;}
nerit 3:36da019e6bb6 1060 if ((lowSpeed==0)&&(aspettaStart==0)&&(lockStart==0)){syncroCheck=1;beccoPronto=0;}
nerit 3:36da019e6bb6 1061 }else{
nerit 3:36da019e6bb6 1062 trigCicli=0;
nerit 3:36da019e6bb6 1063 }
nerit 3:36da019e6bb6 1064 */
nerit 3:36da019e6bb6 1065 // ----------------------------------------
nerit 3:36da019e6bb6 1066 // filtra il segnale del tamburo per lo stop in fase del tamburo stesso
nerit 3:36da019e6bb6 1067 if (TBzeroPinInput==0){if (TBfilter.read_ms()>=5){oldTBzeroPinInput=0;}}
nerit 3:36da019e6bb6 1068 if ((TBzeroPinInput==1)&&(oldTBzeroPinInput==0)){
nerit 3:36da019e6bb6 1069 oldTBzeroPinInput=1;
nerit 3:36da019e6bb6 1070 if (loadDaCanInCorso==0){
nerit 3:36da019e6bb6 1071 stopCicloTB=1;
nerit 3:36da019e6bb6 1072 startCicloTB=0;
nerit 3:36da019e6bb6 1073 }
nerit 3:36da019e6bb6 1074 TBfilter.reset();
nerit 3:36da019e6bb6 1075 TBzeroCyclePulse=1;
nerit 3:36da019e6bb6 1076 TBactualPosition=0;
nerit 3:36da019e6bb6 1077 if (cellsCounterLow < 0xFF){
nerit 3:36da019e6bb6 1078 cellsCounterLow++;
nerit 3:36da019e6bb6 1079 }else{
nerit 3:36da019e6bb6 1080 cellsCounterHig++;
nerit 3:36da019e6bb6 1081 cellsCounterLow=0;
nerit 3:36da019e6bb6 1082 }
nerit 3:36da019e6bb6 1083 if (loadDaCanInCorso==1){
nerit 3:36da019e6bb6 1084 cntCellsForLoad++;
nerit 3:36da019e6bb6 1085 if (cntCellsForLoad >= 5){
nerit 3:36da019e6bb6 1086 stopCicloTB=1;
nerit 3:36da019e6bb6 1087 cntCellsForLoad=0;
nerit 3:36da019e6bb6 1088 }
nerit 3:36da019e6bb6 1089 }else{
nerit 3:36da019e6bb6 1090 cntCellsForLoad=0;
nerit 3:36da019e6bb6 1091 }
nerit 3:36da019e6bb6 1092 if (trigSD==0){inhibit=1;trigTB=1;}else{inhibit=0;trigTB=0;trigSD=0;}
nerit 3:36da019e6bb6 1093 // torna indietro per sbloccare il tamburo
nerit 3:36da019e6bb6 1094 if ((TBmotorDirecti==1)&&(erroreTamburo==1)){
nerit 3:36da019e6bb6 1095 cntCellsForReload++;
nerit 3:36da019e6bb6 1096 if (cntCellsForReload > 3){
nerit 3:36da019e6bb6 1097 TBmotorDirecti=0;
nerit 3:36da019e6bb6 1098 erroreTamburo=0;
nerit 3:36da019e6bb6 1099 }
nerit 3:36da019e6bb6 1100 }
nerit 3:36da019e6bb6 1101 }
nerit 3:36da019e6bb6 1102 if ((double)TBactualPosition > ((TBgiroStep/cellsNumber)*1.2f)){
nerit 3:36da019e6bb6 1103 if (firstStart==0){
nerit 3:36da019e6bb6 1104 all_cellSignal=1;
nerit 3:36da019e6bb6 1105 }
nerit 3:36da019e6bb6 1106 if (erroreTamburo==0){
nerit 3:36da019e6bb6 1107 erroreTamburo=1;
nerit 3:36da019e6bb6 1108 TBmotorDirecti=1;
nerit 3:36da019e6bb6 1109 cntCellsForReload=0;
nerit 3:36da019e6bb6 1110 cntTbError++;
nerit 3:36da019e6bb6 1111 }
nerit 3:36da019e6bb6 1112 #if defined(pcSerial)
nerit 3:36da019e6bb6 1113 #if defined(canDataReceived)
nerit 3:36da019e6bb6 1114 pc.printf("allarme error cells");
nerit 3:36da019e6bb6 1115 #endif
nerit 3:36da019e6bb6 1116 #endif
nerit 3:36da019e6bb6 1117 }
nerit 3:36da019e6bb6 1118 if (((double)TBactualPosition > ((TBgiroStep/cellsNumber)*3.0f))||(cntTbError>2)){
nerit 3:36da019e6bb6 1119 if (firstStart==0){
nerit 3:36da019e6bb6 1120 all_noStepRota=1;
nerit 3:36da019e6bb6 1121 }
nerit 3:36da019e6bb6 1122 cntTbError=0;
nerit 3:36da019e6bb6 1123 #if defined(pcSerial)
nerit 3:36da019e6bb6 1124 #if defined(canDataReceived)
nerit 3:36da019e6bb6 1125 pc.printf("allarme no cells sensor");
nerit 3:36da019e6bb6 1126 #endif
nerit 3:36da019e6bb6 1127 #endif
nerit 3:36da019e6bb6 1128 }
nerit 3:36da019e6bb6 1129 // ----------------------------------------
nerit 3:36da019e6bb6 1130 // read and manage joystick
nerit 3:36da019e6bb6 1131 // WARNING - ENABLE CYCLE IS SOFTWARE ALWAYS ACTIVE
nerit 3:36da019e6bb6 1132 if (enableCycle==1){
nerit 3:36da019e6bb6 1133 if(runRequestBuf==1){
nerit 3:36da019e6bb6 1134 if (OldStartCycle!=runRequestBuf){
nerit 3:36da019e6bb6 1135 if((startCycle==0)&&(zeroCycleEnd==1)){
nerit 3:36da019e6bb6 1136 startCycle=1;
nerit 3:36da019e6bb6 1137 OldStartCycle = runRequestBuf;
nerit 3:36da019e6bb6 1138 oldZeroCycle=0;
nerit 3:36da019e6bb6 1139 }
nerit 3:36da019e6bb6 1140 }
nerit 3:36da019e6bb6 1141 }else{
nerit 3:36da019e6bb6 1142 startCycle=0;
nerit 3:36da019e6bb6 1143 pntMedia=0;
nerit 3:36da019e6bb6 1144 }
nerit 3:36da019e6bb6 1145 if (azzeraDaCan==1){
nerit 3:36da019e6bb6 1146 if (tractorSpeed_MtS_timed==0.0f){
nerit 3:36da019e6bb6 1147 zeroRequestBuf=1;
nerit 3:36da019e6bb6 1148 oldZeroCycle=0;
nerit 3:36da019e6bb6 1149 }
nerit 3:36da019e6bb6 1150 azzeraDaCan=0;
nerit 3:36da019e6bb6 1151 }
nerit 3:36da019e6bb6 1152 if (loadDaCan==1){
nerit 3:36da019e6bb6 1153 if (tractorSpeed_MtS_timed==0.0f){
nerit 3:36da019e6bb6 1154 ciclaTB();
nerit 3:36da019e6bb6 1155 }
nerit 3:36da019e6bb6 1156 }
nerit 3:36da019e6bb6 1157 if ((zeroRequestBuf==1)){
nerit 3:36da019e6bb6 1158 if (oldZeroCycle!=zeroRequestBuf){
nerit 3:36da019e6bb6 1159 zeroCycle=1;
nerit 3:36da019e6bb6 1160 zeroCycleEnd=0;
nerit 3:36da019e6bb6 1161 SDzeroed=0;
nerit 3:36da019e6bb6 1162 TBzeroed=0;
nerit 3:36da019e6bb6 1163 zeroTrigger=0;
nerit 3:36da019e6bb6 1164 oldZeroCycle = zeroRequestBuf;
nerit 3:36da019e6bb6 1165 }
nerit 3:36da019e6bb6 1166 }
nerit 3:36da019e6bb6 1167 }else{
nerit 3:36da019e6bb6 1168 startCycle=0;
nerit 3:36da019e6bb6 1169 zeroCycle=0;
nerit 3:36da019e6bb6 1170 }
nerit 3:36da019e6bb6 1171
nerit 3:36da019e6bb6 1172 //***************************************************************************************************
nerit 3:36da019e6bb6 1173 // pulseRised define the event of speed wheel pulse occurs
nerit 3:36da019e6bb6 1174 //
nerit 3:36da019e6bb6 1175 double maxInterval = pulseDistance/minWorkSpeed;
nerit 3:36da019e6bb6 1176 double minIntervalPulse = pulseDistance/maxWorkSpeed;
nerit 3:36da019e6bb6 1177 if (pulseRised==1){
nerit 3:36da019e6bb6 1178 if (enableSpeed<10){enableSpeed++;}
nerit 3:36da019e6bb6 1179 pulseRised=0;pulseRised1=1;speedMediaCalc();
nerit 3:36da019e6bb6 1180 // calcola velocità trattore
nerit 3:36da019e6bb6 1181 if(enableSpeed>=2){
nerit 3:36da019e6bb6 1182 if ((pulseSpeedInterval>=0.0f)){ //minIntervalPulse)&&(pulseSpeedInterval<maxInterval)){
nerit 3:36da019e6bb6 1183 tractorSpeed_MtS_timed = ((pulseDistance / pulseSpeedInterval)); // tractor speed (unit= Mt/S) from pulse time interval
nerit 3:36da019e6bb6 1184 if (checkSDrotation==0){
nerit 3:36da019e6bb6 1185 checkSDrotation=1;
nerit 3:36da019e6bb6 1186 SDwheelTimer.start();
nerit 3:36da019e6bb6 1187 }
nerit 3:36da019e6bb6 1188 }
nerit 3:36da019e6bb6 1189 }
nerit 3:36da019e6bb6 1190 if ((pulseSpeedInterval>=minIntervalPulse)&&(pulseSpeedInterval<maxInterval)){
nerit 3:36da019e6bb6 1191 enableSDcontrol =1;
nerit 3:36da019e6bb6 1192 }
nerit 3:36da019e6bb6 1193 speedTimeOut.reset();
nerit 3:36da019e6bb6 1194 }else{
nerit 3:36da019e6bb6 1195 double oldLastPr = (double)oldLastPulseRead*1.7f;
nerit 3:36da019e6bb6 1196 if((double)speedTimeOut.read_us()> (oldLastPr)){
nerit 3:36da019e6bb6 1197 tractorSpeed_MtS_timed = 0.0f;
nerit 3:36da019e6bb6 1198 enableSDcontrol =0;
nerit 3:36da019e6bb6 1199 pntMedia=0;
nerit 3:36da019e6bb6 1200 speedTimeOut.reset();
nerit 3:36da019e6bb6 1201 enableSpeed=0;
nerit 3:36da019e6bb6 1202 quincCnt=0;
nerit 3:36da019e6bb6 1203 }
nerit 3:36da019e6bb6 1204 }
nerit 3:36da019e6bb6 1205 // esegue il controllo di velocità minima
nerit 3:36da019e6bb6 1206 if ((double)speedTimer.read_ms()>=maxInterval){tractorSpeed_MtS_timed = 0.0f;
nerit 3:36da019e6bb6 1207 enableSDcontrol =0;
nerit 3:36da019e6bb6 1208 enableSpeed=0;
nerit 3:36da019e6bb6 1209 }
nerit 3:36da019e6bb6 1210 //***************************************************************************************************************
nerit 3:36da019e6bb6 1211 // cycle logic control section
nerit 3:36da019e6bb6 1212 //***************************************************************************************************************
nerit 3:36da019e6bb6 1213 if (enableSimula==1){if(simOk==0){tractorSpeed_MtS_timed=0.0f;}}
nerit 3:36da019e6bb6 1214 if ((tractorSpeed_MtS_timed>0.01f)){
nerit 3:36da019e6bb6 1215 cycleStopRequest=1;
nerit 3:36da019e6bb6 1216 tempoGiroSD = seedPerimeter / tractorSpeed_MtS_timed; // tempo Teorico impiegato dalla ruota di semina per fare un giro completo (a 4,5Km/h impiega 1,89 secondi)
nerit 3:36da019e6bb6 1217 if (speedFromPick==1) {
nerit 3:36da019e6bb6 1218 tempoTraBecchi_mS = (tempoGiroSD / pickNumber)*1000.0f; // tempo tra due impulsi dai becchi in millisecondi ( circa 157mS a 4,5Km/h)
nerit 3:36da019e6bb6 1219 }else{
nerit 3:36da019e6bb6 1220 tempoTraBecchi_mS = (tempoGiroSD / 25.0f)*1000.0f; // tempo tra due impulsi dai becchi in millisecondi ( circa 157mS a 4,5Km/h)
nerit 3:36da019e6bb6 1221 }
nerit 3:36da019e6bb6 1222 seedWheelPeriodTeory = semiPeriodoReale;
nerit 3:36da019e6bb6 1223 seedWheelPeriod=seedWheelPeriodTeory;
nerit 3:36da019e6bb6 1224 if (seedWheelPeriod < 180.0f){seedWheelPeriod = 180.0f;}
nerit 3:36da019e6bb6 1225 if ((oldSeedWheelPeriod!=seedWheelPeriod)&&(seedWheelPeriod >=180.0f )){
nerit 3:36da019e6bb6 1226 SDticker.attach_us(&step_SDPulseOut,seedWheelPeriod); // clock time are milliseconds and attach seed motor stepper controls
nerit 3:36da019e6bb6 1227 oldSeedWheelPeriod=seedWheelPeriod;
nerit 3:36da019e6bb6 1228 }
nerit 3:36da019e6bb6 1229 //*******************************************
nerit 3:36da019e6bb6 1230 // segue calcolo duty cycle comando motore DC per allinearsi con la velocità del trattore
nerit 3:36da019e6bb6 1231 double dutyTeorico = 0.00;
nerit 3:36da019e6bb6 1232 if ((tractorSpeed_MtS_timed>0.0)&&(tractorSpeed_MtS_timed<tabSpeed[0])){dutyTeorico = tabComan[0];}
nerit 3:36da019e6bb6 1233 for (int ii = 0;ii<16;ii++){
nerit 3:36da019e6bb6 1234 if ((tractorSpeed_MtS_timed>=tabSpeed[ii])&&(tractorSpeed_MtS_timed<tabSpeed[ii+1])){
nerit 3:36da019e6bb6 1235 dutyTeorico = tabComan[ii+1];
nerit 3:36da019e6bb6 1236 }
nerit 3:36da019e6bb6 1237 }
nerit 3:36da019e6bb6 1238 if (tractorSpeed_MtS_timed > tabSpeed[16]){dutyTeorico=tractorSpeed_MtS_timed/1.25f;}
nerit 3:36da019e6bb6 1239 if ((pulseRised1==1)&&(enableSpeed>3)&&(pulseRised2==1)){
nerit 3:36da019e6bb6 1240 double erroreTempo = 0.0f;
nerit 3:36da019e6bb6 1241 if(speedFromPick==1){
nerit 3:36da019e6bb6 1242 erroreTempo = (double)timeIntraPick - tempoTraBecchi_mS; // errore tra il tempo previsto ed il tempo reale ( >0 se sto andando più piano del previsto)
nerit 3:36da019e6bb6 1243 }else{
nerit 3:36da019e6bb6 1244 erroreTempo = (double)memoTimeHole - tempoTraBecchi_mS; // errore tra il tempo previsto ed il tempo reale ( >0 se sto andando più piano del previsto)
nerit 3:36da019e6bb6 1245 }
nerit 3:36da019e6bb6 1246 double errorePercentuale = erroreTempo / tempoTraBecchi_mS;
nerit 3:36da019e6bb6 1247 double k1=errorePercentuale*0.065f;double k3=0.0f;double k4=0.0f;double k5=0.0f;
nerit 3:36da019e6bb6 1248 if (tractorSpeed_MtS_timed <= minSeedSpeed){k3=0.105;k4=0.207;k5=0.310;
nerit 3:36da019e6bb6 1249 }else{k3=0.1075f;k4=0.211f;k5=0.315f;}
nerit 3:36da019e6bb6 1250 if (errorePercentuale > 25.0f){k1=errorePercentuale*k5;}
nerit 3:36da019e6bb6 1251 if ((errorePercentuale >= 10.0f)&&(errorePercentuale<=25.0f)){k1=errorePercentuale*k4;}
nerit 3:36da019e6bb6 1252 if (errorePercentuale < 10.0f){k1=errorePercentuale*k3;}
nerit 3:36da019e6bb6 1253 double memoCorrezione = (dutyTeorico *k1);
nerit 3:36da019e6bb6 1254 correzione = correzione + memoCorrezione;
nerit 3:36da019e6bb6 1255 pulseRised1=0;
nerit 3:36da019e6bb6 1256 pulseRised2=0;
nerit 3:36da019e6bb6 1257 }
nerit 3:36da019e6bb6 1258 if (correzione > (dutyTeorico*0.20)){correzione = dutyTeorico*0.20;}
nerit 3:36da019e6bb6 1259 if((enableSDcontrol==1)){
nerit 3:36da019e6bb6 1260 if (correzioneAttiva==1){
nerit 3:36da019e6bb6 1261 dcActualDuty = dutyTeorico + correzione;
nerit 3:36da019e6bb6 1262 }
nerit 3:36da019e6bb6 1263 }else{
nerit 3:36da019e6bb6 1264 dcActualDuty = dutyTeorico;
nerit 3:36da019e6bb6 1265 }
nerit 3:36da019e6bb6 1266 // prova a mantenere il quinconce quando attivo
nerit 3:36da019e6bb6 1267 #if defined(speedMaster)
nerit 3:36da019e6bb6 1268 dcActualDuty=dcActualDuty;
nerit 3:36da019e6bb6 1269 DC_brake=0;
nerit 3:36da019e6bb6 1270 DC_forward=1;
nerit 3:36da019e6bb6 1271 DC_prepare();
nerit 3:36da019e6bb6 1272 #else
nerit 3:36da019e6bb6 1273 #if defined(mezzo)
nerit 3:36da019e6bb6 1274 if (quincEnable==1){
nerit 3:36da019e6bb6 1275 if (quinconceActive==1){
nerit 3:36da019e6bb6 1276 quinCalc(2.0f);
nerit 3:36da019e6bb6 1277 }else{
nerit 3:36da019e6bb6 1278 quinCalc(1.0f);
nerit 3:36da019e6bb6 1279 }
nerit 3:36da019e6bb6 1280 }else{
nerit 3:36da019e6bb6 1281 DC_brake=0;
nerit 3:36da019e6bb6 1282 DC_forward=1;
nerit 3:36da019e6bb6 1283 DC_prepare();
nerit 3:36da019e6bb6 1284 }
nerit 3:36da019e6bb6 1285 #else
nerit 3:36da019e6bb6 1286 if (quincEnable==1){
nerit 3:36da019e6bb6 1287 quinCalc(1.0f);
nerit 3:36da019e6bb6 1288 }else{
nerit 3:36da019e6bb6 1289 DC_brake=0;
nerit 3:36da019e6bb6 1290 DC_forward=1;
nerit 3:36da019e6bb6 1291 DC_prepare();
nerit 3:36da019e6bb6 1292 }
nerit 3:36da019e6bb6 1293 #endif
nerit 3:36da019e6bb6 1294 #endif
nerit 3:36da019e6bb6 1295 if (dcActualDuty <0.0f){dcActualDuty=0.0f;}
nerit 3:36da019e6bb6 1296 if (dcActualDuty > 1.0f){dcActualDuty = dcMaxSpeed;}
nerit 3:36da019e6bb6 1297 SDmotorPWM.write(dcActualDuty);
nerit 3:36da019e6bb6 1298 // allarme
nerit 3:36da019e6bb6 1299 if (SDwheelTimer.read_ms()>4000){
nerit 3:36da019e6bb6 1300 if (firstStart==0){
nerit 3:36da019e6bb6 1301 all_noDcRotati=1;
nerit 3:36da019e6bb6 1302 }
nerit 3:36da019e6bb6 1303 #if defined(pcSerial)
nerit 3:36da019e6bb6 1304 #if defined(canDataReceived)
nerit 3:36da019e6bb6 1305 pc.printf("allarme no DC rotation");
nerit 3:36da019e6bb6 1306 #endif
nerit 3:36da019e6bb6 1307 #endif
nerit 3:36da019e6bb6 1308
nerit 3:36da019e6bb6 1309 }
nerit 3:36da019e6bb6 1310
nerit 3:36da019e6bb6 1311 //***************************************************************************************************************
nerit 3:36da019e6bb6 1312 // CONTROLLA TAMBURO
nerit 3:36da019e6bb6 1313 //***************************************************************************************************************
nerit 3:36da019e6bb6 1314 if(lowSpeed==0){
nerit 3:36da019e6bb6 1315 if (syncroCheck==1){
nerit 3:36da019e6bb6 1316 syncroCheck=0;
nerit 3:36da019e6bb6 1317 lockStart=1;
nerit 3:36da019e6bb6 1318 periodo = TBperiod;
nerit 3:36da019e6bb6 1319 if (aspettaStart==0){cambiaTB(periodo);}
nerit 3:36da019e6bb6 1320 }
nerit 3:36da019e6bb6 1321 // controllo di stop
nerit 3:36da019e6bb6 1322 double memoIntraP = (double)memoIntraPick*1.8f;
nerit 3:36da019e6bb6 1323 if ((double)rotationTimeOut.read_ms()> (memoIntraP)){
nerit 3:36da019e6bb6 1324 syncroCheck=0;
nerit 3:36da019e6bb6 1325 aspettaStart=1;
nerit 3:36da019e6bb6 1326 countCicli=0;
nerit 3:36da019e6bb6 1327 if (TBzeroCyclePulse==1){TBticker.detach();}
nerit 3:36da019e6bb6 1328 }
nerit 3:36da019e6bb6 1329 }else{ // fine ciclo fuori da low speed
nerit 3:36da019e6bb6 1330 syncroCheck=0;
nerit 3:36da019e6bb6 1331 lockStart=0;
nerit 3:36da019e6bb6 1332 if (beccoPronto==1){
nerit 3:36da019e6bb6 1333 if (tamburoStandard==1){
nerit 3:36da019e6bb6 1334 double ritardoMassimo = 0.0f;
nerit 3:36da019e6bb6 1335 if(speedFromPick==1){
nerit 3:36da019e6bb6 1336 ritardoMassimo = (double)timeIntraPick;
nerit 3:36da019e6bb6 1337 }else{
nerit 3:36da019e6bb6 1338 ritardoMassimo = (double)memoTimeHole;
nerit 3:36da019e6bb6 1339 }
nerit 3:36da019e6bb6 1340 int tempoDiSincro = (int)((double)(ritardoMassimo - ((tempoBecco/2.0f)+((speedOfSeedWheel/1.25f)*ritardoMassimo)))); //
nerit 3:36da019e6bb6 1341 if (tempoDiSincro <= 1){tempoDiSincro=1;}
nerit 3:36da019e6bb6 1342 if ((sincroTimer.read_ms()>= tempoDiSincro)){
nerit 3:36da019e6bb6 1343 if (tractorSpeed_MtS_timed >= minWorkSpeed){startCicloTB=1;}
nerit 3:36da019e6bb6 1344 beccoPronto=0;
nerit 3:36da019e6bb6 1345 }
nerit 3:36da019e6bb6 1346 }else{
nerit 3:36da019e6bb6 1347 // tamburo per zucca
nerit 3:36da019e6bb6 1348 if (speedOfSeedWheel >= minWorkSpeed){startCicloTB=1;}
nerit 3:36da019e6bb6 1349 beccoPronto=0;
nerit 3:36da019e6bb6 1350 }
nerit 3:36da019e6bb6 1351 }
nerit 3:36da019e6bb6 1352 ciclaTB();
nerit 3:36da019e6bb6 1353 }
nerit 3:36da019e6bb6 1354 //*************************************************************
nerit 3:36da019e6bb6 1355 }else{ // fine ciclo con velocita maggiore di 0
nerit 3:36da019e6bb6 1356 SDwheelTimer.stop();
nerit 3:36da019e6bb6 1357 SDwheelTimer.reset();
nerit 3:36da019e6bb6 1358 checkSDrotation=0;
nerit 3:36da019e6bb6 1359 oldFaseLavoro=0;
nerit 3:36da019e6bb6 1360 aspettaStart=1;
nerit 3:36da019e6bb6 1361 countCicli=0;
nerit 3:36da019e6bb6 1362 startCycle=0;
nerit 3:36da019e6bb6 1363 oldSeedWheelPeriod=0.0f;
nerit 3:36da019e6bb6 1364 oldPeriodoTB=0.0f;
nerit 3:36da019e6bb6 1365 correzione=0.0f;
nerit 3:36da019e6bb6 1366 OLDpulseSpeedInterval=1000.01f;
nerit 3:36da019e6bb6 1367 cicloTbinCorso=0;
nerit 3:36da019e6bb6 1368 cntTbError=0;
nerit 3:36da019e6bb6 1369 if (cycleStopRequest==1){
nerit 3:36da019e6bb6 1370 zeroDelay.reset();
nerit 3:36da019e6bb6 1371 zeroDelay.start();
nerit 3:36da019e6bb6 1372 runRequestBuf=0;
nerit 3:36da019e6bb6 1373 zeroRequestBuf=1;
nerit 3:36da019e6bb6 1374 cycleStopRequest=0;
nerit 3:36da019e6bb6 1375 SDzeroCyclePulse=0;
nerit 3:36da019e6bb6 1376 TBzeroCyclePulse=0;
nerit 3:36da019e6bb6 1377 zeroCycleEnd=0;
nerit 3:36da019e6bb6 1378 zeroCycle=1;
nerit 3:36da019e6bb6 1379 zeroTrigger=0;
nerit 3:36da019e6bb6 1380 }
nerit 3:36da019e6bb6 1381 }
nerit 3:36da019e6bb6 1382 //*************************************************************************************************
nerit 3:36da019e6bb6 1383 // ciclo di azzeramento motori
nerit 3:36da019e6bb6 1384 if ((zeroCycleEnd==0)&&(zeroCycle==1)){//&&(zeroDelay.read_ms()>10000)){
nerit 3:36da019e6bb6 1385 if (zeroTrigger==0){
nerit 3:36da019e6bb6 1386 TBticker.attach_us(&step_TBPulseOut,1000.0f); // clock time are seconds and attach seed motor stepper controls
nerit 3:36da019e6bb6 1387 DC_forward=1;
nerit 3:36da019e6bb6 1388 DC_brake=1;
nerit 3:36da019e6bb6 1389 DC_prepare();
nerit 3:36da019e6bb6 1390 frenata=0;
nerit 3:36da019e6bb6 1391 zeroTrigger=1;
nerit 3:36da019e6bb6 1392 ritardoStop.reset();
nerit 3:36da019e6bb6 1393 ritardoComandoStop.reset();
nerit 3:36da019e6bb6 1394 ritardoComandoStop.start();
nerit 3:36da019e6bb6 1395 timeOutZeroSD.start();
nerit 3:36da019e6bb6 1396 quincTimeSet.reset();
nerit 3:36da019e6bb6 1397 }
nerit 3:36da019e6bb6 1398 int tempoFrenata=300;
nerit 3:36da019e6bb6 1399 if (((ritardoStop.read_ms()>tempoFrenata)&&(SDzeroDebounced==0))||(accensione==1)||(ritardoComandoStop.read_ms()>400)){
nerit 3:36da019e6bb6 1400 accensione=0;
nerit 3:36da019e6bb6 1401 avvicinamento=1;
nerit 3:36da019e6bb6 1402 avvicinamentoOn.reset();
nerit 3:36da019e6bb6 1403 avvicinamentoOn.start();
nerit 3:36da019e6bb6 1404 SDmotorPWM.write(0.32f); // duty cycle = 60% of power
nerit 3:36da019e6bb6 1405 DC_forward=1;
nerit 3:36da019e6bb6 1406 DC_brake=0;
nerit 3:36da019e6bb6 1407 DC_prepare();
nerit 3:36da019e6bb6 1408 ritardoComandoStop.reset();
nerit 3:36da019e6bb6 1409 ritardoComandoStop.stop();
nerit 3:36da019e6bb6 1410 }
nerit 3:36da019e6bb6 1411 if (avvicinamento==1){
nerit 3:36da019e6bb6 1412 if(avvicinamentoOn.read_ms()>300){
nerit 3:36da019e6bb6 1413 SDmotorPWM.write(0.7f);
nerit 3:36da019e6bb6 1414 avvicinamentoOn.reset();
nerit 3:36da019e6bb6 1415 avvicinamentoOff.reset();
nerit 3:36da019e6bb6 1416 avvicinamentoOff.start();
nerit 3:36da019e6bb6 1417 }
nerit 3:36da019e6bb6 1418 if(avvicinamentoOff.read_ms()>100){
nerit 3:36da019e6bb6 1419 SDmotorPWM.write(0.32f);
nerit 3:36da019e6bb6 1420 avvicinamentoOff.reset();
nerit 3:36da019e6bb6 1421 avvicinamentoOff.stop();
nerit 3:36da019e6bb6 1422 avvicinamentoOn.start();
nerit 3:36da019e6bb6 1423 }
nerit 3:36da019e6bb6 1424 }else{
nerit 3:36da019e6bb6 1425 avvicinamentoOn.stop();
nerit 3:36da019e6bb6 1426 avvicinamentoOff.stop();
nerit 3:36da019e6bb6 1427 avvicinamentoOn.reset();
nerit 3:36da019e6bb6 1428 avvicinamentoOff.reset();
nerit 3:36da019e6bb6 1429 }
nerit 3:36da019e6bb6 1430 if (frenata==0){
nerit 3:36da019e6bb6 1431 if (SDzeroCyclePulse==1){
nerit 3:36da019e6bb6 1432 SDticker.detach();
nerit 3:36da019e6bb6 1433 frenata=1;
nerit 3:36da019e6bb6 1434 quincTimeSet.reset();
nerit 3:36da019e6bb6 1435 quincTimeSet.start();
nerit 3:36da019e6bb6 1436 ritardoStop.start();
nerit 3:36da019e6bb6 1437 //ritardoComandoStop.reset();
nerit 3:36da019e6bb6 1438 //ritardoComandoStop.stop();
nerit 3:36da019e6bb6 1439 }
nerit 3:36da019e6bb6 1440 }else{
nerit 3:36da019e6bb6 1441 if (quinconceActive==0){
nerit 3:36da019e6bb6 1442 if (SDzeroCyclePulse==1){
nerit 3:36da019e6bb6 1443 avvicinamento=0;
nerit 3:36da019e6bb6 1444 DC_brake=1;
nerit 3:36da019e6bb6 1445 DC_prepare();
nerit 3:36da019e6bb6 1446 SDzeroed=1;
nerit 3:36da019e6bb6 1447 ritardoStop.reset();
nerit 3:36da019e6bb6 1448 ritardoStop.stop();
nerit 3:36da019e6bb6 1449 }
nerit 3:36da019e6bb6 1450 }else{
nerit 3:36da019e6bb6 1451 if (quincTimeSet.read_ms()>500){
nerit 3:36da019e6bb6 1452 avvicinamento=0;
nerit 3:36da019e6bb6 1453 DC_brake=1;
nerit 3:36da019e6bb6 1454 DC_prepare();
nerit 3:36da019e6bb6 1455 SDzeroed=1;
nerit 3:36da019e6bb6 1456 ritardoStop.reset();
nerit 3:36da019e6bb6 1457 ritardoStop.stop();
nerit 3:36da019e6bb6 1458 quincTimeSet.stop();
nerit 3:36da019e6bb6 1459 }
nerit 3:36da019e6bb6 1460 }
nerit 3:36da019e6bb6 1461 }
nerit 3:36da019e6bb6 1462 // azzera tutto in time out
nerit 3:36da019e6bb6 1463 if (timeOutZeroSD.read_ms()>=10000){
nerit 3:36da019e6bb6 1464 #if defined(pcSerial)
nerit 3:36da019e6bb6 1465 #if defined(canDataReceived)
nerit 3:36da019e6bb6 1466 pc.printf("allarme no zero");
nerit 3:36da019e6bb6 1467 #endif
nerit 3:36da019e6bb6 1468 #endif
nerit 3:36da019e6bb6 1469 if (firstStart==0){
nerit 3:36da019e6bb6 1470 all_no_Zeroing=1;
nerit 3:36da019e6bb6 1471 }
nerit 3:36da019e6bb6 1472 avvicinamento=0;
nerit 3:36da019e6bb6 1473 DC_brake=1;
nerit 3:36da019e6bb6 1474 DC_prepare();
nerit 3:36da019e6bb6 1475 SDzeroed=1;
nerit 3:36da019e6bb6 1476 ritardoStop.reset();
nerit 3:36da019e6bb6 1477 ritardoStop.stop();
nerit 3:36da019e6bb6 1478 avvicinamentoOn.stop();
nerit 3:36da019e6bb6 1479 avvicinamentoOff.stop();
nerit 3:36da019e6bb6 1480 avvicinamentoOn.reset();
nerit 3:36da019e6bb6 1481 avvicinamentoOff.reset();
nerit 3:36da019e6bb6 1482 ritardoComandoStop.reset();
nerit 3:36da019e6bb6 1483 ritardoComandoStop.stop();
nerit 3:36da019e6bb6 1484 timeOutZeroSD.stop();
nerit 3:36da019e6bb6 1485 timeOutZeroSD.reset();
nerit 3:36da019e6bb6 1486 }
nerit 3:36da019e6bb6 1487 if (TBzeroCyclePulse==1){
nerit 3:36da019e6bb6 1488 TBticker.detach();
nerit 3:36da019e6bb6 1489 TBzeroed=1;
nerit 3:36da019e6bb6 1490 }
nerit 3:36da019e6bb6 1491 if ((SDzeroed==1)&&(TBzeroed==1)){
nerit 3:36da019e6bb6 1492 avvicinamentoOn.stop();
nerit 3:36da019e6bb6 1493 avvicinamentoOff.stop();
nerit 3:36da019e6bb6 1494 ritardoComandoStop.stop();
nerit 3:36da019e6bb6 1495 ritardoStop.stop();
nerit 3:36da019e6bb6 1496 zeroCycleEnd=1;
nerit 3:36da019e6bb6 1497 zeroCycle=0;
nerit 3:36da019e6bb6 1498 zeroTrigger=0;
nerit 3:36da019e6bb6 1499 runRequestBuf=1;
nerit 3:36da019e6bb6 1500 zeroRequestBuf=0;
nerit 3:36da019e6bb6 1501 cycleStopRequest=0;
nerit 3:36da019e6bb6 1502 SDzeroed=0;
nerit 3:36da019e6bb6 1503 TBzeroed=0;
nerit 3:36da019e6bb6 1504 timeOutZeroSD.stop();
nerit 3:36da019e6bb6 1505 timeOutZeroSD.reset();
nerit 3:36da019e6bb6 1506 }
nerit 3:36da019e6bb6 1507 }
nerit 3:36da019e6bb6 1508
nerit 3:36da019e6bb6 1509 //*************************************************************************************************
nerit 3:36da019e6bb6 1510 if (enableCycle==0){
nerit 3:36da019e6bb6 1511 zeroTrigger=0;
nerit 3:36da019e6bb6 1512 SDmotorPWM=0;
nerit 3:36da019e6bb6 1513 SDmotorInA=0;
nerit 3:36da019e6bb6 1514 SDmotorInB=0;
nerit 3:36da019e6bb6 1515 }
nerit 3:36da019e6bb6 1516 SDzeroCyclePulse=0;
nerit 3:36da019e6bb6 1517 TBzeroCyclePulse=0;
nerit 3:36da019e6bb6 1518 //*************************************************************************************************
nerit 3:36da019e6bb6 1519 }else{//end ciclo normale
nerit 3:36da019e6bb6 1520 //*************************************************************************************************
nerit 3:36da019e6bb6 1521 // task di prova della scheda
nerit 3:36da019e6bb6 1522 //*************************************************************************************************
nerit 3:36da019e6bb6 1523 #if defined(provaScheda)
nerit 3:36da019e6bb6 1524 clocca++;
nerit 3:36da019e6bb6 1525 //led = !led;
nerit 3:36da019e6bb6 1526 //txMsg.clear();
nerit 3:36da019e6bb6 1527 //txMsg << clocca;
nerit 3:36da019e6bb6 1528 //test.printf("aogs \n");
nerit 3:36da019e6bb6 1529 //if(can1.write(txMsg)){
nerit 3:36da019e6bb6 1530 #if defined(pcSerial)
nerit 3:36da019e6bb6 1531 pc.printf("Can write OK \n");
nerit 3:36da019e6bb6 1532 #endif
nerit 3:36da019e6bb6 1533 //}
nerit 3:36da019e6bb6 1534 switch (clocca){
nerit 3:36da019e6bb6 1535 case 1:
nerit 3:36da019e6bb6 1536 TBmotorStepOut=1; // define step command for up down motor driver
nerit 3:36da019e6bb6 1537 break;
nerit 3:36da019e6bb6 1538 case 2:
nerit 3:36da019e6bb6 1539 SDmotorPWM=1; // define step command for seeding whell motor driver
nerit 3:36da019e6bb6 1540 break;
nerit 3:36da019e6bb6 1541 case 3:
nerit 3:36da019e6bb6 1542 speedClock=1; // define input of
nerit 3:36da019e6bb6 1543 break;
nerit 3:36da019e6bb6 1544 case 4:
nerit 3:36da019e6bb6 1545 break;
nerit 3:36da019e6bb6 1546 case 5:
nerit 3:36da019e6bb6 1547 SDmotorInA=1;
nerit 3:36da019e6bb6 1548 SDmotorInB=0;
nerit 3:36da019e6bb6 1549 break;
nerit 3:36da019e6bb6 1550 case 6:
nerit 3:36da019e6bb6 1551 break;
nerit 3:36da019e6bb6 1552 case 7:
nerit 3:36da019e6bb6 1553 break;
nerit 3:36da019e6bb6 1554 case 8:
nerit 3:36da019e6bb6 1555 break;
nerit 3:36da019e6bb6 1556 case 9:
nerit 3:36da019e6bb6 1557 break;
nerit 3:36da019e6bb6 1558 case 10:
nerit 3:36da019e6bb6 1559 break;
nerit 3:36da019e6bb6 1560 case 11:
nerit 3:36da019e6bb6 1561 break;
nerit 3:36da019e6bb6 1562 case 12:
nerit 3:36da019e6bb6 1563 break;
nerit 3:36da019e6bb6 1564 case 13:
nerit 3:36da019e6bb6 1565 break;
nerit 3:36da019e6bb6 1566 case 14:
nerit 3:36da019e6bb6 1567 SDmotorPWM=1; // power mosfet 2 command out
nerit 3:36da019e6bb6 1568 break;
nerit 3:36da019e6bb6 1569 case 15:
nerit 3:36da019e6bb6 1570 break;
nerit 3:36da019e6bb6 1571 case 16:
nerit 3:36da019e6bb6 1572 case 17:
nerit 3:36da019e6bb6 1573 break;
nerit 3:36da019e6bb6 1574 case 18:
nerit 3:36da019e6bb6 1575 TBmotorStepOut=0; // define step command for up down motor driver
nerit 3:36da019e6bb6 1576 SDmotorPWM=0; // define step command for seeding whell motor driver
nerit 3:36da019e6bb6 1577 speedClock=0; // define input of
nerit 3:36da019e6bb6 1578 SDmotorInA=0;
nerit 3:36da019e6bb6 1579 SDmotorInB=0;
nerit 3:36da019e6bb6 1580 SDmotorPWM=0; // power mosfet 2 command out
nerit 3:36da019e6bb6 1581 break;
nerit 3:36da019e6bb6 1582 default:
nerit 3:36da019e6bb6 1583 clocca=0;
nerit 3:36da019e6bb6 1584 break;
nerit 3:36da019e6bb6 1585 }
nerit 3:36da019e6bb6 1586 wait_ms(100);
nerit 3:36da019e6bb6 1587 #endif // end prova scheda
nerit 3:36da019e6bb6 1588
nerit 3:36da019e6bb6 1589 #if defined(provaDC)
nerit 3:36da019e6bb6 1590 int rampa=1000;
nerit 3:36da019e6bb6 1591 int pausa=3000;
nerit 3:36da019e6bb6 1592 switch (clocca){
nerit 3:36da019e6bb6 1593 case 0:
nerit 3:36da019e6bb6 1594 DC_brake=0;
nerit 3:36da019e6bb6 1595 DC_forward=1;
nerit 3:36da019e6bb6 1596 duty_DC+=0.01f;
nerit 3:36da019e6bb6 1597 if (duty_DC>=0.2f){
nerit 3:36da019e6bb6 1598 duty_DC=0.2f;
nerit 3:36da019e6bb6 1599 clocca=1;
nerit 3:36da019e6bb6 1600 }
nerit 3:36da019e6bb6 1601 wait_ms(rampa);
nerit 3:36da019e6bb6 1602 break;
nerit 3:36da019e6bb6 1603 case 1:
nerit 3:36da019e6bb6 1604 wait_ms(pausa*4);
nerit 3:36da019e6bb6 1605 clocca=2;
nerit 3:36da019e6bb6 1606 break;
nerit 3:36da019e6bb6 1607 case 2:
nerit 3:36da019e6bb6 1608 DC_brake=0;
nerit 3:36da019e6bb6 1609 DC_forward=1;
nerit 3:36da019e6bb6 1610 duty_DC-=0.01f;
nerit 3:36da019e6bb6 1611 if (duty_DC<=0.0f){
nerit 3:36da019e6bb6 1612 duty_DC=0.0f;
nerit 3:36da019e6bb6 1613 clocca=3;
nerit 3:36da019e6bb6 1614 }
nerit 3:36da019e6bb6 1615 wait_ms(rampa);
nerit 3:36da019e6bb6 1616 break;
nerit 3:36da019e6bb6 1617 case 3:
nerit 3:36da019e6bb6 1618 wait_ms(pausa);
nerit 3:36da019e6bb6 1619 clocca=4;
nerit 3:36da019e6bb6 1620 break;
nerit 3:36da019e6bb6 1621 case 4:
nerit 3:36da019e6bb6 1622 DC_brake=0;
nerit 3:36da019e6bb6 1623 DC_forward=1;
nerit 3:36da019e6bb6 1624 duty_DC+=0.01f;
nerit 3:36da019e6bb6 1625 if (duty_DC>=1.0f){
nerit 3:36da019e6bb6 1626 duty_DC=1.0f;
nerit 3:36da019e6bb6 1627 clocca=5;
nerit 3:36da019e6bb6 1628 }
nerit 3:36da019e6bb6 1629 wait_ms(rampa);
nerit 3:36da019e6bb6 1630 break;
nerit 3:36da019e6bb6 1631 case 5:
nerit 3:36da019e6bb6 1632 wait_ms(pausa);
nerit 3:36da019e6bb6 1633 clocca=6;
nerit 3:36da019e6bb6 1634 break;
nerit 3:36da019e6bb6 1635 case 6:
nerit 3:36da019e6bb6 1636 DC_brake=0;
nerit 3:36da019e6bb6 1637 DC_forward=1;
nerit 3:36da019e6bb6 1638 duty_DC-=0.01f;
nerit 3:36da019e6bb6 1639 if (duty_DC<=0.0f){
nerit 3:36da019e6bb6 1640 duty_DC=0.0f;
nerit 3:36da019e6bb6 1641 clocca=7;
nerit 3:36da019e6bb6 1642 }
nerit 3:36da019e6bb6 1643 wait_ms(rampa);
nerit 3:36da019e6bb6 1644 break;
nerit 3:36da019e6bb6 1645 case 7:
nerit 3:36da019e6bb6 1646 wait_ms(pausa);
nerit 3:36da019e6bb6 1647 clocca=0;
nerit 3:36da019e6bb6 1648 break;
nerit 3:36da019e6bb6 1649 default:
nerit 3:36da019e6bb6 1650 break;
nerit 3:36da019e6bb6 1651 }
nerit 3:36da019e6bb6 1652 if (oldDuty_DC != duty_DC){
nerit 3:36da019e6bb6 1653 SDmotorPWM.write(duty_DC); // duty cycle = stop
nerit 3:36da019e6bb6 1654 oldDuty_DC=duty_DC;
nerit 3:36da019e6bb6 1655 DC_prepare();
nerit 3:36da019e6bb6 1656 }
nerit 3:36da019e6bb6 1657 #endif
nerit 3:36da019e6bb6 1658 }//end in prova
nerit 3:36da019e6bb6 1659 } // end while
nerit 3:36da019e6bb6 1660 }// end main
nerit 3:36da019e6bb6 1661