enum DATASTORE_INIT DataStoreInit(void)
{
// First attempt to initialize the EEPROM unit using Microchip's library.
if (DataEEInit() != 0) {
return DATASTORE_INIT_FAIL;
}
// Then attempt to load the onboard parameters. This can fail if:
// a) There was a problem with the EEPROM OR
// b) There are no saved onboard parameters.
// In that case we save the current values and load them again and make sure everything worked.
// And load all stored parameters in the EEPROM. If this errors out, assume its because the
// EEPROM is currently empty (like if the PIC was just flashed). So write the current parameters
// and try reading them again and only error out if either of those fail.
if (!DataStoreLoadParameters()) {
if (DataStoreSaveParameters()) {
if (!DataStoreLoadParameters()) {
return DATASTORE_INIT_FAIL;
} else {
return DATASTORE_INIT_PRELOADED;
}
} else {
return DATASTORE_INIT_FAIL;
}
}
return DATASTORE_INIT_SUCCESS;
}
unsigned int Get_ID_Code(void)
{
int err=0;
if(dee_init==0)
{
err=DataEEInit();
dee_init++;
#ifdef __DEBUG
//printf("Error for Init: %d\r\n",err);
#endif
}
code= DataEERead(MEM_LOCATION_ID);
return code;
}
unsigned char EEPInit(void){
unsigned char eepInitMsg = 0;
// Initialize the EEPROM emulation and read the PID Data
eepInitMsg = DataEEInit();
if (eepInitMsg == 1){
mlActionAck.action = SLUGS_ACTION_EEPROM; // EEPROM Action
mlActionAck.result = EEP_PAGE_EXP; // Page Expired
} else if (eepInitMsg == 6){
mlActionAck.action = SLUGS_ACTION_EEPROM; // EEPROM Action
mlActionAck.result = EEP_MEMORY_CORR; // Memory Corrupted
}
return eepInitMsg;
}
/*******************************************************
* MAIN function, just setup some inits and loops
* "soft" real-time event handlers, defined hereafter
********************************************************/
int main(void)
{
// configuro l'oscillatore interno che mi fornisce Tcy
// Fosc = Fin (M/(N1*N2))
// FCY = Fosc/2
PLLFBD = 39; // M = 40
CLKDIVbits.PLLPOST=0; // N2 = 2
CLKDIVbits.PLLPRE=0; // N1 = 2
RCONbits.SWDTEN = 0; //disabilito il watchdog
DataEEInit();
//Init Peripheral Pin Selection (QEI and UART)
PPS_Init();
control_flags.first_scan = 1;
slow_ticks_limit = SLOW_RATE * (FCY_PWM / 1000) - 1 ;
medium_ticks_limit = MEDIUM_RATE * (FCY_PWM / 1000) - 1;
mposition1 = zero_pos1;//parto dalla posizione iniziale 90 90 90
mposition2 = zero_pos2;
mposition3 = zero_pos3;
/*mtheta1 = 0;
mtheta2 = 0;
mtheta3 = 0;
x_cart = 0;
y_cart = 0;
z_cart = 0;*/
coordinates_actual.x = 0;
coordinates_actual.y = 0;
coordinates_actual.z = 0;
coordinates_temp.x = 0;
coordinates_temp.y = 0;
coordinates_temp.z = 0;
angleJoints_actual.theta1 = 0;
angleJoints_actual.theta2 = 0;
angleJoints_actual.theta3 = 0;
angleJoints_temp.theta1 = 0;
angleJoints_temp.theta2 = 0;
angleJoints_temp.theta3 = 0;
update_params();
direction_flags_prev = direction_flags.word;
// UARTs init
// no need to set TRISx, they are "Module controlled"
UART1_Init();
// Setup control pins and PWM module,
// which is needed also to schedule "soft"
// real-time tasks w/PWM interrupt tick counts
DIR1 = direction_flags.motor1_dir;//0;
DIR2 = direction_flags.motor2_dir;//1;
DIR3 = direction_flags.motor3_dir;
//BRAKE1 = 0;
//BRAKE2 = 0;
DIR1_TRIS = OUTPUT;
DIR2_TRIS = OUTPUT;
DIR3_TRIS = OUTPUT;
//BRAKE1_TRIS = OUTPUT;
//BRAKE2_TRIS = OUTPUT;
CURRSENSE1_TRIS = INPUT;
CURRSENSE2_TRIS = INPUT;
CURRSENSE3_TRIS = INPUT;
PWM_Init();
// MUST SETUP ALSO ANALOG PINS AS INPUTS
AN0_TRIS = INPUT;
AN1_TRIS = INPUT;
AN2_TRIS = INPUT;
ADC_Init();
DMA0_Init();
// SETUP ENCODER INPUTS
// QEI inputs are "module controlled"
// -> no need to set TRISx
QEI1_Init();
QEI2_Init();
// Timers used to acquire Encoder 3
// corresponding PINS set as inputs
T1CK_TRIS = INPUT;
T4CK_TRIS = INPUT;
Timer1_Init();
Timer2_Init();
Timer4_Init();
// Timer5 used to schedule POSITION loops
Timer5_Init();
//Input capture
IC1_Init();
IC2_Init();
// TEST PIN
TEST_PIN_TRIS = OUTPUT;
TEST_PIN = FALSE;
while(1)//a ciclo infinito ripeto queste 2 routine
{
medium_event_handler();
slow_event_handler();
}
return 0; //code should never get here
}// END MAIN()
void InitializationEEPROM(void)
{ DataEEInit();
dataEEFlags.val = 0;
RestoreEEPROMData(); // Carico tutti i dati salvati nell'EEPROM ed eventuali dati di default
}
