diff options
Diffstat (limited to 'Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c')
| -rw-r--r-- | Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c | 64 |
1 files changed, 55 insertions, 9 deletions
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c index bbb3e4040..1920db03a 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c @@ -151,6 +151,39 @@ uint32_t MotorPidRequestMessage(HardwarePidControl* request) 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"; @@ -159,31 +192,37 @@ uint32_t StoreDancerConfigMessage() { uint32_t status = OK; // FRESULT Fresult = FR_OK; - HardwareConfiguration DancerConfig; + //HardwareConfiguration DancerConfig; HardwareDancer Dancers[MAX_SYSTEM_DANCERS]; // uint8_t* response_buffer; // size_t response_size = 0; int Dancer_i; - hardware_configuration__init(&DancerConfig); - DancerConfig.dancers = (HardwareDancer**)my_malloc(sizeof(HardwareDancer*)*MAX_SYSTEM_DANCERS); + //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]; + //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); - DancerConfig.n_dancers++; - DancersCfg[Dancer_i].zeropoint = DancerConfig.dancers[Dancer_i]->zeropoint; + 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); } 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); -/* +#ifdef FOUR_WINDERS + status |= MCU_E2PromProgram(EEPROM_STORAGE_DANCER_3,DancersCfg[HARDWARE_DANCER_3].zeropoint); + status |= MCU_E2PromProgram(EEPROM_STORAGE_DANCER_4,DancersCfg[HARDWARE_DANCER_4].zeropoint); + Report("~~~~~~Store eeprom 1",__FILE__,__LINE__,(int)DancersCfg[HARDWARE_DANCER_3].zeropoint,RpWarning,(int)DancersCfg[HARDWARE_DANCER_4].zeropoint,0); +#endif + /* response_buffer = my_malloc(hardware_configuration__get_packed_size(&DancerConfig)); if (response_buffer) { @@ -208,11 +247,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; |