From 281610ac56799f6870c587a942495d91cd55b227 Mon Sep 17 00:00:00 2001 From: Shlomo Hecht Date: Wed, 30 Dec 2020 13:02:30 +0200 Subject: merge - fix --- .../Embedded/Modules/Thread/Thread_init.c | 119 +++++++++++++-------- 1 file changed, 77 insertions(+), 42 deletions(-) (limited to 'Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c') diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c index 416b53494..588b1b391 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c @@ -23,6 +23,7 @@ #include "drivers/Flash_ram/FlashProgram.h" #include "drivers/Flash_ram/MCU_E2Prom.h" +#include "modules/ids/ids_ex.h" #include "thread.h" MotorDriverConfigStruc MotorsCfg[NUM_OF_MOTORS]={0}; @@ -103,10 +104,14 @@ uint32_t MotorsConfigMessage(HardwareConfiguration * HWrequest) MotorsCfg[Motor_i].tfast = request->tfast ; MotorsCfg[Motor_i].tonmin = request->tonmin ; MotorsCfg[Motor_i].toffmin = request->toffmin ; + MotorsCfg[Motor_i].has_p01configword = (bool)(request->has_p01configword) ;//needed for backward compatibility + MotorsCfg[Motor_i].p01configword = request->p01configword ; status = MotorConfig( Motor_i, &MotorsCfg[Motor_i]); -// if (Motor_i == MOTOR_RDRIVING) -// ThreadInitialTestStub(request); + if ((Motor_i >= HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_1)&&(Motor_i <= HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_8)) + { + IDS_Dispenser_Init(Motor_i-HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_1); + } } return status; @@ -140,56 +145,79 @@ uint32_t MotorPidRequestMessage(HardwarePidControl* request) NormalizedErrorCoEfficient[Motor_i] = NormalizedErrorCoEfficient[Motor_i] / temp; // uint32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES]; temp = 1<<(DancersCfg[ThreadMotorIdToDancerId[Motor_i]].resolutionbits); - temp = (temp*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].maximalmovementmm*3/2); + temp = (temp*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].maximalmovementmm);//*3/2); + if (Motor_i == WINDER_MOTOR) + { + temp = temp*3/2; + } DancerStopActivityLimit[Motor_i] = temp/(2*PI*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].armlength); +#ifdef FOUR_WINDERS + if (Motor_i == WINDER_MOTOR) + { + int j,winderMotorId; + for (j=0;j<3;j++) + { + winderMotorId = j+WINDER_2_MOTOR; + memcpy (&MotorsControl[winderMotorId],request,sizeof(HardwarePidControl)); + if (MotorsControl[winderMotorId].pvinputfilterfactormode > MAX_CONTROL_SAMPLES) + MotorsControl[winderMotorId].pvinputfilterfactormode = MAX_CONTROL_SAMPLES; + #ifdef TEST_LONGER_PID_THREAD + MotorsControl[Motor_i].pvinputfilterfactormode = 10; //test longer control + #endif + for (i = 0;i < (int)MotorsControl[winderMotorId].pvinputfilterfactormode; i++) + { + MotorSamples[winderMotorId][i] = 0; //reset the samples value for control beginning + MotorSpeedSamples[winderMotorId][i] = 0; + } + NormalizedErrorCoEfficient[winderMotorId] = (2*PI*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].armlength); + temp = 1<<(DancersCfg[ThreadMotorIdToDancerId[Motor_i]].resolutionbits); + temp=(NORMAL_COEF_DIVIDER*(temp-1)*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].maximalmovementmm); + NormalizedErrorCoEfficient[winderMotorId] = NormalizedErrorCoEfficient[winderMotorId] / temp; + // uint32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES]; + temp = 1<<(DancersCfg[ThreadMotorIdToDancerId[Motor_i]].resolutionbits); + temp = (temp*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].maximalmovementmm);//*3/2); + if (Motor_i == WINDER_MOTOR) + { + temp = temp*3/2; + } + DancerStopActivityLimit[winderMotorId] = temp/(2*PI*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].armlength); + } + } +#endif return OK; } char DancerConfigPath[50] = "0://SysInfo//DancCfg.cfg"; -uint32_t StoreDancerConfigMessage() +uint32_t StoreDancerConfigMessage(int DancerId) { uint32_t status = OK; - /* FRESULT Fresult = FR_OK; - // HardwareConfiguration DancerConfig; - // HardwareDancer Dancers[MAX_SYSTEM_DANCERS]; - // uint8_t* response_buffer; + // FRESULT Fresult = FR_OK; + //HardwareConfiguration DancerConfig; + HardwareDancer Dancers[MAX_SYSTEM_DANCERS]; +#ifdef FOUR_WINDERS + uint8_t DancerAddress[MAX_SYSTEM_DANCERS] = {EEPROM_STORAGE_DANCER_0,EEPROM_STORAGE_DANCER_1,EEPROM_STORAGE_DANCER_2,EEPROM_STORAGE_DANCER_3,EEPROM_STORAGE_DANCER_4}; +#else + uint8_t DancerAddress[MAX_SYSTEM_DANCERS] = {EEPROM_STORAGE_DANCER_0,EEPROM_STORAGE_DANCER_1,EEPROM_STORAGE_DANCER_2}; +#endif // size_t response_size = 0; - // int Dancer_i; + int Dancer_i = DancerId; - hardware_configuration__init(&DancerConfig); - DancerConfig.dancers = (HardwareDancer**)my_malloc(sizeof(HardwareDancer*)*MAX_SYSTEM_DANCERS); - for (Dancer_i = 0; Dancer_i < MAX_SYSTEM_DANCERS; Dancer_i++) - { - hardware_dancer__init(&Dancers[Dancer_i]); - DancerConfig.dancers[Dancer_i] = &Dancers[Dancer_i]; - Dancers[Dancer_i].has_zeropoint = true; - Dancers[Dancer_i].hardwaredancertype = Dancer_i; - Dancers[Dancer_i].has_hardwaredancertype = true; - DancerConfig.dancers[Dancer_i]->zeropoint=Control_Read_Dancer_Position(Dancer_i,0,0); - DancerConfig.n_dancers++; - DancersCfg[Dancer_i].zeropoint = DancerConfig.dancers[Dancer_i]->zeropoint; - } - */ - status |= MCU_E2PromProgram(EEPROM_STORAGE_DANCER_0,DancersCfg[0].zeropoint); - status |= MCU_E2PromProgram(EEPROM_STORAGE_DANCER_1,DancersCfg[1].zeropoint); - status |= MCU_E2PromProgram(EEPROM_STORAGE_DANCER_2,DancersCfg[2].zeropoint); - Report("Store eeprom 0",__FILE__,DancersCfg[0].zeropoint,(int)DancersCfg[1].zeropoint,RpWarning,(int)DancersCfg[2].zeropoint,0); -/* - response_buffer = my_malloc(hardware_configuration__get_packed_size(&DancerConfig)); - if (response_buffer) - { - response_size = hardware_configuration__pack(&DancerConfig, response_buffer); - } - Fresult = FileWrite(response_buffer,response_size,DancerConfigPath,BIOS_WAIT_FOREVER); - EraseFlashSection(DANCERS_MAP_IN_FLASH,1024); - for (Dancer_i = 0; Dancer_i < MAX_SYSTEM_DANCERS; Dancer_i++) - { - ReadAppAndProgram(DANCERS_MAP_IN_FLASH, sizeof(Dancers), Dancers); - } + //hardware_configuration__init(&DancerConfig); + //DancerConfig.dancers = (HardwareDancer**)my_malloc(sizeof(HardwareDancer*)*MAX_SYSTEM_DANCERS); + hardware_dancer__init(&Dancers[Dancer_i]); + //DancerConfig.dancers[Dancer_i] = &Dancers[Dancer_i]; + Dancers[Dancer_i].has_zeropoint = true; + Dancers[Dancer_i].hardwaredancertype = Dancer_i; + Dancers[Dancer_i].has_hardwaredancertype = true; + Dancers[Dancer_i].zeropoint=Control_Read_Dancer_Position(Dancer_i,0); + //DancerConfig.n_dancers++; + DancersCfg[Dancer_i].zeropoint = Dancers[Dancer_i].zeropoint; + DancersCfg[Dancer_i].zeropoint = Control_Read_Dancer_Position(Dancer_i,0); - my_free(response_buffer); -*/ + + status |= MCU_E2PromProgram(DancerAddress[Dancer_i],DancersCfg[Dancer_i].zeropoint); + Report("~~~~~~Store eeprom dancer",__FILE__,__LINE__,Dancer_i,RpWarning,(int)DancersCfg[Dancer_i].zeropoint,0); return status; } uint32_t LoadDancerConfigMessage() @@ -199,11 +227,18 @@ uint32_t LoadDancerConfigMessage() MCU_E2PromRead(EEPROM_STORAGE_DANCER_0,&DancersCfg[0].zeropoint); MCU_E2PromRead(EEPROM_STORAGE_DANCER_1,&DancersCfg[1].zeropoint); MCU_E2PromRead(EEPROM_STORAGE_DANCER_2,&DancersCfg[2].zeropoint); - +#ifdef FOUR_WINDERS + MCU_E2PromRead(EEPROM_STORAGE_DANCER_3,&DancersCfg[3].zeropoint); + MCU_E2PromRead(EEPROM_STORAGE_DANCER_4,&DancersCfg[4].zeropoint); +#endif status |= Report("Dancer 0 Store data eeprom ",__FILE__,__LINE__,0,RpWarning,(int)DancersCfg[0].zeropoint,0); status |= Report("Dancer 1 Store data eeprom ",__FILE__,__LINE__,1,RpWarning,(int)DancersCfg[1].zeropoint,0); status |= Report("Dancer 2 Store data eeprom ",__FILE__,__LINE__,2,RpWarning,(int)DancersCfg[2].zeropoint,0); +#ifdef FOUR_WINDERS + status |= Report("Dancer 3 Store data eeprom ",__FILE__,__LINE__,3,RpWarning,(int)DancersCfg[3].zeropoint,0); + status |= Report("Dancer 4 Store data eeprom ",__FILE__,__LINE__,4,RpWarning,(int)DancersCfg[4].zeropoint,0); +#endif return status; -- cgit v1.3.1