Add support for goto direction.

use the function MotorGoToDir

7 files changed, 89 insertions, 2 deletions

diff --git a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_GPIO/FPGA_GPIO.c b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_GPIO/FPGA_GPIO.c
index f4b22b3a0..69b6d3b3b 100644
--- a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_GPIO/FPGA_GPIO.c
+++ b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_GPIO/FPGA_GPIO.c
@@ -549,6 +549,10 @@ uint32_t Control_WD(bool IsEnable, unsigned char SetTimer_Steps100mSec) // Contr
 
 #ifndef EVALUATION_BOARD
 
+    #ifndef  FPGA_WATCHDOG
+        IsEnable = DISABLE;
+    #endif
+
     short WD_Enable = 0x4000 | SetTimer_Steps100mSec;
 
     if (IsEnable == DISABLE)
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 45299a57e..9f40a0ca1 100644
--- a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.c
+++ b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.c
@@ -880,6 +880,24 @@ void FPGA_SetGoToPosition(TimerMotors_t _motorId)//GoTo
 
 }
 
+void FPGA_SetGoToDirPosition(TimerMotors_t _motorId)//GoTo_DIR command
+{
+    uint32_t temp;
+
+    temp = x_GOTO_DIR | MotorDriverRequest[_motorId].Direction;
+    temp = temp << 24;
+
+    if (MotorDriverRequest[_motorId].Position > 0x3FFFFF) //TODO to check if it is needed here
+        MotorDriverRequest[_motorId].Position = 0x3FFFFF;
+
+    temp |= (MotorDriverRequest[_motorId].Position);
+    Fpga_Spi[_motorId].TX_MOSI = temp;
+
+    //MillisecWriteToMotor(_motorId, temp, 4, NULL);
+    FPGA_SPI_Transnit(_motorId);
+
+}
+
 void FPGA_SetMotPosition(TimerMotors_t _motorId)//Mov
 {
     uint32_t temp;
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 aa771d100..40dbe3948 100644
--- a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.h
+++ b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.h
@@ -45,5 +45,6 @@ void FPGA_SetGoToPosition(TimerMotors_t _motorId);//GoTo
 void Change_Motor_SPI_Direction(TimerMotors_t _motorId);
 
 void FPGA_SetMotPowerStwp01GateCfg(TimerMotors_t _motorId);
+void FPGA_SetGoToDirPosition(TimerMotors_t _motorId);//GoTo_DIR command
 
 
diff --git a/Software/Embedded_SW/Embedded/Drivers/FPGA/Motors_Driver/L6470.c b/Software/Embedded_SW/Embedded/Drivers/FPGA/Motors_Driver/L6470.c
index f92278657..878b9e19f 100644
--- a/Software/Embedded_SW/Embedded/Drivers/FPGA/Motors_Driver/L6470.c
+++ b/Software/Embedded_SW/Embedded/Drivers/FPGA/Motors_Driver/L6470.c
@@ -187,7 +187,7 @@ void GoTo(unsigned long pos)
 void GoTo_DIR(byte dir, unsigned long pos)
 {
 
-    Data_To_Transfer(x_GOTO_DIR);
+    Data_To_Transfer(x_GOTO_DIR | dir);
     if (pos > 0x3FFFFF) pos = 0x3FFFFF;
     Data_To_Transfer((byte)(pos >> 16));
     Data_To_Transfer((byte)(pos >> 8));
diff --git a/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c
index 2c29ad564..c262642ff 100644
--- a/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c
+++ b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c
@@ -425,6 +425,19 @@ uint32_t MotorGoTo(TimerMotors_t _motorId, uint32_t Steps)
     return OK;
 }
 
+uint32_t MotorGoToDir(TimerMotors_t _motorId,bool direction, uint32_t Steps)
+{
+    #ifdef EVALUATION_BOARD
+    GoTo_DIR(direction , Steps );
+    #else
+            MotorDriverRequest[_motorId].Position = Steps;
+            MotorDriverRequest[_motorId].Direction = direction;
+            FPGA_SetGoToDirPosition(_motorId);
+    #endif
+    Report("MotorGoToDir",__FILE__,__LINE__, _motorId,RpMessage,Steps,0);
+    return OK;
+}
+
 ///////////////// temporary for testing /////////////
 /*
 void loop_Run_Read_Speed()//every miliSec
@@ -513,4 +526,46 @@ void Combined_dispenser_Driver_Mode(uint8_t Dispenser_ID, MOTDRIVER_MODE New_Mod
     Combined_Motor_Driver_Mode(Dispenser_ID + 5, New_Mode);//dispenser ID -> motor id
 }
 
+void test_Home_Pos()
+{
+    SetMotHome(HARDWARE_MOTOR_TYPE__MOTO_SCREW);
+    delayms(10);
+    char i = 0;
+    uint32_t Steps = 100;
+    bool dir = true;
+
+    for(i=0;i<5;i++)
+    {
+        MotorGoToDir(HARDWARE_MOTOR_TYPE__MOTO_SCREW,dir, Steps);
+        //MotorMove(HARDWARE_MOTOR_TYPE__MOTO_SCREW,dir, Steps);
+        Steps+=50;
+        delayms(200);
+    }
+
+
+    MotorStop(HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Hiz);
+    delayms(1000);
+    dir = false;
+
+    for(i=0;i<5;i++)
+    {
+        Steps-=50;
+        MotorGoToDir(HARDWARE_MOTOR_TYPE__MOTO_SCREW,dir, Steps);
+       // MotorMove(HARDWARE_MOTOR_TYPE__MOTO_SCREW,dir, Steps);
+        delayms(200);
+    }
+
+/*
+    dir = true;
+    MotorStop(HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Hiz);
+    delayms(10);
 
+    for(i=0;i<5;i++)
+    {
+        //MotorGoToDir(HARDWARE_MOTOR_TYPE__MOTO_SCREW,0, Steps);
+        MotorMove(HARDWARE_MOTOR_TYPE__MOTO_SCREW,dir, Steps);
+        //Steps+=50;
+        delayms(10);
+    }
+*/
+}
diff --git a/Software/Embedded_SW/Embedded/Main.c b/Software/Embedded_SW/Embedded/Main.c
index 224e32a70..a4cee031d 100644
--- a/Software/Embedded_SW/Embedded/Main.c
+++ b/Software/Embedded_SW/Embedded/Main.c
@@ -74,7 +74,8 @@
 #include "Modules/General/buttons.h"
 #include "Modules/Waste/Waste.h"
 #include <Drivers/I2C_Communication/Dispenser_Card/IO_Ports/Dispenser_IO.h>
-//#define WATCHDOG
+#define WATCHDOG
+#define FPGA_WATCHDOG
 
 
 //*****************************************************************************
@@ -275,6 +276,9 @@ int main(void)
     //IDS_ModuleInit();
     Valve_Set(VALVE_MIXCHIP_WASTECH, Mixer_Waste);
 
+#ifndef  FPGA_WATCHDOG
+    Control_WD(DISABLE,0);
+#endif
     ////////////////////////////
 
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Progress.c b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Progress.c
index c3b94c01f..5d50a21c7 100644
--- a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Progress.c
+++ b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Progress.c
@@ -31,6 +31,11 @@ void Stub_ProgressRequest(MessageContainer* requestContainer)
         response.has_progress = true;
     }
     else
+    if((request->amount == 0x5C4E) && (request->delay == 0x5C4E)) //Screw Home Pos dir
+        {
+            test_Home_Pos();
+        }
+    else
     if((request->amount == 0xCF) && (request->delay == 0xCF)) //Create File System on the Drive
     {
         FRESULT iFResult = Init_Flash_File_System(true);