kinda got angle sensors working

2_Motor_Master/Motor_Master/Motor_Master/.atmelstart/atmel_start_config.atstart

@@ -1904,7 +1904,7 @@ drivers:
       spi_master_arch_dord: MSB first
       spi_master_arch_ibon: In data stream
       spi_master_arch_runstdby: false
-      spi_master_baud_rate: 50000
+      spi_master_baud_rate: 8000000
       spi_master_character_size: 8 bits
       spi_master_dummybyte: 511
       spi_master_rx_enable: true
@@ -2569,9 +2569,10 @@ pads:
   SPI3_SS:
     name: PB00
     definition: Atmel:SAME51_Drivers:0.0.1::SAME51J19A-MF::pad::PB00
-    mode: Peripheral IO
+    mode: Digital output
     user_label: SPI3_SS
-    configuration: null
+    configuration:
+      pad_initial_level: High
   SPI3_MISO:
     name: PB01
     definition: Atmel:SAME51_Drivers:0.0.1::SAME51J19A-MF::pad::PB01

2_Motor_Master/Motor_Master/Motor_Master/Config/hpl_sercom_config.h

@@ -377,7 +377,7 @@
 // <i> The SPI data transfer rate
 // <id> spi_master_baud_rate
 #ifndef CONF_SERCOM_5_SPI_BAUD
-#define CONF_SERCOM_5_SPI_BAUD 50000
+#define CONF_SERCOM_5_SPI_BAUD 8000000
 #endif
 
 // </h>

2_Motor_Master/Motor_Master/Motor_Master/Motor_Master.cproj

@@ -151,7 +151,7 @@
           <AcmeProjectActionInfo Action="File" Source="hri/hri_usb_e51.h" IsConfig="false" Hash="x6M7vYgNCS2oECqykr5+yw" />
           <AcmeProjectActionInfo Action="File" Source="hri/hri_wdt_e51.h" IsConfig="false" Hash="o9Rg/hyuMzwOCphVc7uG1w" />
           <AcmeProjectActionInfo Action="File" Source="main.c" IsConfig="false" Hash="k0AH7j+BrmdFhBPzCCMptA" />
-          <AcmeProjectActionInfo Action="File" Source="driver_init.c" IsConfig="false" Hash="EY0TWjVpAZ1peiFc/X6XZQ" />
+          <AcmeProjectActionInfo Action="File" Source="driver_init.c" IsConfig="false" Hash="HtuRIhj3ked378s6gT3GCA" />
           <AcmeProjectActionInfo Action="File" Source="driver_init.h" IsConfig="false" Hash="9Vb0j3AjI95MGRCecrCnLQ" />
           <AcmeProjectActionInfo Action="File" Source="atmel_start_pins.h" IsConfig="false" Hash="ByCGTBpkOpAk+zk9txhJSA" />
           <AcmeProjectActionInfo Action="File" Source="examples/driver_examples.h" IsConfig="false" Hash="gLjWgjni3Z8tNpbOMxpapQ" />
@@ -221,7 +221,7 @@
           <AcmeProjectActionInfo Action="File" Source="config/hpl_oscctrl_config.h" IsConfig="true" Hash="Uje5LXAS+nQpGryt9t0fYA" />
           <AcmeProjectActionInfo Action="File" Source="config/hpl_port_config.h" IsConfig="true" Hash="rMTNR+5FXtu+wfT1NbfRRA" />
           <AcmeProjectActionInfo Action="File" Source="config/hpl_qspi_config.h" IsConfig="true" Hash="CwZ360eeEYs7T9SYFSvDug" />
-          <AcmeProjectActionInfo Action="File" Source="config/hpl_sercom_config.h" IsConfig="true" Hash="x9H2b9FAETnTpR8cbaWLOg" />
+          <AcmeProjectActionInfo Action="File" Source="config/hpl_sercom_config.h" IsConfig="true" Hash="VqQgNrcYjhppN3Wx5EeLwg" />
           <AcmeProjectActionInfo Action="File" Source="config/hpl_tc_config.h" IsConfig="true" Hash="T93Kr6C+WDuufZob89oPeg" />
           <AcmeProjectActionInfo Action="File" Source="config/hpl_tcc_config.h" IsConfig="true" Hash="2LU7afZ/3Yx7FE2KzF9dSQ" />
           <AcmeProjectActionInfo Action="File" Source="config/peripheral_clk_config.h" IsConfig="true" Hash="rqTY1slZEq9V5moV+8Q+hw" />
@@ -419,7 +419,7 @@
   <armgcc.compiler.optimization.PrepareFunctionsForGarbageCollection>True</armgcc.compiler.optimization.PrepareFunctionsForGarbageCollection>
   <armgcc.compiler.optimization.DebugLevel>Maximum (-g3)</armgcc.compiler.optimization.DebugLevel>
   <armgcc.compiler.warnings.AllWarnings>True</armgcc.compiler.warnings.AllWarnings>
-  <armgcc.compiler.miscellaneous.OtherFlags>-std=gnu99 -mfloat-abi=hard -mfpu=fpv4-sp-d16</armgcc.compiler.miscellaneous.OtherFlags>
+  <armgcc.compiler.miscellaneous.OtherFlags>-std=gnu11 -mfloat-abi=hard -mfpu=fpv4-sp-d16</armgcc.compiler.miscellaneous.OtherFlags>
   <armgcc.linker.general.UseNewlibNano>True</armgcc.linker.general.UseNewlibNano>
   <armgcc.linker.libraries.Libraries>
     <ListValues>
@@ -503,6 +503,12 @@
     </ToolchainSettings>
   </PropertyGroup>
   <ItemGroup>
+    <Compile Include="angle_sensors.c">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="angle_sensors.h">
+      <SubType>compile</SubType>
+    </Compile>
     <Compile Include="arm_math.h">
       <SubType>compile</SubType>
     </Compile>

