From 6235c6ff3684b836bf729bed6db04ddba597b7b0 Mon Sep 17 00:00:00 2001 From: Shlomo Hecht Date: Tue, 9 Oct 2018 16:41:01 +0300 Subject: update the speed sensor and change the sampalling to 100msec --- .../Embedded/Modules/Control/MillisecTask.c | 54 ++++++++++++++-------- 1 file changed, 35 insertions(+), 19 deletions(-) (limited to 'Software/Embedded_SW/Embedded/Modules/Control') diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c index 9b4f945d8..1c4e6d25e 100644 --- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c +++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c @@ -78,6 +78,8 @@ uint32_t DrawerFansStatus = 0; bool watchdogCriticalAlarm = false; +uint32_t msec_millisecondCounter = 0; + MillisecMotorDataStruc ScrewSetMaxSpeedPending = {0}; MillisecMotorDataStruc ScrewMovePending = {0}; MillisecMotorDataStruc MotorData[NUM_OF_MOTORS] = {0}; @@ -97,6 +99,7 @@ uint32_t Control_Delta_Position_Pass(uint32_t Current_Read,uint32_t Previous_Rea //********************************************************************** uint32_t MSBacklog[200]={0}; uint8_t Motor_Id[200]={0}; +uint32_t MSTick[200]={0}; uint16_t MsecLogindex = 0; void MillisecInit(void) @@ -148,7 +151,6 @@ void MillisecStart(void) ADCAcquireStart(0,1); } -uint32_t msec_millisecondCounter = 0; void OneMilliSecondMillisecInterrupt(UArg arg0) { MillisecMessageStruc Message; @@ -189,14 +191,18 @@ int32_t MillisecReadFromTempSensor(uint32_t TempSensorId, MSecFptr Callback) } //typedef uint32_t (* MSecFptr)(uint32_t deviceID, uint32_t ReadValue); uint32_t MotorActivity = 0; + int32_t MillisecMoveScrew(unsigned long Data, int Length, MSecFptr Callback) { //========================== MSBacklog[MsecLogindex]=Data; Motor_Id[MsecLogindex]=HARDWARE_MOTOR_TYPE__MOTO_SCREW; + MSTick[MsecLogindex]=msec_millisecondCounter; MsecLogindex++; if (MsecLogindex>=199) MsecLogindex = 0; + LOG_ERROR(Data, "MillisecMoveScrew"); + //========================== ScrewMovePending.Callback = Callback; @@ -217,10 +223,12 @@ int32_t MillisecSetScrewSpeed(unsigned long Data, int Length, MSecFptr Callback) //========================== MSBacklog[MsecLogindex]=Data; Motor_Id[MsecLogindex]=HARDWARE_MOTOR_TYPE__MOTO_SCREW; + MSTick[MsecLogindex]=msec_millisecondCounter; MsecLogindex++; if (MsecLogindex>=199) MsecLogindex = 0; //========================== + LOG_ERROR(Data, "MillisecSetScrewSpeed"); ScrewSetMaxSpeedPending.Callback = Callback; ScrewSetMaxSpeedPending.Data = Data; @@ -239,6 +247,7 @@ int32_t MillisecSetMotorSpeed(TimerMotors_t MotorId, unsigned long Data, int Len if (MotorId >= NUM_OF_MOTORS) return -1; //========================== MSBacklog[MsecLogindex]=Data; + MSTick[MsecLogindex]=msec_millisecondCounter; Motor_Id[MsecLogindex]=MotorId; MsecLogindex++; if (MsecLogindex>=199) @@ -274,6 +283,7 @@ int32_t MillisecWriteToMotor(TimerMotors_t MotorId, unsigned long Data, int Leng MillisecMotorDataStruc MotorData = {0}; //========================== MSBacklog[MsecLogindex]=Data; + MSTick[MsecLogindex]=msec_millisecondCounter; Motor_Id[MsecLogindex]=MotorId; MsecLogindex++; if (MsecLogindex>=199) @@ -296,6 +306,7 @@ int32_t MillisecReadFromMotor(TimerMotors_t MotorId, unsigned long Data, int Len //========================== MSBacklog[MsecLogindex]=Data; Motor_Id[MsecLogindex]=MotorId; + MSTick[MsecLogindex]=msec_millisecondCounter; MsecLogindex++; if (MsecLogindex>=199) MsecLogindex = 0; @@ -358,23 +369,28 @@ uint32_t MillisecLoop(uint32_t tick) MotorActivity--; } - if (ScrewSetMaxSpeedPending.Active == true) + if (Motor_i == HARDWARE_MOTOR_TYPE__MOTO_SCREW) { - MotorSendFPGARequest(HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSetMaxSpeedPending.Data,ScrewSetMaxSpeedPending.Length); - MotorActivity--; - ScrewSetMaxSpeedPending.Active = false; - if (ScrewSetMaxSpeedPending.Callback) - ScrewSetMaxSpeedPending.Callback(Motor_i,0); + if (ScrewSetMaxSpeedPending.Active == true) + { + MotorSendFPGARequest(HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSetMaxSpeedPending.Data,ScrewSetMaxSpeedPending.Length); + MotorActivity--; + ScrewSetMaxSpeedPending.Active = false; + if (ScrewSetMaxSpeedPending.Callback) + ScrewSetMaxSpeedPending.Callback(Motor_i,0); - } - else if (ScrewMovePending.Active == true) - { - MotorSendFPGARequest(HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewMovePending.Data,ScrewMovePending.Length); - MotorActivity--; - ScrewMovePending.Active = false; - if (ScrewMovePending.Callback) - ScrewMovePending.Callback(Motor_i,0); + } + else if (ScrewMovePending.Active == true) + { + MotorSendFPGARequest(HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewMovePending.Data,ScrewMovePending.Length); + MotorActivity--; + /*if (Screwindex>=50) + Screwindex = 0;*/ + ScrewMovePending.Active = false; + if (ScrewMovePending.Callback) + ScrewMovePending.Callback(Motor_i,0); + } } if (SpeedSetPending[Motor_i].Active == true) { @@ -439,13 +455,13 @@ uint32_t MillisecLoop(uint32_t tick) Dancer_Data[WINDER_DANCER] = Read_Dancer_Position(WINDER_DANCER); if (Ten_msTick) { - Speed_Data = Calculate_Speed_Sensor_Velocity(); + //Speed_Data = Calculate_Speed_Sensor_Velocity(); //MillisecReadFromTempSensor(Sensor_Read, NULL); //if (Sensor_Read++ >= MAX_TEMPERATURE_SENSOR_ID) Sensor_Read = 0; } if (Hundred_msTick) { -// Speed_Data = Calculate_Speed_Sensor_Velocity(); + Speed_Data = Calculate_Speed_Sensor_Velocity(); for (Sensor_i = 0;Sensor_i < MAX_TEMPERATURE_SENSOR_ID;Sensor_i++) { MillisecReadFromTempSensor(Sensor_i, NULL); @@ -475,13 +491,13 @@ uint32_t MillisecLoop(uint32_t tick) { Read_MidTank_Pressure_Sensor(Disp_i); } - for (Motor_i = 0;Motor_i < NUM_OF_MOTORS;Motor_i++) +/* for (Motor_i = 0;Motor_i < NUM_OF_MOTORS;Motor_i++) { if (Motor_i == HARDWARE_MOTOR_TYPE__MOTO_SCREW) continue; // if (isMotorConfigured(Motor_i)) MotorGetStatusFromFPGA(Motor_i); - } + }*/ } //ROM_IntMasterEnable(); -- cgit v1.3.1