Francesco Pistone
/
FORIGO_Modula_V6_R2C_DE_HwV5_OTTOBRE2018
tr
Diff: main.cpp
- Revision:
- 0:1e09cd7d66b4
- Child:
- 1:e88bf5011af6
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Jun 13 08:52:42 2018 +0000 @@ -0,0 +1,1841 @@ + //******************************************************************************************************************** +//******************************************************************************************************************** +// FIRMWARE SEMINATRICE MODULA +// VERSIONE PER SCHEDA DI CONTROLLO CON DRIVER INTEGRATI +// V4 - ATTENZIONE - LA VERSIONE V4 HA IL DRIVER STEPPER LV8727 +// IL MOTORE DC E' GESTITO CON IL DRIVER VNH3SP30-E E CON LA LETTURA +// DELLA CORRENTE ASSORBITA TRAMITE IL CONVERTITORE MLX91210-CAS102 CON 50A FONDOSCALA +// CHE FORNISCE UNA TENSIONE DI USCITA PARI A 40mV/A +// FIRST RELEASE OF BOARD DEC 2017 +// FIRST RELEASE OF FIRMWARE JAN 2018 +// +// THIS RELEASE: 09 JULY 2018 +// +// APPLICATION: MODULA CON DISTRIBUTORE ZUCCA OPPURE RISO E PUO' FUNZIONARE ANCHE CON SENSORE A 25 FORI SUL DISCO O +// ENCODER MOTORE SETTANDO GLI APPOSITI FLAGS +// +// 29 05 2018 - INSERITO SECONDO ENCODER VIRTUALE PER LA GESTIONE DEL SINCRONISMO TRA TAMBURO E RUOTA DI SEMINA +// IN PRATICA IL PRIMO ENCODER è SINCRONO CON IL SEGNALE DEI BECCHI E VIENE AZZERATO DA QUESTI, MENTRE +// IL SECONDO E' INCREMENTATO IN SINCRONO CON IL PRIMO MA AZZERATO DALLA FASE. IL SUO VALORE E' POI DIVISO +// PER IL RAPPORTO RUOTE E LA CORREZIONE AGISCE SULLA VELOCITA' DEL TAMBURO PER MANTENERE LA FASE DEL SECONDO +// ENCODER +// 05 06 2018 - INSERITO IL CONTROLLO DI GESTIONE DEL QUINCONCE SENZA ENCODER +// 09 06 2018 - INSERITO CONTROLLO DI FASE CON ENCODER MASTER PER QUINCONCE - DATO SCAMBIATO IN CAN +// +/******************** +IL FIRMWARE SI COMPONE DI 7 FILES: + - main.cpp + - main.hpp + - iodefinition.hpp + - canbus.hpp + - parameters.hpp + - timeandtick.hpp + - variables.hpp +ED UTILIZZA LE LIBRERIE STANDARD MBED PIU' UNA LIBRERIA MODIFICATA E DEDICATA PER IL CAN +********************* +LA MACCHINA UTILIZZA SEMPRE 2 SOLI SENSORI; UNO PER SENTIRE LE CELLE DI CARICO SEME ED UNO PER SENTIRE I BECCHI DI SEMINA. +GLI AZIONAMENTI SONO COMPOSTI DA DUE MOTORI; UN DC PER IL CONTROLLO DELLA RUOTA DI SEMINA ED UNO STEPPER PER IL CONTROLLO DEL TAMBURO +UN SENSORE AGGIUNTIVO SULL'ELEMENTO MASTER RILEVA LA VELOCITA' DI AVANZAMENTO +UN SENSORE AGGIUNTIVO SULLA RUOTA DI SEMINA RILEVA LA ROTAZIONE DELLA RUOTA STESSA ATTRAVERSO FORI PRESENTI SUL DISCO DI SEMINA +********************* +LA LOGICA GENERALE PREVEDE CHE IL DC DELLA RUOTA DI SEMINA VENGA COMANDATO IN FUNZIONE DELLA VELOCITA' LETTA DAL SENSORE DI AVANZAMAENTO DEL MASTER +IL PROBLEMA PRINCIPALE E' CHE QUANDO I BECCHI SONO INSERITI NEL TERRENO NON VI E' RETROAZIONE REALE SULLA VELOCITA' DI ROTAZIONE DELLA RUOTA STESSA +PROPRIO PERCHE' L'AVANZAMANETO NEL TERRENO IMPRIME UNA VELOCITA' PROPRIA AL BECCO E QUINDI ANCHE ALLA RUOTA. +PER OVVIARE A QUESTO PROBLEMA SI E' INSERITO UN CONTROLLO DI CORRENTE ASSORBITA DAL DC; SE E' BASSA DEVO ACCELERARE, SE E' ALTA DEVO RALLENTARE +IL VALORE DI RIFERIMENTO DELL'ANALOGICA DI INGRESSO VIENE AGGIORNATO OGNI VOLTA CHE LA RUOTA DI SEMINA E' FERMA +IL TAMBURO SEGUE LA RUOTA DI SEMINA RILEVANDONE LA VELOCITA' E RICALCOLANDO LA PROPRIA IN FUNZIONE DELLA REALE VELOCITA' DI ROTAZIONE DELLA RUOTA DI SEMINA +LA FASE VIENE DETERMINATA DAL PASSAGGIO DEI BECCHI SUL SENSORE RELATIVO. +IL PROBLEMA PRINCIPALE NEL MANTENERE LA FASE DEL TAMBURO E' DATO DAL FATTO CHE LA SINCRONIZZAZIONE DELLA FASE SOLO SULL'IMPULSO DEL BECCO NON E' SUFFICIENTE +SOPRATUTTO QUANDO I BECCHI SONO MOLTO DISTANZIATI. +PER OVVIARE A QUESTO PROBLEMA SI SONO INSERITI DUE ENCODER VIRTUALI CHE SEZIONANO LA RUOTA DI SEMINA IN 9000 PARTI. ENTRAMBI VENGONO GESTITI DA UN GENERATORE DINAMICO DI CLOCK INTERNO +TARATO SULLA REALE VELOCITA' DI ROTAZIONE DELLA RUOTA DI SEMINA. +IL PRIMO ENCODER VIRTUALE SI OCCUPA DI DETERMINARE LA POSIZIONE FISICA DELLA RUOTA DI SEMINA E SI AZZERA AL PASSAGGIO DI OGNI BECCO. +IL SECONDO VIENE AZZERATO DALL'IMPULSO DI FASE DEL PRIMO ENCODER DETERMINATO DAI VALORI IMPOSTI SUL TERMINALE TRITECNICA +IL SECONDO ENCODER VIENE CONFRONTATO CON LA POSIZIONE ASSOLUTA DEL TAMBURO (DETERMINATA DAL NUMERO DI STEP EMESSI DAL CONTROLLO), RAPPORTATA TRA CELLE E BECCHI. +IL CONFRONTO DETERMINA LA POSIZIONE RELATIVA DELLA SINGOLA CELLA RISPETTO AL SINGOLO BECCO. IL MANTENIMENTO DELLA SINCRONIZZAZIONE DI FASE, DETERMINA IL SINCRO CELLA/BECCO. +LA SINCRONIZZAZIONE VIENE PERO' E' A SUA VOLTA RICALCOLATA SHIFTANDO LA POSIZIONE DI AZZERAMENTO DEL SECONDO ENCODER IN FUNZIONE DELLA VELOCITA' DI ROTAZIONE GENERALE AL FINE +DI CAMBIARE L'ANGOLO DI ANTICIPO DI RILASCIO DEL SEME IN FUNZIONE DELLA VELOCITA' E RECUPERARE COSI' IL TEMPO DI VOLO DEL SEME. +IL TAMBURO HA DUE TIPI DI FUNZIONAMENTO: CONTINUO E AD IMPULSI. E' SELEZIONABILE IN FUNZIONE DELLA VELOCITA' E DEL TIPO DI DISTRIBUTORE MONTATO. +********************** +TUTTI I VALORI, CELLE, BECCHI, IMPULSI VELOCITA', ANCGOLO DI AVVIO, FASE DI SEMINA, ECC.. SONO IMPOSTABILI DA PANNELLO OPERATORE +I DATI SONO SCAMBIATI CON IL PANNELLO OPERATORE E CON GLI ALTRI MODULI ATTRAVERSO RETE CAN CON PROTOCOLLO FREESTYLE ATTRAVERSO INDIRIZZAMENTI DEDICATI +AL MOMENTO NON E' POSSIBILE ATTRIBUIRE L'INIDIRIZZO BASE DELL'ELEMENTO DA TERMINALE OPERATORE MA SOLO IN FASE DI COMPILAZIONE DEL FIRMWARE. +********************** +ALTRE SEZIONI RIGUARDANO LA GENERAZIONE DEGLI ALLARMI, LA COMUNICAZIONE CAN, LA SIMULAZIONE DI LAVORO, LA GESTIONE DELLA DIAGNOSI ECC.. +IL MOTORE DC E' CONTROLLATO DA DIVERSE ROUTINE; LE PRIORITA' SONO (DALLA PIU' BASSA ALLA PIU' ALTA): CALCOLO TEORICO, RICALCOLO REALE, CONTROLLO DI FASE QUINCONCE, CONTROLLO DI CORRENTE. +LO STEPPER SEGUE IL DC. +********************** +IN FASE DI ACCENSIONE ED OGNI QUALVOLTA SI ARRIVA A VELOCITA' ZERO, LA MACCHINA ESEGUE UN CICLO DI AZZERAMENTO +NON ESISTE PULSANTE DI MARCIA/STOP; E' SEMPRE ATTIVA. +********************** +NEL PROGRAMMA E' PRESENTE UNA SEZIONE DI TEST FISICO DELLA SCHEDA ATTIVABILE SOLO IN FASE DI COMPILAZIONE +********************** +ALTRE FUNZIONI: PRECARICAMENTO DEL TAMBURO + AZZERAMENTO MANUALE + STATISTICA DI SEMINA (CONTA LE CELLE) +*/ +//******************************************************************************************************************** +//******************************************************************************************************************** + +#include "main.hpp" +#include "timeandtick.hpp" +#include "canbus.hpp" +#include "watchdog.h" +#include "iodefinition.hpp" +#include "parameters.hpp" +#include "variables.hpp" +//******************************************************************************************************************** +//******************************************************************************************************************** +// ------------------------------------------------------------------------------------------------------------------------------------------------------------------ +// TASK SECTION +// ------------------------------------------------------------------------------------------------------------------------------------------------------------------ +//************************************************************************ +// rise of seed speed 25 pulse sensor +void sd25Fall(){ + timeHole=metalTimer.read_ms(); + int memo_TimeHole= (memoTimeHole + timeHole)/ (int)2; + memoTimeHole = timeHole; + metalTimer.reset(); + if (speedFromPick==0){ + speedOfSeedWheel=((seedPerimeter/25.0f)/(double)memo_TimeHole)*1000.0f; //mtS + } +} +//************************************************** +// generate speed clock when speed is simulated from Tritecnica display +void speedSimulationClock(){ + lastPulseRead=speedTimer.read_us(); + oldLastPulseRead=lastPulseRead; + speedTimer.reset(); + pulseRised=1; +} +//******************************************************* +// interrupt task for tractor speed reading +//******************************************************* +void tractorReadSpeed(){ + if ((oldTractorSpeedRead==0)){ + lastPulseRead=speedTimer.read_us(); + oldLastPulseRead=lastPulseRead; + speedTimer.reset(); + pulseRised=1; + oldTractorSpeedRead=1; + } + speedFilter.reset(); + speedClock=1; +} +//******************************************************* +void speedMediaCalc(){ + double lastPd=(double) lastPulseRead/1000.0f; + pulseSpeedInterval = (mediaSpeed[0]+lastPd)/2.0f; + if (enableSimula==1){ + double TMT = (double)(speedSimula) * 100.0f /3600.0f; + pulseSpeedInterval = pulseDistance / TMT; + } + mediaSpeed[0]=lastPd; + OLDpulseSpeedInterval=pulseSpeedInterval; +} + +//******************************************************* +// clocked task for manage virtual encoder of seed wheel i/o +//******************************************************* +//******************************************************* +void step_SDPulseOut(){ + SDactualPosition++; + prePosSD++; +} +//******************************************************* +void step_TBPulseOut(){ + TBmotorStepOut=!TBmotorStepOut; + if (TBmotorStepOut==0){ + TBactualPosition++; + } +} +//******************************************************* +// aggiornamento parametri di lavoro fissi e da Tritecnica +void aggiornaParametri(){ + speedPerimeter = Pi * speedWheelDiameter ; // perimeter of speed wheel + pulseDistance = (speedPerimeter / speedWheelPulse)*1000.0f; // linear space between speed wheel pulse + seedPerimeter = Pi * (seedWheelDiameter-(deepOfSeed*2.0f)); // perimeter of seed wheel + intraPickDistance = seedPerimeter/pickNumber; + K_WheelRPM = 60.0f/seedPerimeter; // calcola il K per i giri al minuto della ruota di semina + K_WhellFrequency = (seedWheelMotorSteps*SDreductionRatio)/60.0f; // calcola il K per la frequenza di comando del motore di semina + rapportoRuote = pickNumber/cellsNumber; // calcola il rapporto tra il numero di becchi ed il numero di celle + K_TBfrequency = (TBmotorSteps*TBreductionRatio)/240.0f; + K_percentuale = TBmotorSteps*TBreductionRatio; + SDsectorStep = (double)fixedStepGiroSD / (double)pickNumber; + TBsectorStep = (TBmotorSteps*TBreductionRatio)/cellsNumber; + KcorT = (SDsectorStep/TBsectorStep);///2.0f; + angoloFase=angoloPh; + avvioGradi=angoloAv; + stepGrado=fixedStepGiroSD/360.0f; + TBdeltaStep=(fixedStepGiroSD/pickNumber)+(stepGrado*avvioGradi); + TBfaseStep = (stepGrado*angoloFase); + K_WheelRPM = 60.0f/seedPerimeter; // calcola il K per i giri al minuto della ruota di semina 25.4 25,40 + if (speedFromPick==1) { + intraPickDistance = seedPerimeter/pickNumber; + }else{ + intraPickDistance = seedPerimeter/25.0f; // 25 è il numero di fori presenti nel disco di semina + } + if (anticipoMax > ((360.0f/pickNumber)-(avvioGradi+16.0f))){ + anticipoMax=(360.0f/pickNumber)-(avvioGradi+16.0f); + } +} +//******************************************************* +void cambiaTB(double perio){ + // update TB frequency + double TBper=0.0f; + if (aspettaStart==0){ + if (perio<250.0f){perio=500.0f;} + double scala =0.0f; + if (lowSpeed==1){ + scala =2.0f; + }else{ + scala =1.8f; + } + TBper=perio/scala; + if (oldPeriodoTB!=TBper){ + if (TBper >= 250.0f){ + TBticker.attach_us(&step_TBPulseOut,TBper); // clock time are milliseconds and attach seed motor stepper controls + }else{ + TBticker.detach(); + } + oldPeriodoTB=TBper; + } + } +} +//******************************************************* +void seedCorrect(){ + /* + posError determina la posizione relativa di TB rispetto ad SD + la reale posizione di SD viene modificata in funzione della velocità per + traslare la posizione relativa di TB. All'aumentare della velocità la posizione + di SD viene incrementata così che TB acceleri per raggiungerla in modo da rilasciare il seme prima + La taratura del sistema avviene determinando prima il valore di angoloFase alla minima velocità, + poi, alla massima velocità, dovrebbe spostarsi la posizione relativa con una variabile proporzionale alla velocità, ma c'è un però. + Il problema è che il momento di avvio determina una correzione dell'angolo di partenza del tamburo + angolo che viene rideterminato ogni volta che il sensore becchi legge un transito. + Di fatto c'è una concorrenza tra l'angolo di avvio determinato e la correzione di posizione relativa + del tamburo. E' molto probabile che convenga modificare solo la posizione relativa e non anche l'angolo di avvio + Ancora di più se viene eliminata la parte gestita da ciclata. + In questo modo dovrebbe esserci solo un andamento in accelerazione di TB che viene poi eventualmente decelerato + dal passaggio sul sensore di TB. Funzione corretta perchè il sincronismo tra i sensori genera l'inibizione della correzione + di fase di TB. In pratica il ciclo viene resettato al passaggio sul sensore di SD che riporta a 0 la posizione di SD. + Appena il sensore di TB viene impegnato allora viene abilitato il controllo di fase del tamburo. + Questo si traduce nel fatto che il controllo di posizione viene gestito solo all'interno di uno slot di semina in modo che + il tamburo non risenta della condizione di reset della posizione di SD mentre lui è ancora nella fase precedente. Si fermerebbe. + + // La considerazione finale è che mantenendo l'angolo di avvio fisso e regolato sulla bassa velocità, intervenendo solo sulla correzione + // di posizione in questa routine, dovrebbe essere possibile seminare correttamente a tutte le velocità regolando solo 2 parametri. + */ + /* + SDsectorStep = (double)fixedStepGiroSD / (double)pickNumber; + TBsectorStep = (TBmotorSteps*TBreductionRatio)/cellsNumber; + KcorT = (SDsectorStep/TBsectorStep); + angoloFase=angoloPh; + stepGrado=fixedStepGiroSD/360.0f; + avvioGradi = costante da terminale tritecnica + TBdeltaStep=(fixedStepGiroSD/pickNumber)-(stepGrado*avvioGradi); + TBfaseStep = (stepGrado*angoloFase); + anticipoMax = valore in gradi del massimo anticipo proporzionale alla velocità della ruota di semina + */ + + if ((tractorSpeed_MtS_timed>0.01f)){ + if (inhibit==0){ + double posError =0.0f; + double posSD=((double)SDactualPosition)/KcorT; + posError = posSD - (double)TBactualPosition; + // interviene sulla velocità di TB per raggiungere la corretta posizione relativa + if((lowSpeed==0)&&(aspettaStart==0)){ + if (posError>50.0f){posError=50.0f;} + if (posError<-50.0f){posError=-50.0f;} + if ((posError >=1.0f)||(posError<=-1.0f)){ + ePpos = periodo /(1.0f+ ((posError/100.0f))); + cambiaTB(ePpos); + } + } + } + #if defined(pcSerial) + #if defined(SDreset) + pc.printf("Fase: %d",fase); + pc.printf(" PrePosSD: %d",prePosSD); + pc.printf(" PosSD: %d",SDactualPosition); + pc.printf(" speed: %f",tractorSpeed_MtS_timed); + pc.printf(" Trigger: %d \n", trigRepos); + #endif + #endif + } +} +//******************************************************* +void videoUpdate(){ + for(int aa=0;aa<4;aa++){speedForDisplay[aa]=speedForDisplay[aa+1];} + speedForDisplay[4]=tractorSpeed_MtS_timed; + totalSpeed=0.0f; + for (int aa=0; aa<5; aa++){totalSpeed += speedForDisplay[aa];} + totalSpeed = totalSpeed / 5.0f; +} +//******************************************************* +void ciclaTB(){ + if ((startCicloTB==1)&&(cicloTbinCorso==0)){ + TBticker.attach_us(&step_TBPulseOut,TBperiod/2.5f); // clock time are milliseconds and attach seed motor stepper controls + cicloTbinCorso = 1; + startCicloTB=0; + } + if ((loadDaCan==1)&&(loadDaCanInCorso==0)){ + TBticker.attach_us(&step_TBPulseOut,1000.0f); // clock time are milliseconds and attach seed motor stepper controls + loadDaCanInCorso=1; + stopCicloTB=0; + } + if ((stopCicloTB==1)&&(TBactualPosition>5)){ + TBticker.detach(); + cicloTbinCorso = 0; + stopCicloTB=0; + loadDaCanInCorso=0; + loadDaCan=0; + } +} +//******************************************************* +void allarmi(){ + uint8_t alarmLowRegister1=0x00; + alarmLowRegister=0x00; + alarmHighRegister=0x80; + + //alarmLowRegister=alarmLowRegister+(all_semiFiniti*0x01); // manca il sensore + alarmLowRegister=alarmLowRegister+(all_pickSignal*0x02); // fatto + alarmLowRegister=alarmLowRegister+(all_cellSignal*0x04); // fatto + alarmLowRegister=alarmLowRegister+(all_lowBattery*0x08); // fatto + alarmLowRegister=alarmLowRegister+(all_overCurrDC*0x10); // fatto + alarmLowRegister=alarmLowRegister+(all_stopSistem*0x20); // verificarne la necessità + //alarmLowRegister=alarmLowRegister+(all_upElements*0x40); // manca il sensore + //alarmLowRegister=alarmLowRegister+(all_noSeedOnCe*0x80); // manca il sensore + + //alarmLowRegister1=alarmLowRegister1+(all_cfgnErrors*0x01); // da scrivere + alarmLowRegister1=alarmLowRegister1+(all_noDcRotati*0x02); // fatto + alarmLowRegister1=alarmLowRegister1+(all_noStepRota*0x04); // fatto + alarmLowRegister1=alarmLowRegister1+(all_speedError*0x08); // fatto + alarmLowRegister1=alarmLowRegister1+(all_noSpeedSen*0x10); // fatto + alarmLowRegister1=alarmLowRegister1+(all_no_Zeroing*0x20); // fatto + alarmLowRegister1=alarmLowRegister1+(all_genericals*0x40); + if (alarmLowRegister1 > 0){ + alarmHighRegister = 0x81; + alarmLowRegister = alarmLowRegister1; + } + + #if defined(pcSerial) + #if defined(VediAllarmi) + if (all_pickSignal==1){pc.printf("AllarmeBecchi\n");} + if (all_cellSignal==1){pc.printf("AllarmeCelle\n");} + if (all_lowBattery==1){pc.printf("AllarmeBassaCorrente\n");} + if (all_overCurrDC==1){pc.printf("AllarmeAltaCorrente\n");} + if (all_stopSistem==1){pc.printf("AllarmeStop\n");} + if (all_noDcRotati==1){pc.printf("AllarmeDCnoRotation\n");} + if (all_noStepRota==1){pc.printf("AllarmeNoStepRotation\n");} + if (all_speedError==1){pc.printf("AllarmeSpeedError\n");} + if (all_noSpeedSen==1){pc.printf("AllarmeNoSpeedSensor\n");} + if (all_no_Zeroing==1){pc.printf("AllarmeNoZero\n");} + if (all_genericals==1){pc.printf("AllarmeGenerico\n");} + pc.printf("Code: 0x%x%x\n",alarmHighRegister,alarmLowRegister); + #endif + #endif + all_semiFiniti=0; + all_pickSignal=0; + all_cellSignal=0; + all_lowBattery=0; + all_overCurrDC=0; + all_stopSistem=0; + all_upElements=0; + all_noSeedOnCe=0; + all_cfgnErrors=0; + all_noDcRotati=0; + all_noStepRota=0; + all_speedError=0; + all_noSpeedSen=0; + all_no_Zeroing=0; + all_genericals=0; +} +//******************************************************* +#if defined(speedMaster) + void upDateSincro(){ + char val1[8]={0,0,0,0,0,0,0,0}; + val1[0]=(prePosSD /0x00FF0000)&0x000000FF; + val1[1]=(prePosSD /0x0000FF00)&0x000000FF; + val1[2]=(prePosSD /0x000000FF)&0x000000FF; + val1[3]=prePosSD & 0x000000FF; + #if defined(canbusActive) + #if defined(speedMaster) + if(can1.write(CANMessage(TX_SI, *&val1,8))){ + checkState=0; + } + #endif + #endif + } +#endif +//******************************************************* +void upDateSpeed(){ + /* + aggiorna dati OPUSA3 + val1[0] contiene il dato di velocità + val1[1] contiene il byte basso della tabella allarmi + uint8_t all_semiFiniti = 0; // semi finiti (generato dal relativo sensore) + uint8_t all_pickSignal = 0; // errore segnale becchi (generato dal tempo tra un becco ed il successivo) + uint8_t all_cellSignal = 0; // errore segnale celle (generato dal sensore tamburo ) + uint8_t all_lowBattery = 0; // allarme batteria (valore interno di tritecnica) + uint8_t all_overCurrDC = 0; // sovracorrente motore DC (generato dal sensore di corrente) + uint8_t all_stopSistem = 0; // sistema in stop (a tempo con ruota ferma) + uint8_t all_upElements = 0; // elementi sollevati (generato dal relativo sensore) + uint8_t all_noSeedOnCe = 0; // fallanza di semina (manca il seme nella cella) + uint8_t all_cfgnErrors = 0; // errore di configurazione (incongruenza dei parametri impostati) + uint8_t all_noDcRotati = 0; // ruota di semina bloccata (arriva la velocità ma non i becchi) + uint8_t all_noStepRota = 0; // tamburo di semina bloccato (comando il tamburo ma non ricevo il sensore) + uint8_t all_speedError = 0; // errore sensore velocità (tempo impulsi non costante) + uint8_t all_noSpeedSen = 0; // mancanza segnale di velocità (sento i becchi ma non la velocita) + uint8_t all_no_Zeroing = 0; // mancato azzeramento sistema (generato dal timeout del comando motore DC) + uint8_t all_genericals = 0; // allarme generico + val1[2] contiene il byte alto della tabella di allarmi + val1[3] contiene i segnali per la diagnostica + bit 0= sensore ruota fonica + bit 1= sensore celle + bit 2= sensore posizione + bit 3= sensore becchi + bit 4= motore DC + bit 5= controller + bit 6= motore stepper + */ + char val1[8]={0,0,0,0,0,0,0,0}; + + val1[3]=0; + val1[3]=val1[3]+(tractorSpeedRead*0x01); + val1[3]=val1[3]+(TBzeroPinInputRev*0x02); + val1[3]=val1[3]+(ElementPosition*0x04); + val1[3]=val1[3]+(seedWheelZeroPinInputRev*0x08); + #if defined(simulaPerCan) + if (buttonUser==0){ + val1[1]=0x02; + }else{ + val1[1]=0x00; + } + val1[3]=valore; + valore+=1; + if(valore>50){ + valore=0; + } + tractorSpeed_MtS_timed=valore; + #endif + allarmi(); + valore = totalSpeed*36.0f; // tractorSpeed_MtS_timed*36.0f; + val1[0]=valore; + val1[1]=alarmHighRegister; // registro alto allarmi. Parte sempre da 0x80 + val1[2]=alarmLowRegister; // registro basso allarmi + //val1[3]=val1[3];// registro di stato degli ingressi + val1[4]=resetComandi; + val1[5]=cellsCounterLow; + val1[6]=cellsCounterHig; + #if defined(canbusActive) + if(can1.write(CANMessage(TX_ID, *&val1,8))){ + checkState=0; + } + #endif +} +//******************************************************* +void leggiCAN(){ + #if defined(canbusActive) + if(can1.read(rxMsg)) { + if (firstStart==1){ + #if defined(speedMaster) + sincUpdate.attach(&upDateSincro,0.01f); + #endif + canUpdate.attach(&upDateSpeed,0.11f); + firstStart=0; + } + + if (rxMsg.id==RX_ID){ + #if defined(pcSerial) + #if defined(canDataReceived) + pc.printf("Messaggio ricevuto\n"); + #endif + #endif + } + if (rxMsg.id==RX_Broadcast){ + #if defined(pcSerial) + #if defined(canDataReceived) + pc.printf("BroadCast ricevuto\n"); + #endif + #endif + enableSimula= rxMsg.data[0]; + speedSimula = rxMsg.data[1]; + avviaSimula = rxMsg.data[2]; + selezionato = rxMsg.data[3]; + comandiDaCan = rxMsg.data[4]; + #if defined(pcSerial) + #if defined(canDataReceived) + pc.printf("Comandi: %x\n",comandiDaCan); + #endif + #endif + switch (comandiDaCan){ + case 1: + case 3: + azzeraDaCan=1; + resetComandi=0x01; + comandiDaCan=0; + break; + case 2: + loadDaCan=1; + resetComandi=0x02; + comandiDaCan=0; + break; + default: + comandiDaCan=0; + resetComandi=0xFF; + break; + } + #if defined(pcSerial) + #if defined(canDataReceived) + pc.printf("Comandi: %x\n",resetComandi); + #endif + #endif + if (speedSimula>45){ + speedSimula=45; + } + // modulo 1 + if (RX_ID==0x100){ + if ((selezionato&0x01)==0x01){ + simOk=1; + }else{ + simOk=0; + } + } + // modulo 2 + if (RX_ID==0x102){ + if ((selezionato&0x02)==0x02){ + simOk=1; + }else{ + simOk=0; + } + } + // modulo 3 + if (RX_ID==0x104){ + if ((selezionato&0x04)==0x04){ + simOk=1; + }else{ + simOk=0; + } + } + // modulo 4 + if (RX_ID==0x106){ + if ((selezionato&0x08)==0x08){ + simOk=1; + }else{ + simOk=0; + } + } + // modulo 5 + if (RX_ID==0x108){ + if ((selezionato&0x10)==0x10){ + simOk=1; + }else{ + simOk=0; + } + } + + } + if (rxMsg.id==RX_Settings){ + 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 + seedWheelDiameter = ((rxMsg.data[4]*0xFF)+rxMsg.data[3])/10000.0f; // seed wheel diameter setting + speedWheelDiameter = ((rxMsg.data[6]*0xFF)+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 defined(pcSerial) + #if defined(canDataReceived) + pc.printf("Seed wheel diameter %f \n",seedWheelDiameter); + pc.printf("Speed wheel diameter %f \n",speedWheelDiameter); + #endif + #endif + } + } + if (rxMsg.id==RX_AngoloPh){ + if (tractorSpeed_MtS_timed==0.0f){ + #if defined(M1) + angoloPh = (double) rxMsg.data[0] ; + aggiornaParametri(); + #endif + #if defined(M2) + angoloPh = (double) rxMsg.data[1] ; + aggiornaParametri(); + #endif + #if defined(M3) + angoloPh = (double) rxMsg.data[2] ; + aggiornaParametri(); + #endif + #if defined(M4) + angoloPh = (double) rxMsg.data[3] ; + aggiornaParametri(); + #endif + #if defined(M5) + angoloPh = (double) rxMsg.data[4] ; + aggiornaParametri(); + #endif + #if defined(M6) + angoloPh = (double) rxMsg.data[5] ; + aggiornaParametri(); + #endif + } + } + if (rxMsg.id==RX_AngoloAv){ + if (tractorSpeed_MtS_timed==0.0f){ + #if defined(M1) + angoloAv = (double) rxMsg.data[0] ; + aggiornaParametri(); + #endif + #if defined(M2) + angoloAv = (double) rxMsg.data[1] ; + aggiornaParametri(); + #endif + #if defined(M3) + angoloAv = (double) rxMsg.data[2] ; + aggiornaParametri(); + #endif + #if defined(M4) + angoloAv = (double) rxMsg.data[3] ; + aggiornaParametri(); + #endif + #if defined(M5) + angoloAv = (double) rxMsg.data[4] ; + aggiornaParametri(); + #endif + #if defined(M6) + angoloAv = (double) rxMsg.data[5] ; + aggiornaParametri(); + #endif + } + } + if (rxMsg.id==RX_Quinconce){ + if (tractorSpeed_MtS_timed==0.0f){ + quinconceActive = (uint8_t) rxMsg.data[0]; + aggiornaParametri(); + } + } + if (rxMsg.id==RX_QuincSinc){ + masterSinc = (uint32_t) rxMsg.data[0] * 0x00FF0000; + masterSinc = masterSinc + ((uint32_t) rxMsg.data[1] * 0x0000FF00); + masterSinc = masterSinc + ((uint32_t) rxMsg.data[2] * 0x000000FF); + masterSinc = masterSinc + ((uint32_t) rxMsg.data[3]); + quincVerify=1; + //pc.printf("pippo"); + } + } + #endif + #if defined(overWriteCanSimulation) + enableSimula=1; + speedSimula=25; + avviaSimula=1; + simOk=1; + #endif + +} + +//******************************************************* +void DC_CheckCurrent(){ + + // TODO: tabella di riferimento assorbimenti alle varie velocità al fine di gestire + // gli allarmi e le correzioni di velocità + + //float SD_analogMatrix[10]={0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f}; + //int SD_analogIndex[10]={0,0,0,0,0,0,0,0,0,0}; + // Analog reading + //number = floor(number * 100) / 100; + //if (pwmCheck==1){ + timeout.detach(); + for (int ii=1;ii<20;ii++){ + SD_analogMatrix[ii]=SD_analogMatrix[ii+1]; + } + SD_CurrentAnalog = floor(SDcurrent.read()*100)/100; // valore in ingresso compreso tra 0.00 e 1.00 + SD_analogMatrix[20]=SD_CurrentAnalog; + if (SDmotorPWM==0.0f){ + SD_CurrentStart=SD_CurrentAnalog; + } + float sommaTutto=0.0f; + for (int ii=1;ii<21;ii++){ + sommaTutto=sommaTutto+SD_analogMatrix[ii]; + } + float SD_CurrentAnalogica=sommaTutto/20.0f; + SD_CurrentScaled = floor(( (SD_CurrentAnalogica - SD_CurrentStart)*3.3f) / (SD_CurrentFactor/1000.0f)*10)/10; + #if defined(pcSerial) + #if defined(correnteAssorbita) + pc.printf("CorrenteStart: %f ",SD_CurrentStart); + pc.printf(" CorrenteScaled: %f ",SD_CurrentScaled); + pc.printf(" CorrenteMedia: %f \n",SD_CurrentAnalogica); + #endif + #endif + reduceCurrent=false; + incrementCurrent=false; + /* + if (SD_CurrentScaled < 3.0f){ + #if defined(canbusActive) + all_lowBattery=1; + #endif + incrementCurrent=true; + } + */ + if (SD_CurrentScaled > 13.0f){ + #if defined(canbusActive) + all_overCurrDC=1; + #endif + reduceCurrent=true; + } + //} +} +//******************************************************* +void DC_prepare(){ + // direction or brake preparation + if (DC_brake==1){SDmotorInA=1;SDmotorInB=1;}else{if (DC_forward==1){SDmotorInA=1;SDmotorInB=0;}else{SDmotorInA=0;SDmotorInB=1;}} + // fault reading + if (SDmotorInA==1){SD_faultA=1;}else{SD_faultA=0;} + if (SDmotorInB==1){SD_faultB=1;}else{SD_faultB=0;} +} +//******************************************************* +void startDelay(){ + int ritardo =0; + ritardo = (int)((float)(dcActualDuty*800.0f)); + timeout.attach_us(&DC_CheckCurrent,ritardo); +} +//******************************************************* +void quincTrigga(){ + if (quinconceIn==1){ + quincClock=true; + }else{ + quincClock=false; + } +} +//******************************************************* +void quinCalc(double parametro){ + double riferimento=tempoTraBecchi_mS/parametro; + // riceve l'impulso di sincro dal master e fa partire il timer di verifica dell'errore + if (quinconceActive==0){ + if ((quincClock==true)&&(oldQuincIn==0)){ + oldQuincIn=1; + if (trigSlave==false){ + quincInibit=true; + trigDaMaster=true; + }else{ + quincInibit=false; + trigDaMaster=false; + trigSlave=false; + } + if (quincStart==0){ + quincTime.reset(); + quincStart=1; + } + } + if( quincClock==false){ + oldQuincIn=0; + } + }else{ + if ((quincClock==false)&&(oldQuincIn==0)){ + oldQuincIn=1; + if (trigSlave==false){ + quincInibit=true; + trigDaMaster=true; + }else{ + quincInibit=false; + trigDaMaster=false; + trigSlave=false; + } + if (quincStart==0){ + quincTime.reset(); + quincStart=1; + } + } + if( quincClock==true){ + oldQuincIn=0; + } + } + // riceve l'impulso di passaggio del becco locale e determina il valore dell'errore + // controllo di linea e di quinconcio + if ((quinconceActive==0)||(quinconceActive==1)){ + if ((quincStart==1)&&(SDzeroDebounced==1)){ + if (quincInibit==false){ + memoDuty=correzione; + accelera=0; + decelera=0; + quincStart=0; + percento=1.0f; + scostamento = (double)quincTime.read_ms(); + quincError = 0.0f; + double halfRef=riferimento/2.0f; + if ((scostamento > 0.0f)&&(scostamento < halfRef)){ + decelera=1; + accelera=0; + percento = 1.0f - (abs(scostamento)/riferimento); + } + if ((scostamento < riferimento)&&(scostamento > halfRef)){ + decelera=0; + accelera=1; + quincError = riferimento - scostamento; + percento = 1.0f + (abs(quincError)/riferimento); + } + if (abs(percento) > 1.15f){percento=1.15f;} + if (abs(percento) < 0.5f){percento=0.5f;} + } + } + if (abs(percento) >0.0f){ + correzione = memoDuty* abs(percento); + correggiSD=true; + }else{ + correzione = memoDuty; + correggiSD=false; + } + } + /* + // gestione con confronto encoder + // serve l'encoder fisico sugli elementi slave perchè altrimenti la ruota cambia velocità ma non cambia + // il clock dell'encoder virtuale e quindi l'errore non può essere rilevato correttamente + if ((quinconceActive==0)&&(quincEnable==1)){ + // da encoder master + if (quincVerify==1){ + quincVerify=0; + quincStart=0; + memoDuty=dcActualDuty; + if (quincInibit==false){ + accelera=0; + decelera=0; + percento=0.0f; + quincError = 0.0f; + if (masterSinc > prePosSD){ + decelera=0; + accelera=1; + quincError = masterSinc - prePosSD; + percento = 1.0f + ((abs(quincError)/SDsectorStep)); + } + if (masterSinc < prePosSD){ + decelera=1; + accelera=0; + quincError = prePosSD -masterSinc; + percento = 1.0f - ((abs(quincError)/SDsectorStep)); + } + // se quinconce richiesto deve andare al valore di riferimento + if (abs(percento) > 1.55f){percento=1.55f;} + if (abs(percento) < 0.40f){percento=0.40f;} + if (abs(percento) >0.0f){ + if (abs(percento)>1.0f){ + correzione=correzione+(abs(percento)*0.01f); + }else{ + correzione=correzione-(abs(percento)*0.01f); + } + //dcActualDuty = memoDuty * abs(percento); + }else{ + correzione=correzione; + //dcActualDuty = memoDuty; + } + } + #if defined(pcSerial) + #if defined(Qnc) + pc.printf(" trigMaster: %d", trigDaMaster); + pc.printf(" trigSlave: %d", trigSlave); + pc.printf(" quincinibit: %d", quincInibit); + pc.printf(" masterSinc: %d", masterSinc); + pc.printf(" prePosSD: %d",prePosSD); + pc.printf(" percento: %f",percento); + pc.printf(" correzione: %f\n",correzione); + #endif + #endif + } + } + */ +} +// ------------------------------------------------------------------------------------------------------------------------------------------------------------------ +// MAIN SECTION +// ------------------------------------------------------------------------------------------------------------------------------------------------------------------ + +int main() +{ + wd.Configure(2); //watchdog set at xx seconds + + for (int a=0; a<5;a++){ + mediaSpeed[a]=0; + } + + // Stepper driver init and set + TBmotorRst=0; // reset stepper driver + TBmotorDirecti=0; // reset stepper direction + // M1 M2 M3 RESOLUTION + // 0 0 0 1/2 + // 1 0 0 1/8 + // 0 1 0 1/16 + // 1 1 0 1/32 + // 0 0 1 1/64 + // 1 0 1 1/128 + // 0 1 1 1/10 + // 1 1 1 1/20 + if (TBmotorSteps==400){ + TBmotor_M1=0; + TBmotor_M2=0; + TBmotor_M3=0; + }else if (TBmotorSteps==1600){ + TBmotor_M1=1; + TBmotor_M2=0; + TBmotor_M3=0; + }else if (TBmotorSteps==3200){ + TBmotor_M1=0; + TBmotor_M2=1; + TBmotor_M3=0; + }else if (TBmotorSteps==6400){ + TBmotor_M1=1; + TBmotor_M2=1; + TBmotor_M3=0; + }else if (TBmotorSteps==12800){ + TBmotor_M1=0; + TBmotor_M2=0; + TBmotor_M3=1; + }else if (TBmotorSteps==25600){ + TBmotor_M1=1; + TBmotor_M2=0; + TBmotor_M3=1; + }else if (TBmotorSteps==2000){ + TBmotor_M1=0; + TBmotor_M2=1; + TBmotor_M3=1; + }else if (TBmotorSteps==4000){ + TBmotor_M1=1; + TBmotor_M2=1; + TBmotor_M3=1; + } + TBmotorRst=1; + + // DC reset ad set + int decima = 100; + wait_ms(200); + SD_CurrentStart=floor(SDcurrent.read()*decima)/decima; + wait_ms(2); + SD_CurrentStart+=floor(SDcurrent.read()*decima)/decima; + wait_ms(1); + SD_CurrentStart+=floor(SDcurrent.read()*decima)/decima; + wait_ms(3); + SD_CurrentStart+=floor(SDcurrent.read()*decima)/decima; + SD_CurrentStart=(floor((SD_CurrentStart/4.0f)*decima)/decima)-0.01f; + wait_ms(100); + DC_prepare(); + + speedTimer.start(); // speed pulse timer + intraPickTimer.start(); + speedTimeOut.start(); + speedFilter.start(); + seedFilter.start(); + TBfilter.start(); + sincroTimer.start(); + rotationTimeOut.start(); + metalTimer.start(); + quincTime.start(); + + //******************************************************* + // controls for check DC motor current + //currentCheck.attach(&DC_CheckCurrent, 0.10f); + pwmCheck.rise(&startDelay); + //tbCorrection.attach(&correction,0.1f; + + if (inProva==0){ + tractorSpeedRead.rise(&tractorReadSpeed); + #if !defined(speedMaster) + quinconceIn.rise(&quincTrigga); + quinconceIn.fall(&quincTrigga); + #endif + tftUpdate.attach(&videoUpdate,0.50f); + 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 + if (speedFromPick==0){ + ElementPosition.rise(&sd25Fall); + } + }else{ + tftUpdate.attach(&videoUpdate,0.125f); + } + + aggiornaParametri(); + + SDmotorPWM.period_us(periodoSD); // frequency 1KHz pilotaggio motore DC + SDmotorPWM.write(0.0f); // duty cycle = stop + TBmotorDirecti=0; // tb motor direction set + + #if defined(provaStepper) + TBmotorRst=1; + TBticker.attach_us(&step_TBPulseOut,3000.0f); // clock time are seconds and attach seed motor stepper controls + #endif // end prova stepper + + #if defined(canbusActive) + can1.attach(&leggiCAN, CAN::RxIrq); + #endif + +//************************************************************************************************************** +// MAIN LOOP +//************************************************************************************************************** + while (true){ + // ripetitore segnale di velocità + if (tractorSpeedRead==0){speedClock=0;} + + // inversione segnali ingressi + TBzeroPinInput = !TBzeroPinInputRev; + seedWheelZeroPinInput = !seedWheelZeroPinInputRev; + + // se quinconce attivo ed unita' master invia segnale di sincro + #if defined(speedMaster) + if (seedWheelZeroPinInput==1){ + quinconceOut=1; + } + if ((double)SDactualPosition >= (SDsectorStep/2.0f)){ + quinconceOut=0; + } + #endif + + #if defined(overWriteCanSimulation) + leggiCAN(); + #endif + + // simulazione velocita + if (enableSimula==1){ + double TMT = 0.0f; + if (speedSimula > 0){ + TMT = (double)(speedSimula) * 100.0f /3600.0f; + pulseSpeedInterval = pulseDistance / TMT; + }else{ + pulseSpeedInterval = 10000.0f; + } + if (avviaSimula==1){ + if(oldSimulaSpeed!=pulseSpeedInterval){ + spedSimclock.attach_us(&speedSimulationClock,pulseSpeedInterval); + oldSimulaSpeed=pulseSpeedInterval; + } + }else{ + oldSimulaSpeed=10000.0f; + spedSimclock.detach(); + } + }else{ + spedSimclock.detach(); + } + + //******************************************************* + // determina se sono in bassa velocità per il controllo di TB + if (speedOfSeedWheel<=minSeedSpeed){ + if (lowSpeedRequired==0){ + ritardaLowSpeed.reset(); + ritardaLowSpeed.start(); + } + lowSpeedRequired=1; + }else{ + if (lowSpeedRequired==1){ + lowSpeedRequired=0; + ritardaLowSpeed.reset(); + ritardaLowSpeed.stop(); + } + } + + if (ritardaLowSpeed.read_ms()> 2000){ + lowSpeed=1; + }else{ + lowSpeed=0; + } + + +//************************************************************************************************************** +//************************************************************************************************************** +// LOGICAL CONTROLS +//************************************************************************************************************** +//************************************************************************************************************** + + if (inProva==0){ + if ((startCycleSimulation==0)&&(enableSimula==0)){zeroRequestBuf=1;runRequestBuf=0;enableCycle=1; + }else{zeroRequestBuf=1;runRequestBuf=0;enableCycle=1;} + if ((tractorSpeedRead==0)&&(speedFilter.read_ms()>3)){oldTractorSpeedRead=0;} + // ---------------------------------------- + // filtra il segnale dei becchi per misurare il tempo di intervallo tra loro + // ---------------------------------------- + if (seedWheelZeroPinInput==0){ + //trigSlave=false; + if(seedFilter.read_ms()>=4){ + oldSeedWheelZeroPinInput=0; + SDzeroDebounced=0; + } + } + if ((seedWheelZeroPinInput==1)&&(oldSeedWheelZeroPinInput==0)){ + timeIntraPick = (double)intraPickTimer.read_ms(); + prePosSD=500; // preposizionamento SD + intraPickTimer.reset(); + rotationTimeOut.reset(); + seedFilter.reset(); + sincroTimer.reset(); + oldSeedWheelZeroPinInput=1; + SDzeroDebounced=1; + if (trigDaMaster==false){ + quincInibit=true; + trigSlave=true; + }else{ + quincInibit=false; + trigSlave=false; + trigDaMaster=false; + } + SDwheelTimer.reset(); + if (quincCnt<10){ + quincCnt++; + } + if (quincCnt >3){ + quincEnable=1; + }else{ + quincEnable=0; + } + if ((aspettaStart==0)&&(lowSpeed==1)){ + beccoPronto=1; + } + SDzeroCyclePulse=1; + lockStart=0; + double fase1=0.0f; + forzaFase=0; + double limite=fixedStepGiroSD/pickNumber; + if (pickCounterLow< 0xFF){ + pickCounterLow++; + }else{ + pickCounterHig++; + pickCounterLow=0; + } + if (tamburoStandard==0){ + fase1=TBdeltaStep; + }else{ + if(speedForCorrection >= speedOfSeedWheel){ + fase1=TBdeltaStep; + }else{ + //fase1=(TBdeltaStep)-(((speedOfSeedWheel-speedForCorrection)/1.25f)*(TBfaseStep)); + fase1=(TBdeltaStep)-(((speedOfSeedWheel)/1.25f)*(TBfaseStep)); + } + if (fase1 > limite){ + fase1 -= limite; // se la fase calcolata supera gli step del settore riporta il valore nell'arco precedente (es. fase 1 800, limite 750, risulta 50) + //forzaFase=1; + } + if ((fase1 > (limite -20.0f))&&(fase1<(limite +5.0f))){ + fase1 = limite -20.0f; // se la fase è molto vicina al limite interpone uno spazio minimo di lavoro del sincronismo + forzaFase=1; + } + trigRepos=1; + } + fase = (uint32_t)fase1+500; + #if defined(pcSerial) + #if defined(inCorre) + pc.printf(" limite %f", limite); + pc.printf(" delta %f", TBdeltaStep); + pc.printf(" faseStep %f", TBfaseStep); + pc.printf(" fase %d",fase); + pc.printf(" forzaFase %d",forzaFase); + pc.printf(" trigRepos %d", trigRepos); + pc.printf(" ActualSD: %d",SDactualPosition); + pc.printf(" SpeedWheel: %f",speedOfSeedWheel); + pc.printf(" SPEED: %f \n",tractorSpeed_MtS_timed); + #endif + #endif + if (timeIntraPick >= (memoIntraPick*2)){ + if ((aspettaStart==0)&&(zeroRequestBuf==0)){ + if (firstStart==0){ + all_pickSignal=1; + } + } + } + memoIntraPick = timeIntraPick; + if (speedFromPick==1){ + speedOfSeedWheel=((seedPerimeter / pickNumber)/timeIntraPick)*1000.0f; + } + if ((tractorSpeed_MtS_timed==0.0f)&&(zeroCycle==0)&&(zeroCycleEnd==1)){ + if ((firstStart==0)&&(simOk==0)&&(enableSimula==0)&&(zeroCycleEnd==1)){ + all_noSpeedSen=1; + } + #if defined(pcSerial) + #if defined(canDataReceived) + pc.printf("allarme no speed sensor"); + #endif + #endif + } + pulseRised2=1; + #if defined(speedMaster) + double oldLastPr = (double)oldLastPulseRead*1.2f; + if((double)speedTimeOut.read_us()> (oldLastPr)){ + if ((firstStart==0)&&(simOk==0)&&(enableSimula==0)&&(zeroCycleEnd==1)){ + all_speedError =1; + } + #if defined(pcSerial) + #if defined(canDataReceived) + pc.printf("allarme errore speed sensor"); + #endif + #endif + + } + #endif + //******************************************* + // esegue calcolo clock per la generazione della posizione teorica + // la realtà in base al segnale di presenza del becco + realSpeed = speedOfSeedWheel; + realGiroSD = seedPerimeter / realSpeed; + tempoBecco = (realGiroSD/360.0f)*16000.0f; + frequenzaReale = fixedStepGiroSD/realGiroSD; + semiPeriodoReale = (1000000.0f/frequenzaReale); + tempoTraBecchi_mS = 0.0f; + seedWheelRPM = (speedOfSeedWheel)*K_WheelRPM ; // calcola i giri al minuto della ruota di semina 7.37 31,75 + rapportoRuote = pickNumber/cellsNumber; // 0.67 calcola il rapporto tra il numero di becchi ed il numero di celle 0.8 1,00 + TBrpm = seedWheelRPM*rapportoRuote; // 5.896 31,75 + TBgiroStep = TBmotorSteps*TBreductionRatio; + K_TBfrequency = TBgiroStep/60.0f; // 1600 * 1.65625f /60 = 44 44,00 + TBfrequency = (TBrpm*K_TBfrequency); // 130Hz a 0,29Mts 1397,00 a 1,25mt/s con 15 becchi e 15 celle + TBperiod=1000000.0f/TBfrequency; // 715uS + } +// ---------------------------------------- +// check SD fase + if ((prePosSD >= fase)||(forzaFase==1)){//&&(prePosSD < (fase +30))){ + forzaFase=0; + if (trigRepos==1){ + SDactualPosition=0; + if ((countCicli<30)&&(trigCicli==0)){countCicli++;trigCicli=1;} + if(countCicli>=cicliAspettaStart){aspettaStart=0;} + if ((lowSpeed==0)&&(aspettaStart==0)&&(lockStart==0)){syncroCheck=1;beccoPronto=0;} + if (trigTB==0){ + inhibit=1; + trigSD=1; + }else{ + inhibit=0; + trigTB=0; + trigSD=0; + } + trigRepos=0; + } + }else{ + trigCicli=0; + } +// ---------------------------------------- +// filtra il segnale del tamburo per lo stop in fase del tamburo stesso + if (TBzeroPinInput==0){if (TBfilter.read_ms()>=5){oldTBzeroPinInput=0;}} + if ((TBzeroPinInput==1)&&(oldTBzeroPinInput==0)){ + oldTBzeroPinInput=1; + if (loadDaCanInCorso==0){ + stopCicloTB=1; + startCicloTB=0; + } + TBfilter.reset(); + TBzeroCyclePulse=1; + TBactualPosition=0; + if (cellsCounterLow < 0xFF){ + cellsCounterLow++; + }else{ + cellsCounterHig++; + cellsCounterLow=0; + } + if (loadDaCanInCorso==1){ + cntCellsForLoad++; + if (cntCellsForLoad >= 5){ + stopCicloTB=1; + cntCellsForLoad=0; + } + }else{ + cntCellsForLoad=0; + } + if (trigSD==0){ + inhibit=1; + trigTB=1; + }else{ + inhibit=0; + trigTB=0; + trigSD=0; + } + // torna indietro per sbloccare il tamburo + if ((TBmotorDirecti==1)&&(erroreTamburo==1)){ + cntCellsForReload++; + if (cntCellsForReload > 3){ + TBmotorDirecti=0; + erroreTamburo=0; + } + } + } + if ((double)TBactualPosition > ((TBgiroStep/cellsNumber)*1.2f)){ + if (firstStart==0){ + all_cellSignal=1; + } + if (erroreTamburo==0){ + erroreTamburo=1; + TBmotorDirecti=1; + cntCellsForReload=0; + cntTbError++; + } + #if defined(pcSerial) + #if defined(canDataReceived) + pc.printf("allarme error cells"); + #endif + #endif + } + if (((double)TBactualPosition > ((TBgiroStep/cellsNumber)*3.0f))||(cntTbError>2)){ + if (firstStart==0){ + all_noStepRota=1; + } + cntTbError=0; + #if defined(pcSerial) + #if defined(canDataReceived) + pc.printf("allarme no cells sensor"); + #endif + #endif + } +// ---------------------------------------- +// read and manage joystick + // WARNING - ENABLE CYCLE IS SOFTWARE ALWAYS ACTIVE + if (enableCycle==1){ + if(runRequestBuf==1){ + if (OldStartCycle!=runRequestBuf){ + if((startCycle==0)&&(zeroCycleEnd==1)){ + startCycle=1; + OldStartCycle = runRequestBuf; + oldZeroCycle=0; + } + } + }else{ + startCycle=0; + pntMedia=0; + } + if (azzeraDaCan==1){ + if (tractorSpeed_MtS_timed==0.0f){ + zeroRequestBuf=1; + oldZeroCycle=0; + } + azzeraDaCan=0; + } + if (loadDaCan==1){ + if (tractorSpeed_MtS_timed==0.0f){ + ciclaTB(); + } + } + if ((zeroRequestBuf==1)){ + if (oldZeroCycle!=zeroRequestBuf){ + zeroCycle=1; + zeroCycleEnd=0; + SDzeroed=0; + TBzeroed=0; + zeroTrigger=0; + oldZeroCycle = zeroRequestBuf; + } + } + }else{ + startCycle=0; + zeroCycle=0; + } + + //*************************************************************************************************** + // pulseRised define the event of speed wheel pulse occurs + // + double maxInterval = pulseDistance/minWorkSpeed; + double minIntervalPulse = pulseDistance/maxWorkSpeed; + if (pulseRised==1){ + if (enableSpeed<10){enableSpeed++;} + pulseRised=0;pulseRised1=1;speedMediaCalc(); + // calcola velocità trattore + if(enableSpeed>=2){ + if ((pulseSpeedInterval>=0.0f)){ //minIntervalPulse)&&(pulseSpeedInterval<maxInterval)){ + tractorSpeed_MtS_timed = ((pulseDistance / pulseSpeedInterval)); // tractor speed (unit= Mt/S) from pulse time interval + if (checkSDrotation==0){ + checkSDrotation=1; + SDwheelTimer.start(); + } + } + } + if ((pulseSpeedInterval>=minIntervalPulse)&&(pulseSpeedInterval<maxInterval)){ + enableSDcontrol =1; + } + speedTimeOut.reset(); + }else{ + double oldLastPr = (double)oldLastPulseRead*1.7f; + if((double)speedTimeOut.read_us()> (oldLastPr)){ + tractorSpeed_MtS_timed = 0.0f; + enableSDcontrol =0; + pntMedia=0; + speedTimeOut.reset(); + enableSpeed=0; + quincCnt=0; + } + } + // esegue il controllo di velocità minima + if ((double)speedTimer.read_ms()>=maxInterval){tractorSpeed_MtS_timed = 0.0f; + enableSDcontrol =0; + enableSpeed=0; + } + //*************************************************************************************************************** + // cycle logic control section + //*************************************************************************************************************** + if (enableSimula==1){if(simOk==0){tractorSpeed_MtS_timed=0.0f;}} + if ((tractorSpeed_MtS_timed>0.01f)){ + cycleStopRequest=1; + // calcola il tempo teorico di passaggio becchi sulla base della velocità del trattore + 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) + if (speedFromPick==1) { + tempoTraBecchi_mS = (tempoGiroSD / pickNumber)*1000.0f; // tempo tra due impulsi dai becchi in millisecondi ( circa 157mS a 4,5Km/h) + }else{ + tempoTraBecchi_mS = (tempoGiroSD / 25.0f)*1000.0f; // tempo tra due impulsi dai becchi in millisecondi ( circa 157mS a 4,5Km/h) + } + //******************************************* + // segue calcolo duty cycle comando motore DC per allinearsi con la velocità del trattore + double dutyTeorico = 0.00; + if ((tractorSpeed_MtS_timed>0.0)&&(tractorSpeed_MtS_timed<tabSpeed[0])){dutyTeorico = tabComan[0];} + for (int ii = 0;ii<16;ii++){ + if ((tractorSpeed_MtS_timed>=tabSpeed[ii])&&(tractorSpeed_MtS_timed<tabSpeed[ii+1])){ + dutyTeorico = tabComan[ii+1]; + } + } + if (tractorSpeed_MtS_timed > tabSpeed[16]){dutyTeorico=tractorSpeed_MtS_timed/1.25f;} + if ((pulseRised1==1)&&(enableSpeed>3)&&(pulseRised2==1)){ + double erroreTempo = 0.0f; + if(speedFromPick==1){ + erroreTempo = (double)timeIntraPick - tempoTraBecchi_mS; // errore tra il tempo previsto ed il tempo reale ( >0 se sto andando più piano del previsto) + }else{ + erroreTempo = (double)memoTimeHole - tempoTraBecchi_mS; // errore tra il tempo previsto ed il tempo reale ( >0 se sto andando più piano del previsto) + } + double errorePercentuale = erroreTempo / tempoTraBecchi_mS; + double k1=errorePercentuale*0.065f;double k3=0.0f;double k4=0.0f;double k5=0.0f; + if (tractorSpeed_MtS_timed <= minSeedSpeed){k3=0.105;k4=0.207;k5=0.310; + }else{k3=0.1075f;k4=0.211f;k5=0.315f;} + if (errorePercentuale > 25.0f){k1=errorePercentuale*k5;} + if ((errorePercentuale >= 10.0f)&&(errorePercentuale<=25.0f)){k1=errorePercentuale*k4;} + if (errorePercentuale < 10.0f){k1=errorePercentuale*k3;} + double memoCorrezione = (dutyTeorico *k1); + correzione = correzione + memoCorrezione; + pulseRised1=0; + pulseRised2=0; + } + // introduce il controllo di corrente + if (incrementCurrent){ + boostDcOut +=0.005f; + } + if (reduceCurrent){ + boostDcOut -=0.005f; + } + if (boostDcOut >= 0.2f){ + boostDcOut=0.2f; + all_genericals=1; + } + if (boostDcOut <=-0.2f){ + boostDcOut=-0.2f; + all_genericals=1; + } + correzione += boostDcOut; + #if defined(pcSerial) + #if defined(Qnca) + pc.printf(" DUTY PRE FINALE: %f",dcActualDuty); + #endif + #endif + // prova a mantenere il quinconce quando attivo + #if defined(speedMaster) + //dcActualDuty=dcActualDuty; + DC_brake=0; + DC_forward=1; + DC_prepare(); + #else + #if defined(mezzo) + if (quincEnable==1){ + if (quinconceActive==1){ + quinCalc(2.0f); + }else{ + quinCalc(1.0f); + } + }else{ + DC_brake=0; + DC_forward=1; + DC_prepare(); + } + #else + quinCalc(1.0f); + DC_brake=0; + DC_forward=1; + DC_prepare(); + #endif + #endif + + // il semiperiodoreale è calcolato sulla lettura del passaggio becchi reale + if (correggiSD == true){ + seedWheelPeriod = semiPeriodoReale / abs(percento); + }else{ + seedWheelPeriod = semiPeriodoReale; + } + if (seedWheelPeriod < 180.0f){seedWheelPeriod = 180.0f;} + if ((oldSeedWheelPeriod!=seedWheelPeriod)&&(seedWheelPeriod >=180.0f )){ + SDticker.attach_us(&step_SDPulseOut,seedWheelPeriod); // clock time are milliseconds and attach seed motor stepper controls + oldSeedWheelPeriod=seedWheelPeriod; + } + + //if (correzione > (dutyTeorico*0.20)){correzione = dutyTeorico*0.20;} + if (correzione > 0.30){correzione = 0.30;} + if (correzione < -0.30){correzione = -0.30;} + if((enableSDcontrol==1)){ + if (correzioneAttiva==1){ + dcActualDuty = dutyTeorico + correzione; + if (dcActualDuty<dcStarting){dcActualDuty=dcStarting;} + } + }else{ + dcActualDuty = dutyTeorico; + } + if (dcActualDuty <0.0f){dcActualDuty=0.0f;} + if (dcActualDuty > 1.0f){dcActualDuty = dcMaxSpeed;} + #if defined(pcSerial) + #if defined(Qnca) + pc.printf(" DUTY FINALE: %f \n",dcActualDuty); + #endif + #endif + SDmotorPWM.write(dcActualDuty); + // allarme + if (SDwheelTimer.read_ms()>4000){ + if (firstStart==0){ + all_noDcRotati=1; + } + #if defined(pcSerial) + #if defined(VediAllarmi) + pc.printf("allarme no DC rotation"); + #endif + #endif + + } + + //*************************************************************************************************************** + // CONTROLLA TAMBURO + //*************************************************************************************************************** + if(lowSpeed==0){ + if (syncroCheck==1){ + syncroCheck=0; + lockStart=1; + periodo = TBperiod; + if (aspettaStart==0){cambiaTB(periodo);} + } + // controllo di stop + double memoIntraP = (double)memoIntraPick*1.8f; + if ((double)rotationTimeOut.read_ms()> (memoIntraP)){ + syncroCheck=0; + aspettaStart=1; + countCicli=0; + if (TBzeroCyclePulse==1){TBticker.detach();} + } + }else{ // fine ciclo fuori da low speed + syncroCheck=0; + lockStart=0; + if (beccoPronto==1){ + if (tamburoStandard==1){ + double ritardoMassimo = 0.0f; + if(speedFromPick==1){ + ritardoMassimo = (double)timeIntraPick; + }else{ + ritardoMassimo = (double)memoTimeHole; + } + int tempoDiSincro = (int)((double)(ritardoMassimo - ((tempoBecco/2.0f)+((speedOfSeedWheel/1.25f)*ritardoMassimo)))); // + if (tempoDiSincro <= 1){tempoDiSincro=1;} + if ((sincroTimer.read_ms()>= tempoDiSincro)){ + if (tractorSpeed_MtS_timed >= minWorkSpeed){startCicloTB=1;} + beccoPronto=0; + } + }else{ + // tamburo per zucca + if (speedOfSeedWheel >= minWorkSpeed){startCicloTB=1;} + beccoPronto=0; + } + } + ciclaTB(); + } + //************************************************************* + }else{ // fine ciclo con velocita maggiore di 0 + SDwheelTimer.stop(); + SDwheelTimer.reset(); + checkSDrotation=0; + oldFaseLavoro=0; + aspettaStart=1; + countCicli=0; + startCycle=0; + oldSeedWheelPeriod=0.0f; + oldPeriodoTB=0.0f; + correzione=0.0f; + OLDpulseSpeedInterval=1000.01f; + cicloTbinCorso=0; + cntTbError=0; + if (cycleStopRequest==1){ + zeroDelay.reset(); + zeroDelay.start(); + runRequestBuf=0; + zeroRequestBuf=1; + cycleStopRequest=0; + SDzeroCyclePulse=0; + TBzeroCyclePulse=0; + zeroCycleEnd=0; + zeroCycle=1; + zeroTrigger=0; + } + } + //************************************************************************************************* + // ciclo di azzeramento motori + if ((zeroCycleEnd==0)&&(zeroCycle==1)){//&&(zeroDelay.read_ms()>10000)){ + if (zeroTrigger==0){ + TBticker.attach_us(&step_TBPulseOut,1000.0f); // clock time are seconds and attach seed motor stepper controls + DC_forward=1; + DC_brake=1; + DC_prepare(); + frenata=0; + zeroTrigger=1; + ritardoStop.reset(); + ritardoComandoStop.reset(); + ritardoComandoStop.start(); + timeOutZeroSD.start(); + quincTimeSet.reset(); + } + int tempoFrenata=300; + if (((ritardoStop.read_ms()>tempoFrenata)&&(SDzeroDebounced==0))||(accensione==1)||(ritardoComandoStop.read_ms()>400)){ + accensione=0; + avvicinamento=1; + avvicinamentoOn.reset(); + avvicinamentoOn.start(); + SDmotorPWM.write(0.32f); // duty cycle = 60% of power + DC_forward=1; + DC_brake=0; + DC_prepare(); + ritardoComandoStop.reset(); + ritardoComandoStop.stop(); + } + if (avvicinamento==1){ + if(avvicinamentoOn.read_ms()>300){ + SDmotorPWM.write(0.7f); + avvicinamentoOn.reset(); + avvicinamentoOff.reset(); + avvicinamentoOff.start(); + } + if(avvicinamentoOff.read_ms()>100){ + SDmotorPWM.write(0.32f); + avvicinamentoOff.reset(); + avvicinamentoOff.stop(); + avvicinamentoOn.start(); + } + }else{ + avvicinamentoOn.stop(); + avvicinamentoOff.stop(); + avvicinamentoOn.reset(); + avvicinamentoOff.reset(); + } + if (frenata==0){ + if (SDzeroCyclePulse==1){ + SDticker.detach(); + frenata=1; + quincTimeSet.reset(); + quincTimeSet.start(); + ritardoStop.start(); + //ritardoComandoStop.reset(); + //ritardoComandoStop.stop(); + } + }else{ + if (quinconceActive==0){ + if (SDzeroCyclePulse==1){ + avvicinamento=0; + DC_brake=1; + DC_prepare(); + SDzeroed=1; + ritardoStop.reset(); + ritardoStop.stop(); + } + }else{ + if (quincTimeSet.read_ms()>500){ + avvicinamento=0; + DC_brake=1; + DC_prepare(); + SDzeroed=1; + ritardoStop.reset(); + ritardoStop.stop(); + quincTimeSet.stop(); + } + } + } + // azzera tutto in time out + if (timeOutZeroSD.read_ms()>=10000){ + #if defined(pcSerial) + #if defined(canDataReceived) + pc.printf("allarme no zero"); + #endif + #endif + if (firstStart==0){ + all_no_Zeroing=1; + } + avvicinamento=0; + DC_brake=1; + DC_prepare(); + SDzeroed=1; + ritardoStop.reset(); + ritardoStop.stop(); + avvicinamentoOn.stop(); + avvicinamentoOff.stop(); + avvicinamentoOn.reset(); + avvicinamentoOff.reset(); + ritardoComandoStop.reset(); + ritardoComandoStop.stop(); + timeOutZeroSD.stop(); + timeOutZeroSD.reset(); + } + if (TBzeroCyclePulse==1){ + TBticker.detach(); + TBzeroed=1; + } + if ((SDzeroed==1)&&(TBzeroed==1)){ + avvicinamentoOn.stop(); + avvicinamentoOff.stop(); + ritardoComandoStop.stop(); + ritardoStop.stop(); + zeroCycleEnd=1; + zeroCycle=0; + zeroTrigger=0; + runRequestBuf=1; + zeroRequestBuf=0; + cycleStopRequest=0; + SDzeroed=0; + TBzeroed=0; + timeOutZeroSD.stop(); + timeOutZeroSD.reset(); + } + } + +//************************************************************************************************* + if (enableCycle==0){ + zeroTrigger=0; + SDmotorPWM=0; + SDmotorInA=0; + SDmotorInB=0; + } + SDzeroCyclePulse=0; + TBzeroCyclePulse=0; +//************************************************************************************************* + }else{//end ciclo normale +//************************************************************************************************* +// task di prova della scheda +//************************************************************************************************* + #if defined(provaScheda) + clocca++; + //led = !led; + //txMsg.clear(); + //txMsg << clocca; + //test.printf("aogs \n"); + //if(can1.write(txMsg)){ + #if defined(pcSerial) + pc.printf("Can write OK \n"); + #endif + //} + switch (clocca){ + case 1: + TBmotorStepOut=1; // define step command for up down motor driver + break; + case 2: + SDmotorPWM=1; // define step command for seeding whell motor driver + break; + case 3: + speedClock=1; // define input of + break; + case 4: + break; + case 5: + SDmotorInA=1; + SDmotorInB=0; + break; + case 6: + break; + case 7: + break; + case 8: + break; + case 9: + break; + case 10: + break; + case 11: + break; + case 12: + break; + case 13: + break; + case 14: + SDmotorPWM=1; // power mosfet 2 command out + break; + case 15: + break; + case 16: + case 17: + break; + case 18: + TBmotorStepOut=0; // define step command for up down motor driver + SDmotorPWM=0; // define step command for seeding whell motor driver + speedClock=0; // define input of + SDmotorInA=0; + SDmotorInB=0; + SDmotorPWM=0; // power mosfet 2 command out + break; + default: + clocca=0; + break; + } + wait_ms(100); + #endif // end prova scheda + + #if defined(provaDC) + int rampa=1000; + int pausa=3000; + switch (clocca){ + case 0: + DC_brake=0; + DC_forward=1; + duty_DC+=0.01f; + if (duty_DC>=0.2f){ + duty_DC=0.2f; + clocca=1; + } + wait_ms(rampa); + break; + case 1: + wait_ms(pausa*4); + clocca=2; + break; + case 2: + DC_brake=0; + DC_forward=1; + duty_DC-=0.01f; + if (duty_DC<=0.0f){ + duty_DC=0.0f; + clocca=3; + } + wait_ms(rampa); + break; + case 3: + wait_ms(pausa); + clocca=4; + break; + case 4: + DC_brake=0; + DC_forward=1; + duty_DC+=0.01f; + if (duty_DC>=1.0f){ + duty_DC=1.0f; + clocca=5; + } + wait_ms(rampa); + break; + case 5: + wait_ms(pausa); + clocca=6; + break; + case 6: + DC_brake=0; + DC_forward=1; + duty_DC-=0.01f; + if (duty_DC<=0.0f){ + duty_DC=0.0f; + clocca=7; + } + wait_ms(rampa); + break; + case 7: + wait_ms(pausa); + clocca=0; + break; + default: + break; + } + if (oldDuty_DC != duty_DC){ + SDmotorPWM.write(duty_DC); // duty cycle = stop + oldDuty_DC=duty_DC; + DC_prepare(); + } + #endif + }//end in prova + wd.Service(); // kick the dog before the timeout + } // end while +}// end main +