2_Motor_Master/Motor_Master/Motor_Master/angle_sensors.c

@@ -0,0 +1,130 @@
+
+/*
+ * angle_sensors.c
+ *
+ * Created: 8/21/2021 11:34:44 AM
+ *  Author: ge37vez
+ */ 
+
+#include "angle_sensors.h"
+#define	AS5048A_MAX_VALUE					   (8191.0f)
+#include "arm_math.h"
+
+void angle_sensor_init()
+{
+	spi_m_sync_disable(ANGLESENSOR.SPI_descr);
+	// Set Mode 1 : CPOL = 0; CPHA = 1;
+	
+	#ifdef AS5048
+	/* Set Mode 1 */
+	hri_sercomspi_write_CTRLA_CPOL_bit((SercomSpi *)(ANGLESENSOR.SPI_descr->dev.prvt), false);
+	hri_sercomspi_write_CTRLA_CPHA_bit((SercomSpi *)(ANGLESENSOR.SPI_descr->dev.prvt), true);
+	#elif defined A1335
+	/* Set Mode 3 */
+	hri_sercomspi_write_CTRLA_CPOL_bit((SercomSpi *)(ANGLESENSOR.SPI_descr->dev.prvt), true);
+	hri_sercomspi_write_CTRLA_CPHA_bit((SercomSpi *)(ANGLESENSOR.SPI_descr->dev.prvt), true);	
+	#endif 
+	
+	spi_m_sync_enable(ANGLESENSOR.SPI_descr);
+	gpio_set_pin_level(ANGLESENSOR.SS_pin, true);
+}
+
+//void read(ASCommand command)
+//{
+	//_read(command); // Send command to device(s)
+	//_read(AS_CMD_NOP); // Read-out device(s)
+//}
+
+int16_t* read_angle()
+{
+	_read(AS_CMD_ANGLE); // Send command to device(s)
+	_read(AS_CMD_ANGLE); // Read-out device(s)
+	return &ANGLESENSOR._readBuffer;
+}
+
+void _read(ASCommand command)
+{
+	
+	if(ANGLESENSOR.n_dev == 1)
+	{
+		// Give command to start reading the angle
+		gpio_set_pin_level(ANGLESENSOR.SS_pin, false);
+		uint16_t temp = _spi_transfer16(&ANGLESENSOR, command);
+		ANGLESENSOR._readBuffer[0] = (temp & AS_MASK);
+		gpio_set_pin_level(ANGLESENSOR.SS_pin, true);
+		//delay_us(1); // Wait at least 350ns after chip select
+	} else {
+		// Enable the sensor on the chain
+		gpio_set_pin_level(ANGLESENSOR.SS_pin, false);
+		//delay_us(1); // Wait at least 350ns after chip select
+		for(int i = 0; i < ANGLESENSOR.n_dev; ++i)
+		{
+			uint16_t temp = _spi_transfer16(&ANGLESENSOR, command);
+			ANGLESENSOR._readBuffer[i] = (temp & AS_MASK);
+		}
+		gpio_set_pin_level(ANGLESENSOR.SS_pin, true);
+		//delay_us(1); // Wait at least 350ns after chip select
+	}
+}
+
+
+bool parity_check(int16_t sensor_result)
+{
+	// Use the LSb of result to keep track of parity (0 = even, 1 = odd)
+	int16_t result = sensor_result;
+	    
+	for(int i = 1; i <= 15; ++i) {
+		sensor_result >>= 1;
+		result ^= sensor_result;
+	}
+	// Parity should be even
+	return (result & 0x0001) == 0;
+}
+
+int16_t degrees(int16_t sensor_result)
+{
+	uint16_t data;
+	int16_t rotation;
+	
+	#ifdef AS5048
+	
+	rotation = (int16_t)(sensor_result); //- static_cast<int16_t>(this->position);
+	if (rotation > AS5048A_MAX_VALUE) {
+		rotation = -((0x3FFF) - rotation); //more than -180
+	}
+		
+	float angleDegrees = 360.0 * ((rotation) + AS5048A_MAX_VALUE) / (AS5048A_MAX_VALUE * 2.0);
+	//float angleDegrees = 360.0 * ((sensor_result &= 0x3FFF) + AS5048A_MAX_VALUE) / (AS5048A_MAX_VALUE * 2.0);
+	//float32_t angleDegrees = 360.0 * (rotation + AS5048A_MAX_VALUE) * 0.000061042607;
+	
+	return (int16_t)(angleDegrees * 10 + .5);
+	
+	#elif defined A1335
+	float angleDegrees = ((sensor_result & AS_MASK) * 360.0 / 4096.0);
+	
+	//int16_t angle_deg = (sensor_result &= 0x3FFF) * 36000 / 0x4000;
+	//return angle_deg;
+	return (int16_t)(angleDegrees * 10 + .5);
+	#endif
+}
+
+int16_t mask(int16_t sensor_results)
+{
+	return (sensor_results &= 0x3FFF);
+}
+
+uint16_t _spi_transfer16(struct spi_m_sync_descriptor *spi, uint16_t command)
+{
+	uint8_t in_buf[2], out_buf[2];
+	uint16_t wordRead;
+	
+	out_buf[1] = command & 0xFF;
+	out_buf[0] = ( command >> 8 ) & 0xFF;
+	struct spi_xfer  xfer;
+	xfer.rxbuf = in_buf;
+	xfer.txbuf = (uint8_t *)out_buf;
+	xfer.size  = 2;
+	spi_m_sync_transfer(&SPI_3, &xfer);
+	wordRead = (uint16_t)((in_buf[0] << 8) | in_buf[1]);
+	return (wordRead);
+}

