uint8_t storeWaypointInEeprom (mavlink_waypoint_t* mlSingleWp){ uint8_t indexOffset = 0, indx= 0, writeSuccess = 0; tFloatToChar tempFloat; // get the WP index indx = (uint8_t)mlSingleWp->seq; // Compute the adecuate index offset indexOffset = indx*WP_SIZE_IN_EEPROM; // Save the data to the EEPROM tempFloat.flData = mlSingleWp->y; writeSuccess += DataEEWrite(tempFloat.shData[0], WPS_OFFSET+indexOffset); writeSuccess += DataEEWrite(tempFloat.shData[1], WPS_OFFSET+indexOffset+1); tempFloat.flData = mlSingleWp->x; writeSuccess += DataEEWrite(tempFloat.shData[0], WPS_OFFSET+indexOffset+2); writeSuccess += DataEEWrite(tempFloat.shData[1], WPS_OFFSET+indexOffset+3); tempFloat.flData = mlSingleWp->z; writeSuccess += DataEEWrite(tempFloat.shData[0], WPS_OFFSET+indexOffset+4); writeSuccess += DataEEWrite(tempFloat.shData[1], WPS_OFFSET+indexOffset+5); writeSuccess += DataEEWrite((unsigned short)mlSingleWp->command, WPS_OFFSET+indexOffset+6); writeSuccess += DataEEWrite((unsigned short)mlSingleWp->param3, WPS_OFFSET+indexOffset+7); return writeSuccess; }
uint8_t storeParameterInEeprom (float parameter, uint8_t pmIndex){ uint8_t indexOffset = 0, indx= 0, writeSuccess = 0; tFloatToChar tempFloat; // Save the data to the EEPROM indexOffset = pmIndex*2; tempFloat.flData = parameter; writeSuccess += DataEEWrite(tempFloat.shData[0], PARAM_OFFSET+indexOffset); writeSuccess += DataEEWrite(tempFloat.shData[1], PARAM_OFFSET+indexOffset+1); return writeSuccess; }
bool _Write4BytesIntoMemory(const uint8_t data[4], uint8_t *addr) { // And pack those into uint16s. uint16_t word1, word2; LEUnpackUint16(&word1, &data[0]); LEUnpackUint16(&word2, &data[2]); // And write into memory (DataEEWrite is 0 on success). if (DataEEWrite(word1, *addr)) { return false; } *addr += 1; if (DataEEWrite(word2, *addr)) { return false; } *addr += 1; // Return success. return true; }
char Increment_ID_Code(void) { if(dee_init) { int err=0; code++; err = DataEEWrite(code,MEM_LOCATION_ID); return err; } return 0xff; }
bool DataStoreSaveParameters(void) { // The address to use for this parameter. Stored as some data types are larger than a single // word. uint8_t offset = HAS_BEEN_WRITTEN_LOCATION + 1; int i; for (i = 0; i < PARAMETERS_TOTAL; ++i) { switch (onboardParameters[i].dataType) { case PARAMETERS_DATATYPE_UINT8: { // First grab the parameter data. uint8_t param; ParameterGetValueById(i, ¶m); // And write it into EEPROM (non-zero values mean failure). if (DataEEWrite(param, offset)) { return false; } // And increment the memory location for the next parameter. ++offset; } break; case PARAMETERS_DATATYPE_UINT16: { // First grab the parameter data. uint16_t param; ParameterGetValueById(i, ¶m); // And write it into EEPROM (non-zero values mean failure). if (DataEEWrite(param, offset)) { return false; } // And increment the memory location for the next parameter. ++offset; } break; case PARAMETERS_DATATYPE_UINT32: { // First grab the parameter data. uint32_t param; ParameterGetValueById(i, ¶m); // Now split it up into bytes. uint8_t tmp[4]; LEPackUint32(tmp, param); // And write it into EEPROM. if (!_Write4BytesIntoMemory(tmp, &offset)) { return false; } } break; case PARAMETERS_DATATYPE_INT32: { // First grab the parameter data. int32_t param; ParameterGetValueById(i, ¶m); // Now split it up into bytes. uint8_t tmp[4]; LEPackInt32(tmp, param); // And write it into EEPROM. if (!_Write4BytesIntoMemory(tmp, &offset)) { return false; } } break; case PARAMETERS_DATATYPE_REAL32: { // First grab the parameter data. float param; ParameterGetValueById(i, ¶m); // Now split it up into bytes. uint8_t tmp[4]; LEPackReal32(tmp, param); // And write it into EEPROM. if (!_Write4BytesIntoMemory(tmp, &offset)) { return false; } } break; default: return false; } } // Now try to update the metadata and fail out if that can't be updated. if (DataEEWrite(GOOD_DATA, HAS_BEEN_WRITTEN_LOCATION) == 0) { return true; } else { return false; } }
void RestoreEEPROMData(void) { /* ******************************************************************** */ /* Recupero tutti i dati salvati in EEPROM, li carico nelle rispettive */ /* variabili ed effettuo tutti i controlli di coerenza necessari */ /* */ /* ******************************************************************** */ /* * Tutte le funzioni del tipo: * ParametriEEPROM[EEPROM_MODBUS_STATUSBIT2] = DataEERead(EEPROM_MODBUS_STATUSBIT2); * possono essere sostituite con una unica funzione * for(i=0; i<NUMERO_PARAMETRI_EEPROM; i++) * { ParametriEEPROM[i] = DataEERead(i); * } * che esegue il caricamento di tutti i parametri, ma per ora per motivi di debug * trovo più comodo eseguire i singoli caricamenti. */ // La variabile modbus STATUSBIT2 è sotto EEPROM ParametriEEPROM[EEPROM_MODBUS_STATUSBIT2] = DataEERead(EEPROM_MODBUS_STATUSBIT2); if (ParametriEEPROM[EEPROM_MODBUS_STATUSBIT2] == 65535) { ParametriEEPROM[EEPROM_MODBUS_STATUSBIT2] = 0; ParametriEEPROM[EEPROM_MODBUS_STATUSBIT2] |= FLG_EEPROM_OUTPUT_DRIVER_ENABLE_POLARITY; // Di default l'enable è attivo a "1" DataEEWrite(ParametriEEPROM[EEPROM_MODBUS_STATUSBIT2], EEPROM_MODBUS_STATUSBIT2); } /* Ricarico l'ID modbus del dispositivo e ne verifico la coerenza altrimenti lo imposto a "1" */ ParametriEEPROM[EEPROM_MODBUS_ADDRESS_SLAVE] = DataEERead(EEPROM_MODBUS_ADDRESS_SLAVE); if ((ParametriEEPROM[EEPROM_MODBUS_ADDRESS_SLAVE] == 0) || (ParametriEEPROM[EEPROM_MODBUS_ADDRESS_SLAVE] >= 255)) { ParametriEEPROM[EEPROM_MODBUS_ADDRESS_SLAVE] = 1; // l'ID errato lo sovrascrivo anche in caso di EEPROM disabilitata per l'utente DataEEWrite(ParametriEEPROM[EEPROM_MODBUS_ADDRESS_SLAVE], EEPROM_MODBUS_ADDRESS_SLAVE); } /* Ricarico il valore del tempo di WatchDog sulla comunicazione. */ ParametriEEPROM[EEPROM_MODBUS_COMWATCHDOG_TIME] = DataEERead(EEPROM_MODBUS_COMWATCHDOG_TIME); if ((ParametriEEPROM[EEPROM_MODBUS_COMWATCHDOG_TIME] <= 100) || (ParametriEEPROM[EEPROM_MODBUS_COMWATCHDOG_TIME] >= 2000)) { ParametriEEPROM[EEPROM_MODBUS_COMWATCHDOG_TIME] = 500; DataEEWrite(ParametriEEPROM[EEPROM_MODBUS_COMWATCHDOG_TIME], EEPROM_MODBUS_COMWATCHDOG_TIME); } //Inizializzo il timer per il watchdog sulla comunicazione ComunicationWatchDogTimer = ParametriEEPROM[EEPROM_MODBUS_COMWATCHDOG_TIME]; /* **************************************************************************************************** */ /* Carico dall'EEPROM i parametri di conversione. */ /* Inizializzo sucessivamente tutti i parametri necessari alla conversione utilizzando dei parametri di */ /* default in caso di taratura non eseguita. */ /* **************************************************************************************************** */ ParametriEEPROM[TARAT_ZERO_RB_AN0] = DataEERead(TARAT_ZERO_RB_AN0); ParametriEEPROM[TARAT_ZERO_RB_AN1] = DataEERead(TARAT_ZERO_RB_AN1); ParametriEEPROM[TARAT_ZERO_RB_AN2] = DataEERead(TARAT_ZERO_RB_AN2); ParametriEEPROM[TARAT_ZERO_RB_AN3] = DataEERead(TARAT_ZERO_RB_AN3); ParametriEEPROM[TARAT_ZERO_RB_AN4] = DataEERead(TARAT_ZERO_RB_AN4); ParametriEEPROM[TARAT_CONV_RB_AN0] = DataEERead(TARAT_CONV_RB_AN0); ParametriEEPROM[TARAT_CONV_RB_AN1] = DataEERead(TARAT_CONV_RB_AN1); ParametriEEPROM[TARAT_CONV_RB_AN2] = DataEERead(TARAT_CONV_RB_AN2); ParametriEEPROM[TARAT_CONV_RB_AN3] = DataEERead(TARAT_CONV_RB_AN3); ParametriEEPROM[TARAT_CONV_RB_AN4] = DataEERead(TARAT_CONV_RB_AN4); ParametriEEPROM[TARAT_COSTCONV_AN0_LOW] = DataEERead(TARAT_COSTCONV_AN0_LOW); // CostanteTaraturaAN0.fval ParametriEEPROM[TARAT_COSTCONV_AN0_HIGH] = DataEERead(TARAT_COSTCONV_AN0_HIGH); ParametriEEPROM[TARAT_COSTCONV_AN1_LOW] = DataEERead(TARAT_COSTCONV_AN1_LOW); // CostanteTaraturaAN1.fval ParametriEEPROM[TARAT_COSTCONV_AN1_HIGH] = DataEERead(TARAT_COSTCONV_AN1_HIGH); ParametriEEPROM[TARAT_COSTCONV_AN2_LOW] = DataEERead(TARAT_COSTCONV_AN2_LOW); // CostanteTaraturaAN2.fval ParametriEEPROM[TARAT_COSTCONV_AN2_HIGH] = DataEERead(TARAT_COSTCONV_AN2_HIGH); ParametriEEPROM[TARAT_COSTCONV_AN3_LOW] = DataEERead(TARAT_COSTCONV_AN3_LOW); // CostanteTaraturaAN3.fval ParametriEEPROM[TARAT_COSTCONV_AN3_HIGH] = DataEERead(TARAT_COSTCONV_AN3_HIGH); ParametriEEPROM[TARAT_COSTCONV_AN4_LOW] = DataEERead(TARAT_COSTCONV_AN4_LOW); // CostanteTaraturaAN4.fval ParametriEEPROM[TARAT_COSTCONV_AN4_HIGH] = DataEERead(TARAT_COSTCONV_AN4_HIGH); if((ParametriEEPROM[TARAT_COSTCONV_AN0_LOW] == 0) && (ParametriEEPROM[TARAT_COSTCONV_AN0_HIGH] == 0)) { // Se non è stato tarato il canale analogico uso una impostazione di default per la conversione CostanteTaraturaAN0.fval = 1; } else { CostanteTaraturaAN0.low_part = ParametriEEPROM[TARAT_COSTCONV_AN0_LOW]; CostanteTaraturaAN0.high_part = ParametriEEPROM[TARAT_COSTCONV_AN0_HIGH]; } if((ParametriEEPROM[TARAT_COSTCONV_AN1_LOW] == 0) && (ParametriEEPROM[TARAT_COSTCONV_AN1_HIGH] == 0)) { // Se non è stato tarato il canale analogico uso una impostazione di default per la conversione CostanteTaraturaAN1.fval = 1; } else { CostanteTaraturaAN1.low_part = ParametriEEPROM[TARAT_COSTCONV_AN1_LOW]; CostanteTaraturaAN1.high_part = ParametriEEPROM[TARAT_COSTCONV_AN1_HIGH]; } if((ParametriEEPROM[TARAT_COSTCONV_AN2_LOW] == 0) && (ParametriEEPROM[TARAT_COSTCONV_AN2_HIGH] == 0)) { // Se non è stato tarato il canale analogico uso una impostazione di default per la conversione CostanteTaraturaAN2.fval = 1; } else { CostanteTaraturaAN2.low_part = ParametriEEPROM[TARAT_COSTCONV_AN2_LOW]; CostanteTaraturaAN2.high_part = ParametriEEPROM[TARAT_COSTCONV_AN2_HIGH]; } if((ParametriEEPROM[TARAT_COSTCONV_AN3_LOW] == 0) && (ParametriEEPROM[TARAT_COSTCONV_AN3_HIGH] == 0)) { // Se non è stato tarato il canale analogico uso una impostazione di default per la conversione CostanteTaraturaAN3.fval = 1; } else { CostanteTaraturaAN3.low_part = ParametriEEPROM[TARAT_COSTCONV_AN3_LOW]; CostanteTaraturaAN3.high_part = ParametriEEPROM[TARAT_COSTCONV_AN3_HIGH]; } if((ParametriEEPROM[TARAT_COSTCONV_AN4_LOW] == 0) && (ParametriEEPROM[TARAT_COSTCONV_AN4_HIGH] == 0)) { // Se non è stato tarato il canale analogico uso una impostazione di default per la conversione CostanteTaraturaAN4.fval = 1; } else { CostanteTaraturaAN4.low_part = ParametriEEPROM[TARAT_COSTCONV_AN4_LOW]; CostanteTaraturaAN4.high_part = ParametriEEPROM[TARAT_COSTCONV_AN4_HIGH]; } /* **************************************************************************************************** */ /* */ /* Carico dall'EEPROM i parametri di Configurazione del Robot */ /* */ /* */ /* */ /* **************************************************************************************************** */ ParametriEEPROM[EEPROM_MODBUS_ROBOT_DIMENSION_WEIGHT] = DataEERead(EEPROM_MODBUS_ROBOT_DIMENSION_WEIGHT); ParametriEEPROM[EEPROM_MODBUS_ROBOT_DIMENSION_WIDTH] = DataEERead(EEPROM_MODBUS_ROBOT_DIMENSION_WIDTH); ParametriEEPROM[EEPROM_MODBUS_ROBOT_DIMENSION_HEIGHT] = DataEERead(EEPROM_MODBUS_ROBOT_DIMENSION_HEIGHT); ParametriEEPROM[EEPROM_MODBUS_ROBOT_DIMENSION_LENGHT] = DataEERead(EEPROM_MODBUS_ROBOT_DIMENSION_LENGHT); ParametriEEPROM[EEPROM_MODBUS_ROBOT_DIMENSION_WHEELBASE] = DataEERead(EEPROM_MODBUS_ROBOT_DIMENSION_WHEELBASE); ParametriEEPROM[EEPROM_MODBUS_ROBOT_WHEEL_RADIUS_LEFT] = DataEERead(EEPROM_MODBUS_ROBOT_WHEEL_RADIUS_LEFT); // Raggio in Centesimi di mm della ruota ( 3,3Cm = 3300 Cent/mm) ParametriEEPROM[EEPROM_MODBUS_ROBOT_WHEEL_RADIUS_RIGHT] = DataEERead(EEPROM_MODBUS_ROBOT_WHEEL_RADIUS_RIGHT); // Raggio in Centesimi di mm della ruota ( 3,3Cm = 3300 Cent/mm) ParametriEEPROM[EEPROM_MODBUS_ROBOT_ENCODER_CPR_LEFT] = DataEERead(EEPROM_MODBUS_ROBOT_ENCODER_CPR_LEFT); // Numero di impulsi encoder per rivoluzione della ruota ( già moltiplicato per 4 ) ParametriEEPROM[EEPROM_MODBUS_ROBOT_ENCODER_CPR_RIGHT] = DataEERead(EEPROM_MODBUS_ROBOT_ENCODER_CPR_RIGHT); // Numero di impulsi encoder per rivoluzione della ruota ( già moltiplicato per 4 ) ParametriEEPROM[EEPROM_MODBUS_ROBOT_MOTOR_RPMMAX_LEFT] = DataEERead(EEPROM_MODBUS_ROBOT_MOTOR_RPMMAX_LEFT); ParametriEEPROM[EEPROM_MODBUS_ROBOT_MOTOR_RPMMAX_RIGHT] = DataEERead(EEPROM_MODBUS_ROBOT_MOTOR_RPMMAX_RIGHT); ParametriEEPROM[EEPROM_MODBUS_ROBOT_MOTOR_IMAX_LEFT] = DataEERead(EEPROM_MODBUS_ROBOT_MOTOR_IMAX_LEFT); ParametriEEPROM[EEPROM_MODBUS_ROBOT_MOTOR_IMAX_RIGHT] = DataEERead(EEPROM_MODBUS_ROBOT_MOTOR_IMAX_RIGHT); ParametriEEPROM[EEPROM_MODBUS_ROBOT_MOTOR_TORQUEMAX_LEFT] = DataEERead(EEPROM_MODBUS_ROBOT_MOTOR_TORQUEMAX_LEFT); ParametriEEPROM[EEPROM_MODBUS_ROBOT_MOTOR_TORQUEMAX_RIGHT] = DataEERead(EEPROM_MODBUS_ROBOT_MOTOR_TORQUEMAX_RIGHT); ParametriEEPROM[EEPROM_MODBUS_ROBOT_GEARBOX_RATIO_AXE_LEFT] = DataEERead(EEPROM_MODBUS_ROBOT_GEARBOX_RATIO_AXE_LEFT); ParametriEEPROM[EEPROM_MODBUS_ROBOT_GEARBOX_RATIO_AXE_RIGHT] = DataEERead(EEPROM_MODBUS_ROBOT_GEARBOX_RATIO_AXE_RIGHT); ParametriEEPROM[EEPROM_MODBUS_ROBOT_GEARBOX_RATIO_MOTOR_LEFT] = DataEERead(EEPROM_MODBUS_ROBOT_GEARBOX_RATIO_MOTOR_LEFT); ParametriEEPROM[EEPROM_MODBUS_ROBOT_GEARBOX_RATIO_MOTOR_RIGHT] = DataEERead(EEPROM_MODBUS_ROBOT_GEARBOX_RATIO_MOTOR_RIGHT); ParametriEEPROM[EEPROM_MODBUS_ROBOT_CHARGED_BATT] = DataEERead(EEPROM_MODBUS_ROBOT_CHARGED_BATT); ParametriEEPROM[EEPROM_MODBUS_ROBOT_DISCHARGED_BATT] = DataEERead(EEPROM_MODBUS_ROBOT_DISCHARGED_BATT); if (ParametriEEPROM[EEPROM_MODBUS_STATUSBIT2] == 65535) { ParametriEEPROM[EEPROM_MODBUS_STATUSBIT2] = 0; DataEEWrite(ParametriEEPROM[EEPROM_MODBUS_STATUSBIT2], EEPROM_MODBUS_STATUSBIT2); } //Impostazione default se valori sono quelli della cella cancellata ( = 65535 ) if(ParametriEEPROM[EEPROM_MODBUS_ROBOT_DIMENSION_WEIGHT] == 65535) { ParametriEEPROM[EEPROM_MODBUS_ROBOT_DIMENSION_WEIGHT] = 300; DataEEWrite(ParametriEEPROM[EEPROM_MODBUS_ROBOT_DIMENSION_WEIGHT], EEPROM_MODBUS_ROBOT_DIMENSION_WEIGHT); } if(ParametriEEPROM[EEPROM_MODBUS_ROBOT_DIMENSION_WIDTH] == 65535) { ParametriEEPROM[EEPROM_MODBUS_ROBOT_DIMENSION_WIDTH] = 200; DataEEWrite(ParametriEEPROM[EEPROM_MODBUS_ROBOT_DIMENSION_WIDTH], EEPROM_MODBUS_ROBOT_DIMENSION_WIDTH); } if(ParametriEEPROM[EEPROM_MODBUS_ROBOT_DIMENSION_HEIGHT] == 65535) { ParametriEEPROM[EEPROM_MODBUS_ROBOT_DIMENSION_HEIGHT] = 100; DataEEWrite(ParametriEEPROM[EEPROM_MODBUS_ROBOT_DIMENSION_HEIGHT], EEPROM_MODBUS_ROBOT_DIMENSION_HEIGHT); } if(ParametriEEPROM[EEPROM_MODBUS_ROBOT_DIMENSION_LENGHT] == 65535) { ParametriEEPROM[EEPROM_MODBUS_ROBOT_DIMENSION_LENGHT] = 200; DataEEWrite(ParametriEEPROM[EEPROM_MODBUS_ROBOT_DIMENSION_LENGHT], EEPROM_MODBUS_ROBOT_DIMENSION_LENGHT); } if(ParametriEEPROM[EEPROM_MODBUS_ROBOT_DIMENSION_WHEELBASE] == 65535) { ParametriEEPROM[EEPROM_MODBUS_ROBOT_DIMENSION_WHEELBASE] = 190; DataEEWrite(ParametriEEPROM[EEPROM_MODBUS_ROBOT_DIMENSION_WHEELBASE], EEPROM_MODBUS_ROBOT_DIMENSION_WHEELBASE); } if(ParametriEEPROM[EEPROM_MODBUS_ROBOT_WHEEL_RADIUS_LEFT] == 65535) { ParametriEEPROM[EEPROM_MODBUS_ROBOT_WHEEL_RADIUS_LEFT] = 3300; DataEEWrite(ParametriEEPROM[EEPROM_MODBUS_ROBOT_WHEEL_RADIUS_LEFT], EEPROM_MODBUS_ROBOT_WHEEL_RADIUS_LEFT); } if(ParametriEEPROM[EEPROM_MODBUS_ROBOT_WHEEL_RADIUS_RIGHT] == 65535) { ParametriEEPROM[EEPROM_MODBUS_ROBOT_WHEEL_RADIUS_RIGHT] = 3300; DataEEWrite(ParametriEEPROM[EEPROM_MODBUS_ROBOT_WHEEL_RADIUS_RIGHT], EEPROM_MODBUS_ROBOT_WHEEL_RADIUS_RIGHT); } if(ParametriEEPROM[EEPROM_MODBUS_ROBOT_ENCODER_CPR_LEFT] == 65535) { ParametriEEPROM[EEPROM_MODBUS_ROBOT_ENCODER_CPR_LEFT] = 400; //2048; DataEEWrite(ParametriEEPROM[EEPROM_MODBUS_ROBOT_ENCODER_CPR_LEFT], EEPROM_MODBUS_ROBOT_ENCODER_CPR_LEFT); } if(ParametriEEPROM[EEPROM_MODBUS_ROBOT_ENCODER_CPR_RIGHT] == 65535) { ParametriEEPROM[EEPROM_MODBUS_ROBOT_ENCODER_CPR_RIGHT] = 400; //2048; DataEEWrite(ParametriEEPROM[EEPROM_MODBUS_ROBOT_ENCODER_CPR_RIGHT], EEPROM_MODBUS_ROBOT_ENCODER_CPR_RIGHT); } if(ParametriEEPROM[EEPROM_MODBUS_ROBOT_MOTOR_RPMMAX_LEFT] == 65535) { ParametriEEPROM[EEPROM_MODBUS_ROBOT_MOTOR_RPMMAX_LEFT] = 6750; DataEEWrite(ParametriEEPROM[EEPROM_MODBUS_ROBOT_MOTOR_RPMMAX_LEFT], EEPROM_MODBUS_ROBOT_MOTOR_RPMMAX_LEFT); } if(ParametriEEPROM[EEPROM_MODBUS_ROBOT_MOTOR_RPMMAX_RIGHT] == 65535) { ParametriEEPROM[EEPROM_MODBUS_ROBOT_MOTOR_RPMMAX_RIGHT] = 6750; DataEEWrite(ParametriEEPROM[EEPROM_MODBUS_ROBOT_MOTOR_RPMMAX_RIGHT], EEPROM_MODBUS_ROBOT_MOTOR_RPMMAX_RIGHT); } if(ParametriEEPROM[EEPROM_MODBUS_ROBOT_MOTOR_IMAX_LEFT] == 65535) { ParametriEEPROM[EEPROM_MODBUS_ROBOT_MOTOR_IMAX_LEFT] = 580; DataEEWrite(ParametriEEPROM[EEPROM_MODBUS_ROBOT_MOTOR_IMAX_LEFT], EEPROM_MODBUS_ROBOT_MOTOR_IMAX_LEFT); } if(ParametriEEPROM[EEPROM_MODBUS_ROBOT_MOTOR_IMAX_RIGHT] == 65535) { ParametriEEPROM[EEPROM_MODBUS_ROBOT_MOTOR_IMAX_RIGHT] = 580; DataEEWrite(ParametriEEPROM[EEPROM_MODBUS_ROBOT_MOTOR_IMAX_RIGHT], EEPROM_MODBUS_ROBOT_MOTOR_IMAX_RIGHT); } if(ParametriEEPROM[EEPROM_MODBUS_ROBOT_MOTOR_TORQUEMAX_LEFT] == 65535) { ParametriEEPROM[EEPROM_MODBUS_ROBOT_MOTOR_TORQUEMAX_LEFT] = 18; DataEEWrite(ParametriEEPROM[EEPROM_MODBUS_ROBOT_MOTOR_TORQUEMAX_LEFT], EEPROM_MODBUS_ROBOT_MOTOR_TORQUEMAX_LEFT); } if(ParametriEEPROM[EEPROM_MODBUS_ROBOT_MOTOR_TORQUEMAX_RIGHT] == 65535) { ParametriEEPROM[EEPROM_MODBUS_ROBOT_MOTOR_TORQUEMAX_RIGHT] = 18; DataEEWrite(ParametriEEPROM[EEPROM_MODBUS_ROBOT_MOTOR_TORQUEMAX_RIGHT], EEPROM_MODBUS_ROBOT_MOTOR_TORQUEMAX_RIGHT); } if(ParametriEEPROM[EEPROM_MODBUS_ROBOT_GEARBOX_RATIO_AXE_LEFT] == 65535) { ParametriEEPROM[EEPROM_MODBUS_ROBOT_GEARBOX_RATIO_AXE_LEFT] = 1; DataEEWrite(ParametriEEPROM[EEPROM_MODBUS_ROBOT_GEARBOX_RATIO_AXE_LEFT], EEPROM_MODBUS_ROBOT_GEARBOX_RATIO_AXE_LEFT); } if(ParametriEEPROM[EEPROM_MODBUS_ROBOT_GEARBOX_RATIO_AXE_RIGHT] == 65535) { ParametriEEPROM[EEPROM_MODBUS_ROBOT_GEARBOX_RATIO_AXE_RIGHT] = 1; DataEEWrite(ParametriEEPROM[EEPROM_MODBUS_ROBOT_GEARBOX_RATIO_AXE_RIGHT], EEPROM_MODBUS_ROBOT_GEARBOX_RATIO_AXE_RIGHT); } if(ParametriEEPROM[EEPROM_MODBUS_ROBOT_GEARBOX_RATIO_MOTOR_LEFT] == 65535) { ParametriEEPROM[EEPROM_MODBUS_ROBOT_GEARBOX_RATIO_MOTOR_LEFT] = 43; DataEEWrite(ParametriEEPROM[EEPROM_MODBUS_ROBOT_GEARBOX_RATIO_MOTOR_LEFT], EEPROM_MODBUS_ROBOT_GEARBOX_RATIO_MOTOR_LEFT); } if(ParametriEEPROM[EEPROM_MODBUS_ROBOT_GEARBOX_RATIO_MOTOR_RIGHT] == 65535) { ParametriEEPROM[EEPROM_MODBUS_ROBOT_GEARBOX_RATIO_MOTOR_RIGHT] = 43; DataEEWrite(ParametriEEPROM[EEPROM_MODBUS_ROBOT_GEARBOX_RATIO_MOTOR_RIGHT], EEPROM_MODBUS_ROBOT_GEARBOX_RATIO_MOTOR_RIGHT); } if(ParametriEEPROM[EEPROM_MODBUS_ROBOT_CHARGED_BATT] == 65535) { ParametriEEPROM[EEPROM_MODBUS_ROBOT_CHARGED_BATT] = 1680; // 16,8V DataEEWrite(ParametriEEPROM[EEPROM_MODBUS_ROBOT_CHARGED_BATT], EEPROM_MODBUS_ROBOT_CHARGED_BATT); } if(ParametriEEPROM[EEPROM_MODBUS_ROBOT_DISCHARGED_BATT] == 65535) { ParametriEEPROM[EEPROM_MODBUS_ROBOT_DISCHARGED_BATT] = 1200; //12,0V DataEEWrite(ParametriEEPROM[EEPROM_MODBUS_ROBOT_DISCHARGED_BATT], EEPROM_MODBUS_ROBOT_DISCHARGED_BATT); } /* **************************************************************************************************** */ /* */ /* Carico dall'EEPROM i parametri di Configurazione del PID */ /* */ /* */ /* */ /* **************************************************************************************************** */ ParametriEEPROM[EEPROM_MODBUS_PID_P_LEFT] = DataEERead(EEPROM_MODBUS_PID_P_LEFT); ParametriEEPROM[EEPROM_MODBUS_PID_I_LEFT] = DataEERead(EEPROM_MODBUS_PID_I_LEFT); ParametriEEPROM[EEPROM_MODBUS_PID_D_LEFT] = DataEERead(EEPROM_MODBUS_PID_D_LEFT); ParametriEEPROM[EEPROM_MODBUS_PID_P_RIGHT] = DataEERead(EEPROM_MODBUS_PID_P_RIGHT); ParametriEEPROM[EEPROM_MODBUS_PID_I_RIGHT] = DataEERead(EEPROM_MODBUS_PID_I_RIGHT); ParametriEEPROM[EEPROM_MODBUS_PID_D_RIGHT] = DataEERead(EEPROM_MODBUS_PID_D_RIGHT); ParametriEEPROM[EEPROM_MODBUS_PID_RAMP_LEFT] = DataEERead(EEPROM_MODBUS_PID_RAMP_LEFT); ParametriEEPROM[EEPROM_MODBUS_PID_RAMP_RIGHT] = DataEERead(EEPROM_MODBUS_PID_RAMP_RIGHT); if(ParametriEEPROM[EEPROM_MODBUS_PID_P_LEFT] == 65535) { ParametriEEPROM[EEPROM_MODBUS_PID_P_LEFT] = 100; DataEEWrite(ParametriEEPROM[EEPROM_MODBUS_PID_P_LEFT], EEPROM_MODBUS_PID_P_LEFT); } if(ParametriEEPROM[EEPROM_MODBUS_PID_I_LEFT] == 65535) { ParametriEEPROM[EEPROM_MODBUS_PID_I_LEFT] = 0; DataEEWrite(ParametriEEPROM[EEPROM_MODBUS_PID_I_LEFT], EEPROM_MODBUS_PID_I_LEFT); } if(ParametriEEPROM[EEPROM_MODBUS_PID_D_LEFT] == 65535) { ParametriEEPROM[EEPROM_MODBUS_PID_D_LEFT] = 0; DataEEWrite(ParametriEEPROM[EEPROM_MODBUS_PID_D_LEFT], EEPROM_MODBUS_PID_D_LEFT); } if(ParametriEEPROM[EEPROM_MODBUS_PID_P_RIGHT] == 65535) { ParametriEEPROM[EEPROM_MODBUS_PID_P_RIGHT] = 100; DataEEWrite(ParametriEEPROM[EEPROM_MODBUS_PID_P_RIGHT], EEPROM_MODBUS_PID_P_RIGHT); } if(ParametriEEPROM[EEPROM_MODBUS_PID_I_RIGHT] == 65535) { ParametriEEPROM[EEPROM_MODBUS_PID_I_RIGHT] = 0; DataEEWrite(ParametriEEPROM[EEPROM_MODBUS_PID_I_RIGHT], EEPROM_MODBUS_PID_I_RIGHT); } if(ParametriEEPROM[EEPROM_MODBUS_PID_D_RIGHT] == 65535) { ParametriEEPROM[EEPROM_MODBUS_PID_D_RIGHT] = 0; DataEEWrite(ParametriEEPROM[EEPROM_MODBUS_PID_D_RIGHT], EEPROM_MODBUS_PID_D_RIGHT); } if(ParametriEEPROM[EEPROM_MODBUS_PID_RAMP_LEFT] == 65535) { ParametriEEPROM[EEPROM_MODBUS_PID_RAMP_LEFT] = 1; DataEEWrite(ParametriEEPROM[EEPROM_MODBUS_PID_RAMP_LEFT], EEPROM_MODBUS_PID_RAMP_LEFT); } if(ParametriEEPROM[EEPROM_MODBUS_PID_RAMP_RIGHT] == 65535) { ParametriEEPROM[EEPROM_MODBUS_PID_RAMP_RIGHT] = 1; DataEEWrite(ParametriEEPROM[EEPROM_MODBUS_PID_RAMP_RIGHT], EEPROM_MODBUS_PID_RAMP_RIGHT); } // Aggiorno le variabili PID toccate dai dati EEPROM PID1.Kp = ParametriEEPROM[EEPROM_MODBUS_PID_P_LEFT]; PID1.Ki = ParametriEEPROM[EEPROM_MODBUS_PID_I_LEFT]; PID1.Kd = ParametriEEPROM[EEPROM_MODBUS_PID_D_LEFT]; PID2.Kp = ParametriEEPROM[EEPROM_MODBUS_PID_P_LEFT]; PID2.Ki = ParametriEEPROM[EEPROM_MODBUS_PID_I_LEFT]; PID2.Kd = ParametriEEPROM[EEPROM_MODBUS_PID_D_LEFT]; PID1.RampaStep = ParametriEEPROM[EEPROM_MODBUS_PID_RAMP_LEFT]; PID2.RampaStep = ParametriEEPROM[EEPROM_MODBUS_PID_RAMP_RIGHT]; }