Merge branch 'master' of https://twinetfs.visualstudio.com/_git/Tango

12 files changed, 132 insertions, 28 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
index 6bbf1efc3..2fe813395 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
@@ -31,6 +31,7 @@
 
 #include "modules/thread/thread_ex.h"
 
+Task_Handle Millisecond_Task_Handle;
 /******************** Definitions  ********************************************/
 #define INVALID_MSG_ID    0xFFFF
 #define MAX_TANGO_CONTROL_DEVICES 200
@@ -231,12 +232,13 @@ uint32_t MillisecLoop(uint32_t tick)
     //gather Motor data from FPGA
 #ifndef EVALUATION_BOARD
         FPGA_GetBusy();  //load the busy motor information to all motors
+        FPGA_Read_limit_Switches();
 #endif
 
     for (Motor_i = 0;Motor_i < NUM_OF_MOTORS;Motor_i++)
     {
-//        if (MotorDriverResponse[Motor_i].Busy == true)
-//            continue;
+        if (MotorDriverResponse[Motor_i].Busy == true)
+            continue;
         if (MotorData[Motor_i].WaitForData == true) //Read request sent, data is waiting
         {
             if (MotorGetFPGAResponse(Motor_i,&MotorInfo) == OK) //got the data from the FPGA
@@ -296,9 +298,9 @@ uint32_t MillisecLoop(uint32_t tick)
             }
         }
     }
-    Dancer_Data[FEEDER_DANCER] = Read_Dancer_Position(ROTENC_RDANCER);
-    Dancer_Data[POOLER_DANCER] = Read_Dancer_Position(ROTENC_LDANCER1);
-    Dancer_Data[WINDER_DANCER] = Read_Dancer_Position(ROTENC_LDANCER2);
+    Dancer_Data[FEEDER_DANCER] = Read_Dancer_Position(FEEDER_DANCER);
+    Dancer_Data[POOLER_DANCER] = Read_Dancer_Position(POOLER_DANCER);
+    Dancer_Data[WINDER_DANCER] = Read_Dancer_Position(WINDER_DANCER);
     if (Hundred_msTick)
     {
     }
@@ -370,7 +372,7 @@ void MillisecTask(UArg arg0, UArg arg1)
     //Clock_setTimeout(HostKAClock, 1000);
     //Clock_start(HostKAClock);
     MillisecInit();