2_Motor_Master/Motor_Master/Motor_Master/angle_sensors.h

@@ -0,0 +1,110 @@
+/*
+ * angle_sensors.h
+ *
+ * Created: 8/21/2021 11:16:13 AM
+ *  Author: ge37vez
+ */ 
+
+
+#ifndef ANGLE_SENSORS_H_
+#define ANGLE_SENSORS_H_
+
+#include "atmel_start.h"
+
+//Define Sensor type (One one at at time supported)
+//#define AS5048
+#define A1335
+
+// ----------------------------------------------------------------------
+// AS5048 Registers & Commands
+// ----------------------------------------------------------------------
+#ifdef AS5048
+
+typedef enum {
+	AS_FLAG_PARITY = 0x8000,
+	AS_FLAG_READ = 0x4000,
+	AS_MASK = 0x3FFF,
+} ASFlag;
+
+typedef enum {
+	AS_CMD_NOP = 0x0000,
+	AS_CMD_ERROR = 0x0001 | AS_FLAG_READ,       // Reads error register of sensor and clear error flags
+	AS_CMD_DIAGNOSTICS = 0x3FFD |  AS_FLAG_READ, // Reads automatic gain control and diagnostics info
+	AS_CMD_MAGNITUDE = 0x3FFE | AS_FLAG_READ,
+	AS_CMD_ANGLE = 0x3FFF| AS_FLAG_PARITY | AS_FLAG_READ,
+} ASCommand;
+
+// Masks for bits in the result of the AS_CMD_DIAGNOSTICS command
+typedef enum {
+	AS_DIAG_CORDIC_OVERFLOW = 0x0200,
+	AS_DIAG_HIGH_MAGNETIC = 0x0400,
+	AS_DIAG_LOW_MAGNETIC = 0x0800,
+} ASDiagnostics;
+
+// ----------------------------------------------------------------------
+// A1335 Registers & Commands
+// ----------------------------------------------------------------------
+#elif defined A1335
+
+typedef enum {
+	AS_FLAG_PARITY = 0x8000,
+	AS_FLAG_READ = 0x00,
+	AS_ADDRESS_MASK = 0x3F,
+	AS_MASK = 0x0FFF,
+} ASFlag;
+
+typedef enum {
+	AS_CMD_NOP = 0x0000,
+	AS_CMD_ERROR = 0x0001 | AS_FLAG_READ,       // Reads error register of sensor and clear error flags
+	AS_CMD_DIAGNOSTICS = 0x3FFD |  AS_FLAG_READ, // Reads automatic gain control and diagnostics info
+	AS_CMD_MAGNITUDE = 0x3FFE | AS_FLAG_READ,
+	AS_CMD_ANGLE = (uint16_t)(((0x20 & AS_ADDRESS_MASK) | AS_FLAG_READ) << 8),
+} ASCommand;
+
+// Masks for bits in the result of the AS_CMD_DIAGNOSTICS command
+typedef enum {
+	AS_DIAG_CORDIC_OVERFLOW = 0x0200,
+	AS_DIAG_HIGH_MAGNETIC = 0x0400,
+	AS_DIAG_LOW_MAGNETIC = 0x0800,
+} ASDiagnostics;
+
+#endif
+
+/* Struct Definitions */
+struct SPI_ANGLE_SENSOR {
+	struct spi_m_sync_descriptor *SPI_descr;
+	uint8_t flags;
+	uint32_t SS_pin;
+	uint8_t n_dev;
+	int16_t _readBuffer[2]; // Must Equal n_dev
+};
+
+static struct SPI_ANGLE_SENSOR ANGLESENSOR = {
+	.SPI_descr = &SPI_3,
+	.flags = 0,
+	.SS_pin = SPI3_SS,
+	.n_dev = 2,
+	._readBuffer = {0} // Must Equal n_dev
+};
+
+void angle_sensor_init();
+void read(ASCommand command);
+// Performs a single angle measurement on all sensors
+// @return Array of raw angle data. To get the 14-bit value representing
+//     the angle, apply the mask() to the result.
+int16_t* read_angle();
+// Applies the mask to the first n bytes in the read buffer (for daisychained sensors).
+static int16_t mask(int16_t sensor_results);
+// Checks if the return value from the sensor has the right parity
+// @return true if ok
+static bool parity_check(int16_t sensor_result);
+// Returns an angle from 0 to 36000 (degrees times 100).
+// @param sensor_result is one of the values returned by read_angle or read_angle_sequential
+int16_t degrees(int16_t sensor_result);
+    
+// Intended to be used Internally
+void _read(ASCommand command);	
+uint16_t _spi_transfer16(struct spi_m_sync_descriptor *spi, uint16_t value);
+	
+
+#endif /* ANGLE_SENSORS_H_ */

2_Motor_Master/Motor_Master/Motor_Master/bldc.c

@@ -35,7 +35,7 @@ void motor_StateMachine(BLDCMotor_t* const motor)
 		case  MOTOR_IDLE:
 			//hri_tcc_write_PATTBUF_reg(motor->motor_param->pwm_desc->device.hw,  DISABLE_PATTERN);
 			motor->motor_state.previousstate = motor->motor_state.currentstate;
-			motor->motor_state.currentstate = MOTOR_PVI_CTRL_STATE;
+			motor->motor_state.currentstate = MOTOR_IDLE;
 		break;
 		case MOTOR_OPEN_LOOP_STATE:
 			BLDC_runOpenLoop(motor, *M1_Desired_dc);

2_Motor_Master/Motor_Master/Motor_Master/driver_init.c

@@ -713,6 +713,16 @@ void system_init(void)
 
 	// GPIO on PB00
 
