nick
/
thesis_bldc_controller
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

kinda got angle sensors working

This commit is contained in:
Nicolas Trimborn 2021-08-21 18:17:23 +02:00
parent ed161629eb
commit 9d2f2a2598
18 changed files with 812 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
2_Motor_Master/Motor_Master/Motor_Master/.atmelstart/atmel_start_config.atstart
View File

@ -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
2_Motor_Master/Motor_Master/Motor_Master/Config/hpl_sercom_config.h
View File

@ -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>

12
2_Motor_Master/Motor_Master/Motor_Master/Motor_Master.cproj
View File

@ -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>

130
2_Motor_Master/Motor_Master/Motor_Master/angle_sensors.c Normal file
View File

@ -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);
}

110
2_Motor_Master/Motor_Master/Motor_Master/angle_sensors.h Normal file
View File

@ -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
2_Motor_Master/Motor_Master/Motor_Master/bldc.c
View File

@ -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);

10
2_Motor_Master/Motor_Master/Motor_Master/driver_init.c
View File

@ -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

15
2_Motor_Master/Motor_Master/Motor_Master/main.c
View File

@ -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);
}

37
2_Motor_Slave/Motor_Slave/Motor_Slave/AS5048.c
View File

@ -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);

11
2_Motor_Slave/Motor_Slave/Motor_Slave/AS5048.h
View File

@ -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;

8
2_Motor_Slave/Motor_Slave/Motor_Slave/Motor_Slave.cproj
View File

@ -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>

125
2_Motor_Slave/Motor_Slave/Motor_Slave/angle_sensors.c Normal file
View File

@ -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);
}

110
2_Motor_Slave/Motor_Slave/Motor_Slave/angle_sensors.h Normal file
View File

@ -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_ */

32
2_Motor_Slave/Motor_Slave/Motor_Slave/main.c
View File

@ -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);

6
Examples/AS5048-mbed/.hg_archival.txt Normal file
View File

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

136
Examples/AS5048-mbed/as5048spi.cpp Normal file
View File

@ -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;
}

86
Examples/AS5048-mbed/as5048spi.h Normal file
View File

@ -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);
};

BIN
Twincat/MotorData/.vs/MotorData/v15/.suo
View File

Binary file not shown.