Calculate Speed Sensor Velocity

15 files changed, 192 insertions, 90 deletions

diff --git a/Software/Embedded_SW/Embedded/.cproject b/Software/Embedded_SW/Embedded/.cproject
index 763ec6609..f81cf0c62 100644
--- a/Software/Embedded_SW/Embedded/.cproject
+++ b/Software/Embedded_SW/Embedded/.cproject
@@ -163,7 +163,7 @@
 						</toolChain>
 					</folderInfo>
 					<sourceEntries>
-						<entry excluding="Drivers/I2C_Communication/I2C_Switch|Drivers/I2C_Communication/Previous_Code|Drivers/I2C_Communication/Previous_Code/TestI2C|JigCommands.c|PWM.c|USBCDCD.c|Message.c|Pin_config.c|src|Configuration.c|SlowMotors.c|Timer.c|Communication.c|FastMotors.c|ADCUtils.c|Drivers/L6470|Pin.c|ADCLogger.c|Flashstore.c|Profile.c" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name=""/>
+						<entry excluding="JigCommands.c|PWM.c|USBCDCD.c|Message.c|Pin_config.c|src|Configuration.c|SlowMotors.c|Timer.c|Communication.c|FastMotors.c|ADCUtils.c|Drivers/I2C_Communication/Previous_Code|Drivers/L6470|Pin.c|ADCLogger.c|Drivers/I2C_Communication/Previous_Code/TestI2C|Flashstore.c|Profile.c|Drivers/I2C_Communication/I2C_Switch" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name=""/>
 					</sourceEntries>
 				</configuration>
 			</storageModule>
@@ -431,7 +431,7 @@
 						</toolChain>
 					</folderInfo>
 					<sourceEntries>
-						<entry excluding="Drivers/I2C_Communication/Previous_Code|Drivers/I2C_Communication/Previous_Code/TestI2C|JigCommands.c|PWM.c|USBCDCD.c|Message.c|Pin_config.c|src|Configuration.c|SlowMotors.c|Timer.c|Communication.c|FastMotors.c|ADCUtils.c|Drivers/L6470|Pin.c|ADCLogger.c|Flashstore.c|Profile.c" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name=""/>
+						<entry excluding="JigCommands.c|PWM.c|USBCDCD.c|Message.c|Pin_config.c|src|Configuration.c|SlowMotors.c|Timer.c|Communication.c|FastMotors.c|ADCUtils.c|Drivers/I2C_Communication/Previous_Code|Drivers/L6470|Pin.c|ADCLogger.c|Drivers/I2C_Communication/Previous_Code/TestI2C|Flashstore.c|Profile.c" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name=""/>
 					</sourceEntries>
 				</configuration>
 			</storageModule>
diff --git a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SSI_Comm.c b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SSI_Comm.c
index cd6643c72..5d22b2d67 100644
--- a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SSI_Comm.c
+++ b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SSI_Comm.c
@@ -10,7 +10,7 @@
 #include <drivers/FPGA/FPGA_Comm.h>
 #include <DataDef.h>
 #include "drivers/FPGA/FPGA_SSI_Comm.h"
-#include "drivers/Danser_SSI/SSI_Comm.h"
+#include "drivers/SSI_Comm/SSI_Comm.h"
 #include "PMR/Hardware/HardwareDancerType.pb-c.h"
 
 
diff --git a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SSI_Comm.h b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SSI_Comm.h
index 168fa43a1..0efb9b8a2 100644
--- a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SSI_Comm.h
+++ b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SSI_Comm.h
@@ -8,7 +8,7 @@
 #ifndef DRIVERS_FPGA_FPGA_SSI_COMM_H_
 #define DRIVERS_FPGA_FPGA_SSI_COMM_H_
 