+	gpio_set_pin_level(SPI3_SS,
+	                   // <y> Initial level
+	                   // <id> pad_initial_level
+	                   // <false"> Low
+	                   // <true"> High
+	                   true);
+
+	// Set pin direction to output
+	gpio_set_pin_direction(SPI3_SS, GPIO_DIRECTION_OUT);
+
 	gpio_set_pin_function(SPI3_SS, GPIO_PIN_FUNCTION_OFF);
 
 	// GPIO on PB22

2_Motor_Master/Motor_Master/Motor_Master/main.c

@@ -12,7 +12,7 @@
 #include "interrupts.h"
 #include "statemachine.h"
 
-
+#include "angle_sensors.h"
 
 
 void process_currents()
@@ -54,7 +54,7 @@ static struct timer_task Onems_task;
 
 void One_ms_timer_init(void)
 {
-	Onems_task.interval = 1;
+	Onems_task.interval = 10;
 	Onems_task.cb       = One_ms_cycle_callback;
 	Onems_task.mode     = TIMER_TASK_REPEAT;
 	timer_add_task(&TIMER_0, &Onems_task);
@@ -174,6 +174,9 @@ int main(void)
 	One_ms_timer_init();
 	custom_logic_enable();
 	//speed_timer_init();
+	
+	angle_sensor_init();
+		
 	enable_NVIC_IRQ();
 	
 	/* Replace with your application code */
@@ -187,6 +190,14 @@ int main(void)
 			_dma_enable_transaction(DMAC_CHANNEL_CONF_SERCOM_1_RECEIVE, false);
 			_dma_enable_transaction(DMAC_CHANNEL_CONF_SERCOM_1_TRANSMIT, false);
 			volatile int x = 1;
+			
+			
+			int16_t* angles;
+			angles = read_angle();
+			*M1_Joint_abs_position = degrees(angles[0]);
+			*M2_Joint_abs_position = degrees(angles[1]);
+			volatile int y = 0;
+		
 			//spi_m_dma_transfer(&SPI_1_MSIF, (uint8_t*)Slave_1.tx_buffer, (uint8_t*)Slave_1.rx_buffer, MASTER_BUFFER_SIZE);
 		}
 		

2_Motor_Slave/Motor_Slave/Motor_Slave/AS5048.c

@@ -7,16 +7,7 @@
 
 #include "AS5048.h"
 
-#define AS5048A_CMD_NOP						  (0x0000)
+
-#define AS5048A_READ						  (0x4000)
-#define AS5048A_CLEAR_ERROR_FLAG			  (0x0001)
-#define AS5048A_PROGRAMMING_CONTROL           (0x0003)
-#define AS5048A_OTP_REGISTER_ZERO_POS_HIGH    (0x0016)
-#define AS5048A_OTP_REGISTER_ZERO_POS_LOW     (0x0017)
-#define AS5048A_DIAG_AGC                      (0x3FFD)
-#define AS5048A_MAGNITUDE                     (0x3FFE)
-#define AS5048A_ANGLE                         (0x3FFF)
-#define AS5048A_ANGLEUNC					  (0x3FFE)
 
 
 #define	AS5048A_MAX_VALUE					   (8191.0f)
@@ -73,6 +64,30 @@ void as5048a_init(struct spi_m_sync_descriptor *spi, struct SPI_AS5048 *Sn)
 	}
 }
 
