int main(int argc, char** argv)
{
unsigned int test;
Settings();
/* ******************************************************************************************************** */
/* ******************************************************************************************************** */
/* Inizializzazione da EEPROM */
/* Recupero i dati salvati in EEPROM ed eseguo un controllo di correttezza su di essi, questo serve */
/* principalmente per il default dopo la programmazione */
/* ******************************************************************************************************** */
/* ******************************************************************************************************** */
InitializationEEPROM();
/* ******************************************************************************************************** */
/* ******************************************************************************************************** */
/* Inizializzazione variabili */
/* */
/* ******************************************************************************************************** */
/* ******************************************************************************************************** */
LED1 = LED_OFF;
LED2 = LED_OFF;
MotorControlEnable(MOTORE1, MOTOR_DEACTIVE);
MotorControlEnable(MOTORE2, MOTOR_DEACTIVE);
DmaBuffer = 0;
VarModbus[INDICE_ENC1_TICK] = 0;
VarModbus[INDICE_ENC2_TICK] = 0;
VarModbus[INDICE_ENC1_PERIOD] = 0;
VarModbus[INDICE_ENC2_PERIOD] = 0;
VarModbus[INDICE_ENC1_SPEED] = 0;
VarModbus[INDICE_ENC2_SPEED] = 0;
//Motore1.L_WheelSpeed = 0;
//Motore2.L_WheelSpeed = 0;
VarModbus[INDICE_STATUSBIT1] = 0;
//VarModbus[INDICE_STATUSBIT1] &= ~(FLG_STATUSBI1_JOYMODE); // Al reset disattivo la modalità JoyStick
//VarModbus[INDICE_STATUSBIT1] &= ~(FLG_STATUSBI1_PID_EN); // Al reset disattivo la modalità PID
VarModbus[INDICE_STATUSBIT1] |= FLG_STATUSBI1_PID_EN; // Al reset attivo la modalità PID
OLD_INDICE_STATUSBIT1 = 0; // Se parto con il PID abilitato deve valere 0.
//OLD_INDICE_STATUSBIT1 = 1; // Se parto con il PWM abilitato deve valere 1.
VarModbus[INDICE_STATUSBIT1] |= FLG_STATUSBI1_COMWATCHDOG; // Al reset attivo il WatchDog sulla comunicazione
//VarModbus[INDICE_STATUSBIT1] &= ~(FLG_STATUSBI1_COMWATCHDOG); // Al reset disattivo il WatchDog sulla comunicazione
VarModbus[INDICE_STATUSBIT1] |= FLG_STATUSBI1_EEPROM_SAVE_EN; // Al reset attivo il Salvataggio automatico dei parametri in EEPROM
//VarModbus[INDICE_STATUSBIT1] &= ~(FLG_STATUSBI1_EEPROM_SAVE_EN); // Al reset disattivo il Salvataggio automatico dei parametri in EEPROM
VarModbus[INDICE_FLAG_TARATURA] = 0;
//VarModbus[INDICE_PWM_CH1] = MOTORE_STOP; // Motori fermi all'accensione
//VarModbus[INDICE_PWM_CH2] = MOTORE_STOP; // Motori fermi all'accensione
VarModbus[INDICE_PWM_CH1] = MOTORE_STOP_PID; // Motori fermi all'accensione
VarModbus[INDICE_PWM_CH2] = MOTORE_STOP_PID; // Motori fermi all'accensione
InitMotorStructure();
/* ******************************************************************************************************** */
/* ******************************************************************************************************** */
/* ******************************************************************************************************** */
/* Inizializzo i timer SW */
Timer1mSec.T_flag = FALSE;
Timer1mSec.T_initial_value = 1; // 1 = 1mSec
Timer1mSec.T_count_time = Timer1mSec.T_initial_value;
Timer10mSec.T_flag = FALSE;
Timer10mSec.T_initial_value = 10; // 10 = 10mSec
Timer10mSec.T_count_time = Timer10mSec.T_initial_value;
/* Inizializzazione porte seriali */
InizializzaSeriale(PORT_COM1);
InizializzaSeriale(PORT_COM2);
InitADC();
MotorControlEnable(MOTORE1, MOTOR_ACTIVE);
MotorControlEnable(MOTORE2, MOTOR_ACTIVE);
// TEST SEGNALAZIONI LED, sarà da eseguire dopo il controllo dei relativi FLAG...
SetLedErrorCode(&Led1Segnalazione, LED_ERRORCODE_05_CHECKERRORIOK, 1, SEGNALAZIONELED_TON, SEGNALAZIONELED_TOFF, SEGNALAZIONELED_TPAUSE);
SetLedErrorCode(&Led2Segnalazione, LED_POWERON_05_POWERTEST, 1, SEGNALAZIONELED_TON * 2, SEGNALAZIONELED_TOFF * 2, SEGNALAZIONELED_TPAUSE);
test = INTCON1;
ISR_Settings(); // Configures and enables ISRs
// /* DEBUG */
// SetpointRPM_M1 = Motore1.FL_Costante_Conversione_Vlin_to_Vang * ((float)((int)(100)));
// SetpointRPM_M2 = Motore2.FL_Costante_Conversione_Vlin_to_Vang * ((float)((int)(0)));
// if(!Motore1.UC_Fail) PID1.Setpoint = (long)(SetpointRPM_M1);
// if(!Motore2.UC_Fail) PID2.Setpoint = (long)(SetpointRPM_M2);
// /* ***** */
while (1)
{ // ---------------------- Gestione protocollo ModBus ---------------------- //
ModbusRoutine(PORT_COM1);
ModbusRoutine(PORT_COM2);
// ----------------- PID and speed calculation every 1ms ----------------- //
if (PID1_CALC_FLAG) Pid1(); // Ogni 1mSec ricalcola il prescaler, avvia un ciclo
if (PID2_CALC_FLAG) Pid2(); // di lettura dell'IC e esegue il PID sul dato prec.
// ---------------------- Task eseguito ogni 1mSec ---------------------- //
if (Timer1mSec.T_flag)
{
Timer1mSec.T_flag = FALSE;
GestioneWatchdog(); // GESTIONE WATCHDOG COMUNICAZIONE
GestioneSicurezzaMotore(); // Gestisco situazioni di FAIL dei motori
}
// ---------------------- Task eseguito ogni 10mSec ---------------------- //
if (Timer10mSec.T_flag)
{
Timer10mSec.T_flag = FALSE;
// GestioneWatchdog(); // GESTIONE WATCHDOG COMUNICAZIONE
// GestioneSicurezzaMotore(); // Gestisco situazioni di FAIL dei motori
GestioneLed1ErrorCode(&Led1Segnalazione);
GestioneLed2ErrorCode(&Led2Segnalazione);
}
GestioneAllarmi();
GestioneSetpoint();
AggiornaDatiVelocita();
AggiornaVariabiliModbus();
}
return (EXIT_SUCCESS);
}
int main(int argc, char** argv)
{
// Initialise le microcontroleur
Settings();
DelayN1ms(30);
// Initialise l'UART
InitTextIO();
// Initialise les interruptions
ISR_Settings();
initAsservissement();
initOdometrie();
// Configure tous les moteurs à l'arret
OC1RS = 0;
OC2RS = 0;
OC3RS = 0;
OC4RS = 0;
// Active les modules pour les PWM
OC1CONbits.OCM = 0b110;
OC2CONbits.OCM = 0b110;
OC3CONbits.OCM = 0b110;
OC4CONbits.OCM = 0b110;
uartNextOut = 0;
uartCommande = 0;
Cycle1 = 0;
Cycle2 = 0;
CYCLE1_FLAG = 0;
CYCLE2_FLAG = 0;
// Temporisation de 1 seconde
RtTimer = 10;
// Boucle Principale
while(1)
{
if(PID_CALC_FLAG)
{
Pid1();
Pid2();
compteurVitesse ++;
if(compteurVitesse == 10)
{
Vitesse[L] = VitesseCpteur[L]/400; // 400 = 10 échantillons x 40 pour le 1/40 mm/s => Le résultat est en mm/s
Vitesse[R] = VitesseCpteur[R]/400;
compteurVitesse = 0;
VitesseCpteur[L] = 0;
VitesseCpteur[R] = 0;
}
PID_CALC_FLAG = 0;
}
if(CYCLE1_FLAG)
{
Odometrie();
if(ordreEnCours != DEBUG)
{
Orientation();
}
CYCLE1_FLAG=0;
}
if(CYCLE2_FLAG)
{
Navigation();
CYCLE2_FLAG=0;
}
// blink LED
if (RtTimer <= 0)
{
RtTimer = 10;
LED_BLINK = !LED_BLINK;
}
if( uartCommande )
{
GererCommande();
}
}
return (EXIT_SUCCESS);
}