-#include "drivers/Danser_SSI/SSI_Comm.h"
+#include "drivers/SSI_Comm/SSI_Comm.h"
 
 typedef struct  // 24bit
 {
diff --git a/Software/Embedded_SW/Embedded/Drivers/SSI_Comm/Dancer/Dancer.c b/Software/Embedded_SW/Embedded/Drivers/SSI_Comm/Dancer/Dancer.c
new file mode 100644
index 000000000..514ba7134
--- /dev/null
+++ b/Software/Embedded_SW/Embedded/Drivers/SSI_Comm/Dancer/Dancer.c
@@ -0,0 +1,91 @@
+/*
+ * Dancer.c
+ *
+ *  Created on: Aug 5, 2018
+ *      Author: avi
+ */
+
+
+#include <stdbool.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <stdint.h>
+#include <string.h>
+
+#include "inc/hw_memmap.h"
+#include "driverlib/gpio.h"
+#include "driverlib/pin_map.h"
+#include "driverlib/ssi.h"
+#include "driverlib/sysctl.h"
+#include "utils/uartstdio.h"
+
+#include <driverlib/rom.h>
+#include <driverlib/rom_map.h>
+#include <DataDef.h>
+#include <Drivers/SSI_Comm/SSI_Comm.h>
+#include "driverlib/uart.h"
+
+#include "drivers/FPGA/FPGA_SSI_Comm.h"
+#include "PMR/Hardware/HardwareDancerType.pb-c.h"
+
+
+
+extern SSI_DANCER_ENC DANCER_ENC[NUM_OF_ROTENC];
+
+typedef struct  // 24bit
+{
+    unsigned short Position:14;
+    unsigned char Gen_status:2;
+    unsigned char Det_status:8;
+}SSI_ENC;
+
+SSI_ENC SSI_enc;
+
+uint32_t Control_Read_Dancer_Position(HardwareDancerType DancerId, uint32_t Parameter1, uint32_t Parameter2)
+{
+    return DANCER_ENC[DancerId].Position;
+    //return SSI_enc.Position;
+}
+
+
+uint32_t Read_Dancer_Position (HardwareDancerType DancerId)
+{
+    #ifdef EVALUATION_BOARD
+        uint32_t ENC_SSI_Data = 0;
+
+        SSIEnable(SSI1_BASE);
+
+        ENC_SSI_Data = Write_Dummy_Byte();
+
+
+        //SSI_enc.Det_status = ENC_SSI_Data & 0xFF;
+        //ENC_SSI_Data = ENC_SSI_Data >> 8;
+        SSI_enc.Det_status = 0;
+
+        SSI_enc.Gen_status = ENC_SSI_Data & 0x3;
+        ENC_SSI_Data = ENC_SSI_Data >> 2;
+        SSI_enc.Position =   ENC_SSI_Data & 0x3FFF; // up 11130 down 8747
+
+        SSIDisable(SSI1_BASE);
+        return SSI_enc.Position;
+    #else
+        // must call FPGA_SSI_Transmit first!
+        HardwareDancerType Dancer_Id = (HardwareDancerType)DancerId;//to remove warning
+        FPGA_SSI_Receive(Dancer_Id);
+        FPGA_SSI_Transmit(Dancer_Id);
+        return DANCER_ENC[Dancer_Id].Position;
+    #endif
+}
+
+void Loop_SSI()
+{
+    while(1)
+    {
+        Read_Dancer_Position(HARDWARE_DANCER_TYPE__RightDancer);
+        SysCtlDelay(1000); // Min 1000 (Pause time > 20uSec)
+    }
+
+}
+
+
+
diff --git a/Software/Embedded_SW/Embedded/Drivers/SSI_Comm/Dancer/Dancer.h b/Software/Embedded_SW/Embedded/Drivers/SSI_Comm/Dancer/Dancer.h
new file mode 100644
index 000000000..8d21de87b
--- /dev/null
+++ b/Software/Embedded_SW/Embedded/Drivers/SSI_Comm/Dancer/Dancer.h
@@ -0,0 +1,20 @@
+/*
+ * Dancer.h
+ *
+ *  Created on: Aug 5, 2018
+ *      Author: avi
+ */
+
+#ifndef DRIVERS_SSI_COMM_DANCER_DANCER_H_
+#define DRIVERS_SSI_COMM_DANCER_DANCER_H_
+
+uint32_t Read_Dancer_Position (HardwareDancerType DancerId);
+uint32_t Control_Read_Dancer_Position(HardwareDancerType DancerId, uint32_t Parameter1, uint32_t Parameter2);
+uint32_t Read_Speed_Sensor (void);
+
+void Loop_SSI();
+
+
+
+
+#endif /* DRIVERS_SSI_COMM_DANCER_DANCER_H_ */
diff --git a/Software/Embedded_SW/Embedded/Drivers/Danser_SSI/SSI_Comm.c b/Software/Embedded_SW/Embedded/Drivers/SSI_Comm/SSI_Comm.c
index cff0ed994..7f35ead98 100644
--- a/Software/Embedded_SW/Embedded/Drivers/Danser_SSI/SSI_Comm.c
+++ b/Software/Embedded_SW/Embedded/Drivers/SSI_Comm/SSI_Comm.c
@@ -46,23 +46,22 @@ up
 #include <driverlib/rom.h>
 #include <driverlib/rom_map.h>
 #include <DataDef.h>
+#include <Drivers/SSI_Comm/SSI_Comm.h>
 #include "driverlib/uart.h"
 
-#include "SSI_Comm.h"
-
 #include "drivers/FPGA/FPGA_SSI_Comm.h"
 #include "PMR/Hardware/HardwareDancerType.pb-c.h"
 
 
-
-extern SSI_DANCER_ENC DANCER_ENC[NUM_OF_ROTENC];
-
-typedef struct  // 24bit
-{
-    unsigned short Position:14;
-    unsigned char Gen_status:2;
-    unsigned char Det_status:8;
-}SSI_ENC;
+//
+//extern SSI_DANCER_ENC DANCER_ENC[NUM_OF_ROTENC];
+//
+//typedef struct  // 24bit
+//{
+//    unsigned short Position:14;
+//    unsigned char Gen_status:2;
+//    unsigned char Det_status:8;
+//}SSI_ENC;
 
 /*
 typedef enum
@@ -77,7 +76,7 @@ typedef enum
        {LDANCER2_ROTENC2,"LDANCER2_ROTENC2"},
        {RDANCER_ROTENC2,"RDANCER_ROTENC2"},
 */
-SSI_ENC SSI_enc;
+
 
 void SSI1_Init()
 {
@@ -202,56 +201,3 @@ uint32_t Write_Dummy_Byte()
     //return (RByte & 0xffffff);// 24 bytes
     return (RByte & 0xffff);//14 bit position + 2 bit general status
 }
-uint32_t Control_Read_Dancer_Position(HardwareDancerType DancerId, uint32_t Parameter1, uint32_t Parameter2)
-{
-    return DANCER_ENC[DancerId].Position;
-    //return SSI_enc.Position;
-}
-
-
-uint32_t Read_Dancer_Position (HardwareDancerType DancerId)
-{
-    #ifdef EVALUATION_BOARD
-        uint32_t ENC_SSI_Data = 0;
-
-        SSIEnable(SSI1_BASE);
-
-        ENC_SSI_Data = Write_Dummy_Byte();
-
-
-        //SSI_enc.Det_status = ENC_SSI_Data & 0xFF;
-        //ENC_SSI_Data = ENC_SSI_Data >> 8;
-        SSI_enc.Det_status = 0;
-
-        SSI_enc.Gen_status = ENC_SSI_Data & 0x3;
-        ENC_SSI_Data = ENC_SSI_Data >> 2;
-        SSI_enc.Position =   ENC_SSI_Data & 0x3FFF; // up 11130 down 8747
-
-        SSIDisable(SSI1_BASE);
-        return SSI_enc.Position;
-    #else
-        // must call FPGA_SSI_Transmit first!
-        HardwareDancerType Dancer_Id = (HardwareDancerType)DancerId;//to remove warning
-        FPGA_SSI_Receive(Dancer_Id);
-        FPGA_SSI_Transmit(Dancer_Id);
-        return DANCER_ENC[Dancer_Id].Position;
-    #endif
-}
-uint32_t Read_Speed_Sensor () // //The speed sensor, RMB14SC12BC59N , is 12bit SSI encoder
-{
-    FPGA_SSI_Speed_Sensor_Transnit();
-    FPGA_SSI_Speed_Sensor_Receive();
-    return SpeedSensorResponseS.Speed;
-}
-void Loop_SSI()
-{
-    while(1)
-    {
-        Read_Dancer_Position(HARDWARE_DANCER_TYPE__RightDancer);
-        SysCtlDelay(1000); // Min 1000 (Pause time > 20uSec)
-    }
-
-}
-
-
-
diff --git a/Software/Embedded_SW/Embedded/Drivers/Danser_SSI/SSI_Comm.h b/Software/Embedded_SW/Embedded/Drivers/SSI_Comm/SSI_Comm.h
index 220f607f8..9028de45b 100644
--- a/Software/Embedded_SW/Embedded/Drivers/Danser_SSI/SSI_Comm.h
+++ b/Software/Embedded_SW/Embedded/Drivers/SSI_Comm/SSI_Comm.h
@@ -25,11 +25,6 @@
 
 #define NUM_OF_ROTENC HARDWARE_DANCER_TYPE__RightDancer+1
 void SSI1_Init();
-uint32_t Read_Dancer_Position (HardwareDancerType DancerId);
-uint32_t Control_Read_Dancer_Position(HardwareDancerType DancerId, uint32_t Parameter1, uint32_t Parameter2);
-uint32_t Read_Speed_Sensor (void);
-
-void Loop_SSI();
 
 
 #endif  //DRIVERS_DANCER_SSI_SSI_COMM_H_
diff --git a/Software/Embedded_SW/Embedded/Drivers/Danser_SSI/Speed_Sensor.c b/Software/Embedded_SW/Embedded/Drivers/SSI_Comm/Speed_Sensor/Speed_Sensor.c
index 628bab5e1..fc831da41 100644
--- a/Software/Embedded_SW/Embedded/Drivers/Danser_SSI/Speed_Sensor.c
+++ b/Software/Embedded_SW/Embedded/Drivers/SSI_Comm/Speed_Sensor/Speed_Sensor.c
@@ -24,14 +24,13 @@
 #include <driverlib/rom.h>
 #include <driverlib/rom_map.h>
 #include <DataDef.h>
+#include <Drivers/SSI_Comm/Speed_Sensor/Speed_Sensor.h>
+#include <Drivers/SSI_Comm/SSI_Comm.h>
 #include "driverlib/uart.h"
 
-#include "SSI_Comm.h"
-
 #include "drivers/FPGA/FPGA_SSI_Comm.h"
 
-#include "Speed_Sensor.h"
-
+/*
 SSReg0 SSREG_RW_ADR_49;
 SSReg1 SSREG_RW_ADR_50;
 SSReg2 SSREG_RW_ADR_51;
@@ -60,11 +59,60 @@ void Init_Speed_Sensor_Registers()
 
     SSREG_RW_ADR_52.bits.Dact    = 1;
     SSREG_RW_ADR_52.bits.Dac     = 0;
-    SSREG_RW_ADR_52.bits.SSIcfg  = 0; // SSI ring congig ???????
+    SSREG_RW_ADR_52.bits.SSIcfg  = 0; // SSI ring config ???????
     SSREG_RW_ADR_52.bits.Sth     = 0x07;
     SSREG_RW_ADR_52.bits.UVW     = 0;
     SSREG_RW_ADR_52.bits.Res     = 0;
 
 }
 
+*/
+
+uint32_t Read_Speed_Sensor () // //The speed sensor, RMB14SC12BC59N , is 12bit SSI encoder
+{
+    FPGA_SSI_Speed_Sensor_Transnit();
+    FPGA_SSI_Speed_Sensor_Receive();
+    return SpeedSensorResponseS.Speed;
+}
+
+uint32_t Calculate_Speed_Sensor_Velocity( void ) // Call the function every 100mSec
+{
+
+    /*
+     * Velocity = 2*Phi*n/Nt
+        Where:
+        Velocity = angular speed (rad/s)
+        n = number of pulses
+        t = sampling period (s)
+        N = pulses per rotation
+     */
+
+    static uint32_t Prev_Enc_Position = 0;
+    uint32_t Enc_Position = Read_Speed_Sensor();
+
+    uint32_t number_of_pulses = Enc_Position - Prev_Enc_Position;
+
+    uint32_t sampling_period_mSec = 100;
+
+    uint32_t pulses_per_rotation = 0x0FFF;// ???
+
+    float temp = 2 * 3.14159265; // 2 *Phi
+
+    temp *=number_of_pulses;//2 *Phi*n
+
+    temp *=1000; //msec to sec;
+
+    temp /= pulses_per_rotation;
+
+    temp /= sampling_period_mSec;
+
+    uint32_t velocity = (uint32_t) temp; // Rad/Sec
+
+    Prev_Enc_Position = Enc_Position;
+
+    //1 Rad/Sec = 9.5493 RPM
+
+    return velocity;
+}
+
 
diff --git a/Software/Embedded_SW/Embedded/Drivers/Danser_SSI/Speed_Sensor.h b/Software/Embedded_SW/Embedded/Drivers/SSI_Comm/Speed_Sensor/Speed_Sensor.h
index c49bc2c6f..d3eae1129 100644
--- a/Software/Embedded_SW/Embedded/Drivers/Danser_SSI/Speed_Sensor.h
+++ b/Software/Embedded_SW/Embedded/Drivers/SSI_Comm/Speed_Sensor/Speed_Sensor.h
@@ -5,10 +5,10 @@
  *      Author: avi
  */
 
-#ifndef DRIVERS_DANSER_SSI_SPEED_SENSOR_H_
-#define DRIVERS_DANSER_SSI_SPEED_SENSOR_H_
-
+#ifndef DRIVERS_SSI_COMM_SPEED_SENSOR_H_
+#define DRIVERS_SSI_COMM_SPEED_SENSOR_H_
 
+/*
 
 #define EEPROM_RW_ADR_0    0x00
 #define EEPROM_RW_ADR_1    0x01
@@ -88,6 +88,8 @@ typedef union
     unsigned short ushort;
 }SSReg3;
 
+*/
 
+uint32_t Calculate_Speed_Sensor_Velocity( void );
 
-#endif /* DRIVERS_DANSER_SSI_SPEED_SENSOR_H_ */
+#endif /* DRIVERS_SSI_COMM_SPEED_SENSOR_H_ */
diff --git a/Software/Embedded_SW/Embedded/Main.c b/Software/Embedded_SW/Embedded/Main.c
index 05c03254a..662426d41 100644
--- a/Software/Embedded_SW/Embedded/Main.c
+++ b/Software/Embedded_SW/Embedded/Main.c
@@ -44,7 +44,7 @@
 extern Semaphore_Handle sdCardSem;
 
 
-#include "drivers/Danser_SSI/SSI_Comm.h"
+#include <Drivers/SSI_Comm/SSI_Comm.h>
 #include "drivers/SPI/SPI_Comm.h"
 #include "drivers/Uart_Comm/Uart.h"
 
diff --git a/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c b/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c
index 9337676e4..0ea10bafc 100644
--- a/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c
+++ b/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c
@@ -11,13 +11,13 @@
 
 #include "AlarmHandling.h"
 #include <driverlib/timer.h>
+#include <Drivers/SSI_Comm/SSI_Comm.h>
 #include <inc/hw_ints.h>
 
 #include "drivers/adc_sampling/adc.h"
 #include "Control/control.h"
 
 #include "drivers/Motors/Motor.h"
-#include "drivers/Danser_SSI/SSI_Comm.h"
 #include "drivers/Heater/TemperatureSensor.h"
 #include "drivers/FPGA/FPGA_SPI_Comm.h"
 #include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h"
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
index 8243c5faa..64ac4789d 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
@@ -20,13 +20,13 @@
 
 #include "MillisecTask.h"
 #include <driverlib/timer.h>
+#include <Drivers/SSI_Comm/SSI_Comm.h>
 #include <inc/hw_ints.h>
 
 #include "drivers/adc_sampling/adc.h"
 #include "control.h"
 
 #include "drivers/Motors/Motor.h"
-#include "drivers/Danser_SSI/SSI_Comm.h"
 #include "drivers/Heater/TemperatureSensor.h"
 #include "drivers/FPGA/FPGA_SPI_Comm.h"
 #include "drivers/FPGA/FPGA.h"
diff --git a/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c b/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c
index ead95388a..a163bd18f 100644
--- a/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c
+++ b/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c
@@ -13,9 +13,9 @@
 #include "Control/control.h"
 
 #include <Container.h>
+#include <Drivers/SSI_Comm/SSI_Comm.h>
 
 #include "drivers/Motors/Motor.h"
-#include "drivers/Danser_SSI/SSI_Comm.h"
 #include "drivers/Heater/TemperatureSensor.h"
 #include "drivers/FPGA/FPGA_SPI_Comm.h"
 #include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h"
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
index 3a0b25ba8..d37015b61 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
@@ -2,6 +2,7 @@
 #ifndef MODULES_THREAD_THREAD_H_
 #define MODULES_THREAD_THREAD_H_
 
+#include "../../Drivers/SSI_Comm/SSI_Comm.h"
 #include "PMR/common/MessageContainer.pb-c.h"
 #include "PMR/Hardware/HardwareMotor.pb-c.h"
 #include "PMR/Hardware/HardwareMotorType.pb-c.h"
@@ -13,8 +14,6 @@
 #include "PMR/Printing/JobSpoolType.pb-c.h"
 
 #include "drivers/Motors/Motor.h"
-#include "drivers/Danser_SSI/ssi_comm.h"
-
 #include "thread_ex.h"
 
 typedef struct
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index 75d54ef02..af9be2cc7 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -20,7 +20,8 @@
 #include "StateMachines/Printing/PrintingSTM.h"
 
 #include "drivers/Motors/Motor.h"
-#include "drivers/Danser_SSI/ssi_comm.h"
+//#include "drivers/SSI_Comm/ssi_comm.h"
+#include "drivers/SSI_Comm/Dancer/Dancer.h"
 #include "drivers/Heater/TemperatureSensor.h"
 #include "drivers/Heater/Heater.h"
 #include "drivers/Motors/Motor.h"