+void as5048a_read_dual(struct SPI_AS5048 *Sn, uint16_t address, int16_t* buff)
+{
+	uint16_t returnedVal[2] = {0};
+	uint8_t in_buf[2], out_buf[2];
+	volatile uint16_t command = 0b0100000000000000; // PAR=0 R/W=R
+	command = command | address;
+
+	//Add a parity bit on the the MSB
+	command |= ((uint16_t)as5048a_CalcEvenParity(command)<<15);
+	
+	/* Send Command */
+	gpio_set_pin_level(Sn->SS_pin, false);
+	as5048a_transfer16(Sn, command);
+	as5048a_transfer16(Sn, command);
+	gpio_set_pin_level(Sn->SS_pin, true);
+	
+	///* Send again to Get Response */
+	gpio_set_pin_level(Sn->SS_pin, false);
+	buff[0] = as5048a_transfer16(Sn, command);
+	buff[1] = as5048a_transfer16(Sn, command);
+	gpio_set_pin_level(Sn->SS_pin, true);
+	//return	returnedVal;
+}
+
 uint16_t as5048a_read(struct SPI_AS5048 *Sn, uint16_t address)
 {
 	uint8_t in_buf[2], out_buf[2];
@@ -93,7 +108,7 @@ uint16_t as5048a_read(struct SPI_AS5048 *Sn, uint16_t address)
 	as5048a_transfer16(Sn, command);
 	gpio_set_pin_level(Sn->SS_pin, true);
 	
-	/* Send again to Get Response */
+	///* Send again to Get Response */
 	gpio_set_pin_level(Sn->SS_pin, false);
 	returnedVal = as5048a_transfer16(Sn, command);
 	gpio_set_pin_level(Sn->SS_pin, true);

2_Motor_Slave/Motor_Slave/Motor_Slave/AS5048.h

@@ -10,6 +10,17 @@
 #include <atmel_start.h>
 #include "arm_math.h"
 
+#define AS5048A_CMD_NOP						  (0x0000)
+#define AS5048A_READ						  (0x4000)
+#define AS5048A_CLEAR_ERROR_FLAG			  (0x0001)
+#define AS5048A_PROGRAMMING_CONTROL           (0x0003)
+#define AS5048A_OTP_REGISTER_ZERO_POS_HIGH    (0x0016)
+#define AS5048A_OTP_REGISTER_ZERO_POS_LOW     (0x0017)
+#define AS5048A_DIAG_AGC                      (0x3FFD)
+#define AS5048A_MAGNITUDE                     (0x3FFE)
+#define AS5048A_ANGLE                         (0x3FFF)
+#define AS5048A_ANGLEUNC					  (0x3FFE)
+
 
 struct SPI_AS5048 {
 	struct spi_m_sync_descriptor *SPI_descr;

2_Motor_Slave/Motor_Slave/Motor_Slave/Motor_Slave.cproj

@@ -419,7 +419,7 @@
   <armgcc.compiler.optimization.PrepareFunctionsForGarbageCollection>True</armgcc.compiler.optimization.PrepareFunctionsForGarbageCollection>
   <armgcc.compiler.optimization.DebugLevel>Maximum (-g3)</armgcc.compiler.optimization.DebugLevel>
   <armgcc.compiler.warnings.AllWarnings>True</armgcc.compiler.warnings.AllWarnings>
-  <armgcc.compiler.miscellaneous.OtherFlags>-std=gnu99 -mfloat-abi=hard -mfpu=fpv4-sp-d16</armgcc.compiler.miscellaneous.OtherFlags>
+  <armgcc.compiler.miscellaneous.OtherFlags>-std=gnu11 -mfloat-abi=hard -mfpu=fpv4-sp-d16</armgcc.compiler.miscellaneous.OtherFlags>
   <armgcc.linker.general.UseNewlibNano>True</armgcc.linker.general.UseNewlibNano>
   <armgcc.linker.libraries.Libraries>
     <ListValues>
@@ -503,6 +503,12 @@
     </ToolchainSettings>
   </PropertyGroup>
   <ItemGroup>
+    <Compile Include="angle_sensors.c">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="angle_sensors.h">
+      <SubType>compile</SubType>
+    </Compile>
     <Compile Include="arm_math.h">
       <SubType>compile</SubType>
     </Compile>

2_Motor_Slave/Motor_Slave/Motor_Slave/angle_sensors.c

@@ -0,0 +1,125 @@
+
+/*
+ * angle_sensors.c
+ *
+ * Created: 8/21/2021 11:34:44 AM
+ *  Author: ge37vez
+ */ 
+
+#include "angle_sensors.h"
+#define	AS5048A_MAX_VALUE					   (8191.0f)
+#include "arm_math.h"
+
+void angle_sensor_init()
+{
+	spi_m_sync_disable(ANGLESENSOR.SPI_descr);
+	// Set Mode 1 : CPOL = 0; CPHA = 1;
+	
+	#ifdef AS5048
+	/* Set Mode 1 */
+	hri_sercomspi_write_CTRLA_CPOL_bit((SercomSpi *)(ANGLESENSOR.SPI_descr->dev.prvt), false);
+	hri_sercomspi_write_CTRLA_CPHA_bit((SercomSpi *)(ANGLESENSOR.SPI_descr->dev.prvt), true);
+	#elif defined A1335
+	/* Set Mode 3 */
+	hri_sercomspi_write_CTRLA_CPOL_bit((SercomSpi *)(ANGLESENSOR.SPI_descr->dev.prvt), true);
+	hri_sercomspi_write_CTRLA_CPHA_bit((SercomSpi *)(ANGLESENSOR.SPI_descr->dev.prvt), true);	
+	#endif 
+	
+	spi_m_sync_enable(ANGLESENSOR.SPI_descr);
+	gpio_set_pin_level(ANGLESENSOR.SS_pin, true);
+}
+
+void read(ASCommand command)
+{
+	_read(command); // Send command to device(s)
+	_read(AS_CMD_NOP); // Read-out device(s)
+}
+
+int16_t* read_angle()
+{
+	_read(AS_CMD_ANGLE); // Send command to device(s)
+	_read(AS_CMD_ANGLE); // Read-out device(s)
+	return &ANGLESENSOR._readBuffer;
+}
+
+void _read(ASCommand command)
+{
+	
+	if(ANGLESENSOR.n_dev == 1)
+	{
+		// Give command to start reading the angle
+		gpio_set_pin_level(ANGLESENSOR.SS_pin, false);
+		uint16_t temp = _spi_transfer16(&ANGLESENSOR, command);
+		ANGLESENSOR._readBuffer[0] = (temp & ~0xC000);
+		gpio_set_pin_level(ANGLESENSOR.SS_pin, true);
+		//delay_us(1); // Wait at least 350ns after chip select
+	} else {
+		// Enable the sensor on the chain
+		gpio_set_pin_level(ANGLESENSOR.SS_pin, false);
+		//delay_us(1); // Wait at least 350ns after chip select
+		for(int i = 0; i < ANGLESENSOR.n_dev; ++i)
+		{
+			uint16_t temp = _spi_transfer16(&ANGLESENSOR, command);
+			ANGLESENSOR._readBuffer[i] = (temp & ~0xC000);
+		}
+		gpio_set_pin_level(ANGLESENSOR.SS_pin, true);
+		//delay_us(1); // Wait at least 350ns after chip select
+	}
+}
+
+
+bool parity_check(int16_t sensor_result)
+{
+	// Use the LSb of result to keep track of parity (0 = even, 1 = odd)
+	int16_t result = sensor_result;
+	    
+	for(int i = 1; i <= 15; ++i) {
+		sensor_result >>= 1;
+		result ^= sensor_result;
+	}
+	// Parity should be even
+	return (result & 0x0001) == 0;
+}
+
+int16_t degrees(int16_t sensor_result)
+{
+	uint16_t data;
+	int16_t rotation;
+	
+	rotation = (int16_t)(sensor_result); //- static_cast<int16_t>(this->position);
+	if (rotation > AS5048A_MAX_VALUE) {
+		rotation = -((0x3FFF) - rotation); //more than -180
+	}
+		
+	float angleDegrees = 360.0 * ((rotation) + AS5048A_MAX_VALUE) / (AS5048A_MAX_VALUE * 2.0);
+	//float angleDegrees = 360.0 * ((sensor_result &= 0x3FFF) + AS5048A_MAX_VALUE) / (AS5048A_MAX_VALUE * 2.0);
+	//float32_t angleDegrees = 360.0 * (rotation + AS5048A_MAX_VALUE) * 0.000061042607;
+	
+	return (int16_t)(angleDegrees * 10 + .5);
+	
+	
+	
+	//int16_t angle_deg = (sensor_result &= 0x3FFF) * 36000 / 0x4000;
+	//return angle_deg;
+}
+
+int16_t mask(int16_t sensor_results)
+{
+	return (sensor_results &= 0x3FFF);
+}
+
+uint16_t _spi_transfer16(struct spi_m_sync_descriptor *spi, uint16_t command)
+{
+	uint8_t in_buf[2], out_buf[2];
+	uint16_t wordRead;
+	
+	out_buf[1] = command & 0xFF;
+	out_buf[0] = ( command >> 8 ) & 0xFF;
+	struct spi_xfer  xfer;
+	xfer.rxbuf = in_buf;
+	xfer.txbuf = (uint8_t *)out_buf;
+	xfer.size  = 2;
+	spi_m_sync_transfer(&SPI_3, &xfer);
+	wordRead = (uint16_t)((in_buf[0] << 8) | in_buf[1]);
+	return (wordRead);
+}

2_Motor_Slave/Motor_Slave/Motor_Slave/angle_sensors.h

@@ -0,0 +1,110 @@
+/*
+ * angle_sensors.h
+ *
+ * Created: 8/21/2021 11:16:13 AM
+ *  Author: ge37vez
+ */ 
+
+
+#ifndef ANGLE_SENSORS_H_
+#define ANGLE_SENSORS_H_
+
+#include "atmel_start.h"
+
+//Define Sensor type (One one at at time supported)
+#define AS5048
+//#define A1335
+
+// ----------------------------------------------------------------------
+// AS5048 Registers & Commands
+// ----------------------------------------------------------------------
+#ifdef AS5048
+
+typedef enum {
+	AS_FLAG_PARITY = 0x8000,
+	AS_FLAG_READ = 0x4000,
+	AS_MASK = 0x3FFF,
+} ASFlag;
+
+typedef enum {
+	AS_CMD_NOP = 0x0000,
+	AS_CMD_ERROR = 0x0001 | AS_FLAG_READ,       // Reads error register of sensor and clear error flags
+	AS_CMD_DIAGNOSTICS = 0x3FFD |  AS_FLAG_READ, // Reads automatic gain control and diagnostics info
+	AS_CMD_MAGNITUDE = 0x3FFE | AS_FLAG_READ,
+	AS_CMD_ANGLE = 0x3FFF| AS_FLAG_PARITY | AS_FLAG_READ,
+} ASCommand;
+
+// Masks for bits in the result of the AS_CMD_DIAGNOSTICS command
+typedef enum {
+	AS_DIAG_CORDIC_OVERFLOW = 0x0200,
+	AS_DIAG_HIGH_MAGNETIC = 0x0400,
+	AS_DIAG_LOW_MAGNETIC = 0x0800,
+} ASDiagnostics;
+
+// ----------------------------------------------------------------------
+// A1335 Registers & Commands
+// ----------------------------------------------------------------------
+#elif defined A1335
+
+typedef enum {
+	AS_FLAG_PARITY = 0x8000,
+	AS_FLAG_READ = 0x00,
+	AS_ADDRESS_MASK = 0x3F,
+	AS_MASK = 0x0FFF,
+} ASFlag;
+
+typedef enum {
+	AS_CMD_NOP = 0x0000,
+	AS_CMD_ERROR = 0x0001 | AS_FLAG_READ,       // Reads error register of sensor and clear error flags
+	AS_CMD_DIAGNOSTICS = 0x3FFD |  AS_FLAG_READ, // Reads automatic gain control and diagnostics info
+	AS_CMD_MAGNITUDE = 0x3FFE | AS_FLAG_READ,
+	AS_CMD_ANGLE = (uint16_t)(((0x20 & AS_ADDRESS_MASK) | AS_FLAG_READ) << 8),
+} ASCommand;
+
+// Masks for bits in the result of the AS_CMD_DIAGNOSTICS command
+typedef enum {
+	AS_DIAG_CORDIC_OVERFLOW = 0x0200,
+	AS_DIAG_HIGH_MAGNETIC = 0x0400,
+	AS_DIAG_LOW_MAGNETIC = 0x0800,
+} ASDiagnostics;
+
+#endif
+
+/* Struct Definitions */
+struct SPI_ANGLE_SENSOR {
+	struct spi_m_sync_descriptor *SPI_descr;
+	uint8_t flags;
+	uint32_t SS_pin;
+	uint8_t n_dev;
+	int16_t _readBuffer[2]; // Must Equal n_dev
+};
+
+static struct SPI_ANGLE_SENSOR ANGLESENSOR = {
+	.SPI_descr = &SPI_3,
+	.flags = 0,
+	.SS_pin = SPI3_SS,
+	.n_dev = 1,
+	._readBuffer = {0} // Must Equal n_dev
+};
+
+void angle_sensor_init();
+void read(ASCommand command);
+// Performs a single angle measurement on all sensors
+// @return Array of raw angle data. To get the 14-bit value representing
+//     the angle, apply the mask() to the result.
+int16_t* read_angle();
+// Applies the mask to the first n bytes in the read buffer (for daisychained sensors).
+static int16_t mask(int16_t sensor_results);
+// Checks if the return value from the sensor has the right parity
+// @return true if ok
+static bool parity_check(int16_t sensor_result);
+// Returns an angle from 0 to 36000 (degrees times 100).
+// @param sensor_result is one of the values returned by read_angle or read_angle_sequential
+int16_t degrees(int16_t sensor_result);
+    
+// Intended to be used Internally
+void _read(ASCommand command);	
+uint16_t _spi_transfer16(struct spi_m_sync_descriptor *spi, uint16_t value);
+	
+
+#endif /* ANGLE_SENSORS_H_ */

2_Motor_Slave/Motor_Slave/Motor_Slave/main.c

@@ -11,7 +11,8 @@
 #include "interrupts.h"
 #include "statemachine.h"
 #include "BNO055_imu.h"
-#include "AS5048.h"
+//#include "AS5048.h"
+#include "angle_sensors.h"
 
 void process_currents()
 {
@@ -110,6 +111,12 @@ void SERCOM1_3_Handler()
 	//_dma_enable_transaction(CONF_SERCOM_5_TRANSMIT_DMA_CHANNEL, false);
 }
 
+//void SERCOM5_0_Handler()
+//{		
+	//SERCOM5->SPI.INTFLAG.bit.DRE = 0x01;
+	//volatile int x = 1;
+//}
+
 void enable_NVIC_IRQ(void)
 {
 	
@@ -123,6 +130,7 @@ void enable_NVIC_IRQ(void)
 	NVIC_SetPriority(ADC1_0_IRQn, 3);
 	NVIC_EnableIRQ(TCC0_0_IRQn);
 	NVIC_EnableIRQ(TCC1_0_IRQn);
+	//NVIC_EnableIRQ(SERCOM5_0_IRQn);
 	
 	//NVIC_EnableIRQ(SERCOM1_1_IRQn);
 	//NVIC_SetPriority(SERCOM1_1_IRQn, 1);
@@ -196,7 +204,7 @@ s16 accel_datay = BNO055_INIT_VALUE;
 s16 accel_dataz = BNO055_INIT_VALUE;
 
 
-
+volatile int16_t angles[2] = {0};
 
 int main(void)
 {
@@ -221,10 +229,13 @@ int main(void)
 	enable_NVIC_IRQ();
 	_dma_enable_transaction(CONF_SERCOM_1_RECEIVE_DMA_CHANNEL, false);
 	_dma_enable_transaction(CONF_SERCOM_1_TRANSMIT_DMA_CHANNEL, false);
-	as5048a_init(&SPI_3, &AS_1);
+	
+	//as5048a_init(&SPI_3, &AS_1);
 	
 	//ORIENTATION_SENSOR_0_init();
 	
+	angle_sensor_init();
+	
 	
 	/* Replace with your application code */
 	while (1) {
@@ -243,9 +254,20 @@ int main(void)
 			exec_commutation(&Motor1);
 			exec_commutation(&Motor2);
 			//*M3_Joint_abs_position = as5048a_getRawRotation(&AS_1);
-			*M3_Joint_abs_position = as5048a_getRotationDecInt(&AS_1);
+			//*M3_Joint_abs_position = as5048a_getRotationDecInt(&AS_1);
+			
+			//as5048a_read_dual(&AS_1, AS5048A_ANGLE, &angles);
+			//*M3_Joint_abs_position = angles[0];
+			//*M4_Joint_abs_position = angles[1];
+								
+			int16_t* angles;
+			angles = read_angle();
+			*M3_Joint_abs_position = degrees(angles[0]);
+			//*M4_Joint_abs_position = angles[1];
 			volatile int y = 0;
-
+			
+			
+			
 			/* Raw accel X, Y and Z data can read from the register page - page 0
 			register - 0x08 to 0x0D */
 			//bno055_read_accel_x(&accel_datax);

Examples/AS5048-mbed/.hg_archival.txt

@@ -0,0 +1,6 @@
+repo: 3edcf58e51e78b3cc029c791e3e7318cc6e2338f
+node: 06b89a41109e5e51f534eb5a8b0067fe07d0e7dc
+branch: default
+latesttag: null
+latesttagdistance: 5
+changessincelatesttag: 5

Examples/AS5048-mbed/as5048spi.cpp

@@ -0,0 +1,136 @@
+#include "as5048spi.h"
+
+As5048Spi::As5048Spi(PinName mosi, PinName miso, PinName sclk, PinName chipselect, int ndevices) :
+    _nDevices(ndevices),
+    _chipSelectN(chipselect),
+    _spi(mosi, miso, sclk)
+{
+    _chipSelectN.write(1);
+    // AS5048 needs 16-bits for is commands
+    // Mode = 1: 
+    //  clock polarity = 0 --> clock pulse is high
+    //  clock phase = 1 --> sample on falling edge of clock pulse
+    _spi.format(16, 1);
+    
+    // Set clock frequency to 1 MHz (max is 10Mhz)
+    _spi.frequency(1000000);
+    
+    _readBuffer = new int[ndevices];
+}
+
+As5048Spi::~As5048Spi()
+{
+    delete [] _readBuffer;
+}
+
+int As5048Spi::degrees(int sensor_result)
+{
+    return mask(sensor_result) * 36000 / 0x4000;
+}
+
+
+int As5048Spi::radian(int sensor_result)
+{
+    return mask(sensor_result) * 62832 / 0x4000;
+}
+
+bool As5048Spi::error(int device)
+{
+    if( device == -1 ) {
+        for(int i = 0; i < _nDevices; ++i) {
+            if( _readBuffer[i] & 0x4000 ) {
+                return true;
+            }
+        }
+    } else if( device < _nDevices ) {
+        return (_readBuffer[device] & 0x4000) == 0x4000;
+    }
+    return false; 
+}
+
+
+void As5048Spi::frequency(int hz) 
+{
+    _spi.frequency(hz);
+}
+
+int As5048Spi::mask(int sensor_result)
+{
+    return sensor_result & 0x3FFF; // return lowest 14-bits
+}
+
+
+void As5048Spi::mask(int* sensor_results, int n)
+{
+    for(int i = 0; i < n; ++i) {
+        sensor_results[i] &= 0x3FFF;
+    }
+}
+
+
+bool As5048Spi::parity_check(int sensor_result)
+{
+    // Use the LSb of result to keep track of parity (0 = even, 1 = odd)
+    int result = sensor_result;
+    
+    for(int i = 1; i <= 15; ++i) {
+        sensor_result >>= 1;
+        result ^= sensor_result;
+    }
+    // Parity should be even
+    return (result & 0x0001) == 0;
+}
+
+const int* As5048Spi::read(As5048Command command)
+{
+    _read(command); // Send command to device(s)
+    return _read(AS_CMD_NOP); // Read-out device(s)
+}
+
+const int*  As5048Spi::read_sequential(As5048Command command)
+{
+    return _read(command);
+}
+
+const int*  As5048Spi::read_angle()
+{  
+    _read(AS_CMD_ANGLE); // Send command to device(s)
+    return _read(AS_CMD_NOP); // Read-out device(s)
+}
+
+const int*  As5048Spi::read_angle_sequential()
+{
+    return _read(AS_CMD_ANGLE); 
+}
+
+
+int* As5048Spi::_read(As5048Command command)
+{
+    if(_nDevices == 1)
+    {
+        // Give command to start reading the angle
+        _chipSelectN.write(0);
+        wait_us(1); // Wait at least 350ns after chip select
+        _readBuffer[0] = _spi.write(command);
+        _chipSelectN.write(1);
+        wait_us(1); // Wait at least 350ns after chip select       
+    } else
+    {
+        // Enable the sensor on the chain
+        _chipSelectN.write(0);
+         wait_us(1); // Wait at least 350ns after chip select
+        for(int i = 0; i < _nDevices; ++i)
+        {
+            _readBuffer[i] = _spi.write(command);
+        }
+        _chipSelectN.write(1);
+        wait_us(1); // Wait at least 350ns after chip select
+    }
+    return _readBuffer;
+}
+
+
+
+
+
+    

Examples/AS5048-mbed/as5048spi.h

@@ -0,0 +1,86 @@
+#include "mbed.h"
+typedef enum {
+    AS_FLAG_PARITY = 0x8000,
+    AS_FLAG_READ = 0x4000,
+} As5048Flag;
+
+typedef enum {
+    AS_CMD_NOP = 0x0000,
+    AS_CMD_ERROR = 0x0001 | AS_FLAG_READ,       // Reads error register of sensor and clear error flags
+    AS_CMD_DIAGNOSTICS = 0x3FFD |  AS_FLAG_READ, // Reads automatic gain control and diagnostics info
+    AS_CMD_MAGNITUDE = 0x3FFE | AS_FLAG_READ,
+    
+    AS_CMD_ANGLE = 0x3FFF| AS_FLAG_PARITY | AS_FLAG_READ,
+} As5048Command;
+
+// Masks for bits in the result of the AS_CMD_DIAGNOSTICS command
+typedef enum {
+    AS_DIAG_CORDIC_OVERFLOW = 0x0200,
+    AS_DIAG_HIGH_MAGNETIC = 0x0400,
+    AS_DIAG_LOW_MAGNETIC = 0x0800,
+} As5048Diagnostics;
+
+
+//! Class for interfacing with the AMS AS5048A magnetic rotary sensor over the SPI-interface.
+class As5048Spi
+{
+public:
+    As5048Spi(PinName mosi, PinName miso, PinName sclk, PinName chipselect, int nDevices = 1);
+    ~As5048Spi();
+    
+    bool error(int device = -1);
+    
+    /// Sets the SPI clock frequency in Hz. Maximum tested frequency is 10MHz.
+    void frequency(int frequency = 1000000);
+    
+    /// Sends a read command to the sensor.
+    const int* read(As5048Command command);
+    
+    /// Sends a read command to the sensor. 
+    /// A call to this function will not directly return the requested value. The
+    /// requested value will be returned in a next read_sequential call. 
+    /// Use this function to read sensor values with minimum speed impact on SPI-bus
+    /// and microcontroller.
+    const int* read_sequential(As5048Command command);
+    
+    /// Performs a single angle measurement on all sensors
+    /// @return Array of raw angle data. To get the 14-bit value representing
+    ///     the angle, apply the mask() to the result.
+    const int* read_angle();
+    
+    /// Performs sequential angle measurements on all sensors. The first time this
+    /// method is called the result is not usefull, the measurement data of the call
+    /// will be returned by the next call to this method.
+    /// @return Array of raw angle data. To get the 14-bit value representing
+    ///     the angle, apply the mask() to the result.
+    const int* read_angle_sequential();
+
+    /// Returns lowest 14-bits
+    static int mask(int sensor_result);
+
+    /// Applies the mask to the first n bytes in the read buffer (for daisychained sensors).
+    static void mask(int* sensor_results, int n);
+    
+    /// Checks if the return value from the sensor has the right parity
+    /// @return true if ok
+    static bool parity_check(int sensor_result);
+    
+    /// Returns an angle from 0 to 36000 (degrees times 100).
+    /// @param sensor_result is one of the values returned by read_angle or read_angle_sequential
+    static int degrees(int sensor_result);
+    
+    /// Returns an angle from 0 to 2*PI*100 
+    /// @param sensor_result is one of the values returned by read_angle or read_angle_sequential
+    static int radian(int sensor_result);
+    
+
+protected:
+    int _nDevices;
+    DigitalOut _chipSelectN;
+    SPI _spi;
+    
+    int* _readBuffer; // Stores the results of the last sequential read
+    
+    int* _read(As5048Command command);
+};
+