Add functions to set the KVAL Hold/Run/Acc/Dec

4 files changed, 108 insertions, 0 deletions

diff --git a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.c b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.c
index ea5161b15..e245c4e15 100644
--- a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.c
+++ b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.c
@@ -931,8 +931,73 @@ void FPGA_SetMotStop(TimerMotors_t _motorId)
 //    FPGA_SPI_Transnit(_motorId);
 }
 
+void FPGA_SetMotKvalHold(TimerMotors_t _motorId)
+{
+    uint32_t temp;
+
+    temp = x_SET_PARAM | x_KVAL_HOLD;
+    temp = temp << 24;
+
+    if((MotorDriverResponse[_motorId].DriverType == CurrentMotDriver) && (MotorDriverRequest[_motorId].KvalHold > 0x7F))
+        MotorDriverRequest[_motorId].KvalHold = 0x7F;
+
+    temp |= MotorDriverRequest[_motorId].KvalHold<<16;
 
+    Fpga_Spi[_motorId].TX_MOSI = temp;
+    Fpga_Spi[_motorId].AMT_OF_Words = 4;
+    FPGA_SPI_Transnit(_motorId);
+}
 
+void FPGA_SetMotKvalRun(TimerMotors_t _motorId)
+{
+    uint32_t temp;
+
+    temp = x_SET_PARAM | x_KVAL_RUN;
+    temp = temp << 24;
+
+    if((MotorDriverResponse[_motorId].DriverType == CurrentMotDriver) && (MotorDriverRequest[_motorId].KvalRun > 0x7F))
+        MotorDriverRequest[_motorId].KvalRun = 0x7F;
+
+    temp |= MotorDriverRequest[_motorId].KvalRun<<16;
+
+    Fpga_Spi[_motorId].TX_MOSI = temp;
+    Fpga_Spi[_motorId].AMT_OF_Words = 4;
+    FPGA_SPI_Transnit(_motorId);
+}
+
+void FPGA_SetMotKvalAcc(TimerMotors_t _motorId)
+{
+    uint32_t temp;
+
+    temp = x_SET_PARAM | x_KVAL_ACC;
+    temp = temp << 24;
+
+    if((MotorDriverResponse[_motorId].DriverType == CurrentMotDriver) && (MotorDriverRequest[_motorId].KvalAcc > 0x7F))
+        MotorDriverRequest[_motorId].KvalAcc = 0x7F;
+
+    temp |= MotorDriverRequest[_motorId].KvalAcc<<16;
+
+    Fpga_Spi[_motorId].TX_MOSI = temp;
+    Fpga_Spi[_motorId].AMT_OF_Words = 4;
+    FPGA_SPI_Transnit(_motorId);
+}
+
+void FPGA_SetMotKvalDec(TimerMotors_t _motorId)
+{
+    uint32_t temp;
+
+    temp = x_SET_PARAM | x_KVAL_DEC;
+    temp = temp << 24;
+
+    if((MotorDriverResponse[_motorId].DriverType == CurrentMotDriver) && (MotorDriverRequest[_motorId].KvalDec > 0x7F))
+        MotorDriverRequest[_motorId].KvalDec = 0x7F;
+
+    temp |= MotorDriverRequest[_motorId].KvalDec<<16;
+
+    Fpga_Spi[_motorId].TX_MOSI = temp;
+    Fpga_Spi[_motorId].AMT_OF_Words = 4;
+    FPGA_SPI_Transnit(_motorId);
+}
 
 ///////////////////////////////////////////////////////////////////////////////
 /*
diff --git a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.h b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.h
index cc9d85cca..e2937e597 100644
--- a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.h
+++ b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.h
@@ -27,6 +27,11 @@ void FPGA_SetMotPosition(TimerMotors_t _motorId);
 void FPGA_SetMotSpeed(TimerMotors_t _motorId);
 void FPGA_SetMotSpeedDirect(TimerMotors_t _motorId);
 
+void FPGA_SetMotKvalHold(TimerMotors_t _motorId);
+void FPGA_SetMotKvalRun(TimerMotors_t _motorId);
+void FPGA_SetMotKvalAcc(TimerMotors_t _motorId);
+void FPGA_SetMotKvalDec(TimerMotors_t _motorId);
+
 void FPGA_SetMotorsInit();
 uint32_t FPGA_MotorConfig(TimerMotors_t _motorId, MotorDriverConfigStruc *MotorConfig);
 
diff --git a/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c
index b921a883c..0f460a7f7 100644
--- a/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c
+++ b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c
@@ -350,7 +350,36 @@ uint32_t MotorSetMicroStep(TimerMotors_t _motorId, uint32_t microstep)
     return OK;
 }
 
+uint32_t MotorSetKvalHold(TimerMotors_t _motorId, uint8_t Value)
+{
+    MotorDriverRequest[_motorId].KvalHold = Value;
+    FPGA_SetMotKvalHold(_motorId);
+
+    return OK;
+}
+uint32_t MotorSetKvalRun(TimerMotors_t _motorId, uint8_t Value)
+{
+    MotorDriverRequest[_motorId].KvalRun = Value;
+    FPGA_SetMotKvalRun(_motorId);
 
+    return OK;
+}
+
+uint32_t MotorSetKvalAcc(TimerMotors_t _motorId, uint8_t Value)
+{
+    MotorDriverRequest[_motorId].KvalAcc = Value;
+    FPGA_SetMotKvalAcc(_motorId);
+
+    return OK;
+}
+
+uint32_t MotorSetKvalDec(TimerMotors_t _motorId, uint8_t Value)
+{
+    MotorDriverRequest[_motorId].KvalDec = Value;
+    FPGA_SetMotKvalDec(_motorId);
+
+    return OK;
+}
 
 uint32_t MotorMove(TimerMotors_t _motorId,bool direction, uint32_t Steps)
 {
diff --git a/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.h b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.h
index ecc70765b..6c705e8a8 100644
--- a/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.h
+++ b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.h
@@ -86,6 +86,10 @@ typedef struct
   uint32_t Speed;
   uint32_t Position;
   uint8_t Stop;
+  uint8_t KvalHold;
+  uint8_t KvalRun;
+  uint8_t KvalAcc;
+  uint8_t KvalDec;
 }MotorDriverRequestStruct;
 
 typedef struct
@@ -182,6 +186,11 @@ uint32_t MotorSetSpeedWithCallback (TimerMotors_t _motorId, uint32_t _freq, call
 
 uint32_t SetMotHome(TimerMotors_t _motorId);
 
+uint32_t MotorSetKvalHold(TimerMotors_t _motorId, uint8_t Value);
+uint32_t MotorSetKvalRun(TimerMotors_t _motorId, uint8_t Value);
+uint32_t MotorSetKvalAcc(TimerMotors_t _motorId, uint8_t Value);
+uint32_t MotorSetKvalDec(TimerMotors_t _motorId, uint8_t Value);
+
 void MotorActionsInit(void);
 
 uint32_t MotorGetStatus(TimerMotors_t _motorId);