-
+    Millisecond_Task_Handle = Task_self();
     while(1)
     {
         Mailbox_pend(MillisecMsgQ , &Message, BIOS_WAIT_FOREVER);
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h
index f0009b43b..6949a4abe 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h
@@ -30,4 +30,5 @@ void MillisecInit(void);
 void MillisecStop(void);
 void MillisecStart(void);
 
+extern Task_Handle Millisecond_Task_Handle;
 #endif /* MODULES_CONTROL_MILLISECTASK_H_ */
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/control.c b/Software/Embedded_SW/Embedded/Modules/Control/control.c
index 08001cf01..40eee95c8 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/control.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/control.c
@@ -90,7 +90,7 @@ int ControlPhaseDelay = 300; //the control task enters only after data gathering
 Mailbox_Handle          ControlMsgQ = NULL;
 bool                    ControlRestart;
 static GateMutex_Handle gateControlDB;
-
+Task_Handle Control_Task_Handle;
 ControlDeviceStruc      ControlArray[MAX_TANGO_CONTROL_DEVICES];
 uint32_t                ControlDatalog[MAX_TANGO_CONTROL_DEVICES];
 uint32_t        Control_timerBase = TIMER0_BASE;        //Timer handle
@@ -321,7 +321,7 @@ void controlTask(UArg arg0, UArg arg1)
     //uint16_t length;
     //Clock_setTimeout(HostKAClock, 1000);
     //Clock_start(HostKAClock);
-
+    Control_Task_Handle = Task_self();
 
     while(1)
     {
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/control.h b/Software/Embedded_SW/Embedded/Modules/Control/control.h
index 76e90ed2e..853393739 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/control.h
+++ b/Software/Embedded_SW/Embedded/Modules/Control/control.h
@@ -40,5 +40,6 @@ uint32_t    AddControlCallback( ControlCBFunction Callback, CTRL_TIMING_ENUM Ctr
 int         RemoveControlCallback(uint32_t deviceId, ControlCBFunction Callback );
 uint32_t    TemplateDataReadCBFunction (uint32_t deviceId, uint32_t Parameter1, uint32_t Parameter2);
 
+extern Task_Handle Control_Task_Handle;
 
 #endif
diff --git a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Dancer.c b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Dancer.c
index e36eea05d..499f227a9 100644
--- a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Dancer.c
+++ b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Dancer.c
@@ -53,11 +53,11 @@ void Stub_DancerPositionRequest(MessageContainer* requestContainer)
     response.dancer_id = request->dancer_id;
     response.has_dancer_id = true;
 
-    RotEnc_t Dancer_Id = (RotEnc_t)request->dancer_id;//to remove warning
+    HardwareDancerType Dancer_Id = (HardwareDancerType)request->dancer_id;//to remove warning
 
     if(Dancer_Id < NUM_OF_ROTENC)
     {
-        FPGA_SSI_Transnit(Dancer_Id);
+        FPGA_SSI_Transmit(Dancer_Id);
         SysCtlDelay(1000);
         FPGA_SSI_Receive(Dancer_Id);
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_FPGAReadBackReg.c b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_FPGAReadBackReg.c
index 8b9dc8103..eef70ee37 100644
--- a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_FPGAReadBackReg.c
+++ b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_FPGAReadBackReg.c
@@ -36,7 +36,7 @@ void Stub_FPGAReadBackRegRequest(MessageContainer* requestContainer)
     writeString(", ");
     writeFloat(request->value);
 
-    status = Test_FPGA_ReadBack((unsigned char) request->fpgaid, (unsigned short) request->value, &ReadBack_Value);
+    status = FPGA_Test_ReadBack((unsigned char) request->fpgaid, (unsigned short) request->value, &ReadBack_Value);
 
     StubFPGAReadBackRegResponse response =   STUB_FPGAREAD_BACK_REG_RESPONSE__INIT;
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Motor.c b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Motor.c
index e580c2949..5bb69d230 100644
--- a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Motor.c
+++ b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Motor.c
@@ -116,7 +116,7 @@ void Stub_MotorInitRequest(MessageContainer* requestContainer)
 #else
     TimerMotors_t MotorId =(TimerMotors_t) request->motor_id;
 
-    //MotorConfig(MotorId, &MotorDriverConfig); ------------------------------ to open !!!!!!!!!!!!!!!!!!!!!!1
+    MotorConfig(MotorId, &MotorDriverConfig); //------------------------------ to open !!!!!!!!!!!!!!!!!!!!!!1
 #endif
 
     //setup();
diff --git a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_SpeedSensor.c b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_SpeedSensor.c
new file mode 100644
index 000000000..445674fe1
--- /dev/null
+++ b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_SpeedSensor.c
@@ -0,0 +1,76 @@
+/*
+ * Stub_SpeedSensor.c
+ *
+ *  Created on: May 14, 2018
+ *      Author: avi
+ */
+
+
+
+#include <Container.h>
+#include <DataDef.h>
+#include <stdbool.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <stdint.h>
+#include <string.h>
+
+#include <PMR/Stubs/StubSpeedSensorResponse.pb-c.h>
+#include <PMR/Stubs/StubSpeedSensorRequest.pb-c.h>
+
+#include "inc/hw_memmap.h"
+#include "inc/hw_types.h"
+#include "inc/hw_uart.h"
+
+#include "Drivers/USB_Communication/USBCDCD.h"
+#include "drivers/twine_graphicslib/graphics_adapter.h"
+
+#include "MessageContainer.pb-c.h"
+
+#include "Stub_Status.h"
+#include "drivers/FPGA/FPGA.h"
+#include "drivers/SPI/SPI_Comm.h"
+
+#include "drivers/FPGA/Moters_Driver/L6470.h"
+
+#include "driverlib/ssi.h"
+#include "drivers/SPI/SPI_Comm.h"
+#include "drivers/FPGA/FPGA_SSI_Comm.h"
+#include "Modules/Thread/Thread_ex.h"
+
+
+#include "Modules/thread/thread.h"
+
+
+void Stub_SpeedSensorRequest(MessageContainer* requestContainer)
+{
+
+    MessageContainer responseContainer;
+
+    StubSpeedSensorRequest* request = stub_speed_sensor_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
+
+    StubSpeedSensorResponse response =   STUB_SPEED_SENSOR_RESPONSE__INIT;
+
+    FPGA_SSI_Speed_Sensor_Transnit();
+    SysCtlDelay(100);
+
+    response.speed = FPGA_SSI_Speed_Sensor_Receive();
+    response.has_speed = true;
+
+    responseContainer = createContainer(MESSAGE_TYPE__StubSpeedSensorResponse, requestContainer->token, true, &response, &stub_speed_sensor_response__pack, &stub_speed_sensor_response__get_packed_size);
+
+    //free(request);
+    //-------------------------------------------------------------------------------------------
+    uint8_t* container_buffer = malloc(message_container__get_packed_size(&responseContainer));
+    size_t container_size = message_container__pack(&responseContainer, container_buffer);
+    free(responseContainer.data.data);
+    SendChars((char*)container_buffer, container_size);
+    //free(container_buffer);
+    //free(requestContainer);
+    stub_speed_sensor_request__free_unpacked(request,NULL);
+}
+
+
+
+
+
diff --git a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_SpeedSensor.h b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_SpeedSensor.h
new file mode 100644
index 000000000..ec633cd7a
--- /dev/null
+++ b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_SpeedSensor.h
@@ -0,0 +1,15 @@
+/*
+ * Stub_SpeedSensor.h
+ *
+ *  Created on: May 14, 2018
+ *      Author: avi
+ */
+
+#ifndef MODULES_STUBS_HANDLER_STUB_SPEEDSENSOR_H_
+#define MODULES_STUBS_HANDLER_STUB_SPEEDSENSOR_H_
+
+void Stub_SpeedSensorRequest(MessageContainer* requestContainer);
+
+
+
+#endif /* MODULES_STUBS_HANDLER_STUB_SPEEDSENSOR_H_ */
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
index 0bda4964b..db3bfc46e 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
@@ -43,7 +43,7 @@ typedef enum threadMotorsEnum
 #define MAX_SYSTEM_DANCERS HARDWARE_DANCER_TYPE__RightDancer+1
 
 extern TimerMotors_t ThreadMotorIdToMotorId[MAX_THREAD_MOTORS_NUM];
-extern DANCER_ENUM ThreadMotorIdToDancerId[MAX_THREAD_MOTORS_NUM];
+extern HardwareDancerType ThreadMotorIdToDancerId[MAX_THREAD_MOTORS_NUM];
 extern HardwarePidControlType ThreadMotorIdToControlId[MAX_THREAD_MOTORS_NUM];
 
 extern HardwareMotor                MotorsCfg[NUM_OF_MOTORS];
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
index 3fb78d2f0..e957b0efc 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
@@ -2,13 +2,13 @@
 #ifndef MODULES_THREAD_THREAD_EX_H_
 #define MODULES_THREAD_THREAD_EX_H_
 
-typedef enum
-{
-    WINDER_DANCER,
-    POOLER_DANCER,
-    FEEDER_DANCER,
-    NUM_OF_DANCERS
-} DANCER_ENUM;
+//typedef enum
+//{
+#define WINDER_DANCER HARDWARE_DANCER_TYPE__LeftDancer
+#define POOLER_DANCER HARDWARE_DANCER_TYPE__MiddleDancer
+#define FEEDER_DANCER HARDWARE_DANCER_TYPE__RightDancer
+#define NUM_OF_DANCERS HARDWARE_DANCER_TYPE__RightDancer+1
+//} DANCER_ENUM;
 
 uint32_t ThreadPrepareState(void *JobDetails);
 uint32_t ThreadPreSegmentState(void *JobDetails);
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index 92939dfe4..67f2227d2 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -11,6 +11,7 @@
 #include "../control/pidalgo.h"
 #include "PMR/Hardware/HardwareMotor.pb-c.h"
 #include "PMR/Hardware/HardwareMotorType.pb-c.h"
+#include "PMR/Hardware/HardwareDancerType.pb-c.h"
 #include "PMR/Printing/JobSegment.pb-c.h"
 #include "PMR/Printing/JobTicket.pb-c.h"
 
@@ -25,9 +26,10 @@
 //by recieved esign flow of the user from the UI
 ///////////////////////////////////////////////////////////////////////////////////////////
 
+uint32_t CurrentControlledSpeed[MAX_THREAD_MOTORS_NUM] = {0};
 
 TimerMotors_t ThreadMotorIdToMotorId[MAX_THREAD_MOTORS_NUM] = {HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING,HARDWARE_MOTOR_TYPE__MOTO_LDRIVING,HARDWARE_MOTOR_TYPE__MOTO_WINDER,HARDWARE_MOTOR_TYPE__MOTO_SCREW};
-DANCER_ENUM ThreadMotorIdToDancerId[MAX_THREAD_MOTORS_NUM] = {FEEDER_DANCER,NUM_OF_DANCERS,POOLER_DANCER,WINDER_DANCER,NUM_OF_DANCERS};
+HardwareDancerType ThreadMotorIdToDancerId[MAX_THREAD_MOTORS_NUM] = {FEEDER_DANCER,NUM_OF_DANCERS,POOLER_DANCER,WINDER_DANCER,NUM_OF_DANCERS};
 uint32_t    ControlIdtoMotorId [MAX_THREAD_MOTORS_NUM] = {0xFF};
 int OriginalMotorSpd_2PPS[MAX_THREAD_MOTORS_NUM] = {0};
 
@@ -209,6 +211,8 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
     {
         DancerId = ThreadMotorIdToDancerId[index];
         TranslatedReadValue = ReadValue - DancersCfg[DancerId].zeropoint;
+        if (index == POOLER_MOTOR)
+            TranslatedReadValue = (-1*TranslatedReadValue);
         MotorSamples[index][MotorSamplePointer[index]] = TranslatedReadValue;//(-1 * TranslatedReadValue);
         MotorSamplePointer[index]++;
         if (MotorSamplePointer[index] >= MotorsControl[index].pvinputfilterfactormode) MotorSamplePointer[index] = 0;
@@ -227,9 +231,12 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         {
             MotorControlConfig[index].m_calculatedError = MotorControlConfig[index].m_params.MIN;
         }*/
-
-        MotorSetSpeed(ThreadMotorIdToMotorId[index], (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index],  MotorsCfg[ThreadMotorIdToMotorId[index]].microstep);
-        //SetMotorFreq (index, MotorControlConfig[index].m_calculatedError);
+        uint32_t calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index];
+        if (abs(calculated_speed-CurrentControlledSpeed[index])>5)
+        {
+            CurrentControlledSpeed[index] = calculated_speed;
+            MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed,  MotorsCfg[ThreadMotorIdToMotorId[index]].microstep);
+        }
     }
 
  return OK;
@@ -270,7 +277,7 @@ bool InitialProcess = false;
             MotorControlConfig[Motor_i].m_params.Kp = MotorsControl[Pid_Id].proportionalgain;
             MotorControlConfig[Motor_i].m_params.Ki = MotorsControl[Pid_Id].integraltime;
             MotorControlConfig[Motor_i].m_params.epsilon = 0.01;
-            MotorControlConfig[Motor_i].m_params.dt = eOneMillisecond;
+            MotorControlConfig[Motor_i].m_params.dt = 50;
             MotorControlConfig[Motor_i].m_calculatedError = 0;
             MotorControlConfig[Motor_i].m_integral = 0;
             MotorControlConfig[Motor_i].m_isEnabled = true;
@@ -281,11 +288,13 @@ bool InitialProcess = false;
 
             MotorSetDirection((TimerMotors_t)HW_Motor_Id,MotorsCfg[HW_Motor_Id].directionthreadwize);
     #ifdef DEBUG_TEST_FUNCTIONS
-/*
+
+            if (Motor_i == FEEDER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
+                ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadLengthCBFunction, eHundredMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i);
             if (Motor_i == FEEDER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
-                ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadLengthCBFunction, eOneMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i);
+                ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i);
             if (Motor_i == POOLER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
-                ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadControlCBFunction, eHundredMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i);
+                ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i);
             /*if (HW_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
                 //AddControlCallback(ThreadSpeedControlCBFunction, eOneMillisecond,MotorGetSpeed,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],0);
     //            continue;