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

kinda got angle sensors working

Browse Source
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
Download Patch File Download Diff File Expand all files Collapse all files

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