diff --git a/2_Motor_Master/Motor_Master/Motor_Master/.atmelstart/atmel_start_config.atstart b/2_Motor_Master/Motor_Master/Motor_Master/.atmelstart/atmel_start_config.atstart index db2d737..2fef071 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/.atmelstart/atmel_start_config.atstart +++ b/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 diff --git a/2_Motor_Master/Motor_Master/Motor_Master/Config/hpl_sercom_config.h b/2_Motor_Master/Motor_Master/Motor_Master/Config/hpl_sercom_config.h index 3ce0137..7d62662 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/Config/hpl_sercom_config.h +++ b/2_Motor_Master/Motor_Master/Motor_Master/Config/hpl_sercom_config.h @@ -377,7 +377,7 @@ // The SPI data transfer rate // 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 // diff --git a/2_Motor_Master/Motor_Master/Motor_Master/Motor_Master.cproj b/2_Motor_Master/Motor_Master/Motor_Master/Motor_Master.cproj index aa3b61d..84e5c1e 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/Motor_Master.cproj +++ b/2_Motor_Master/Motor_Master/Motor_Master/Motor_Master.cproj @@ -151,7 +151,7 @@ - + @@ -221,7 +221,7 @@ - + @@ -419,7 +419,7 @@ True Maximum (-g3) True - -std=gnu99 -mfloat-abi=hard -mfpu=fpv4-sp-d16 + -std=gnu11 -mfloat-abi=hard -mfpu=fpv4-sp-d16 True @@ -503,6 +503,12 @@ + + compile + + + compile + compile diff --git a/2_Motor_Master/Motor_Master/Motor_Master/angle_sensors.c b/2_Motor_Master/Motor_Master/Motor_Master/angle_sensors.c new file mode 100644 index 0000000..fe559ac --- /dev/null +++ b/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(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); +} diff --git a/2_Motor_Master/Motor_Master/Motor_Master/angle_sensors.h b/2_Motor_Master/Motor_Master/Motor_Master/angle_sensors.h new file mode 100644 index 0000000..c9627e5 --- /dev/null +++ b/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_ */ \ No newline at end of file diff --git a/2_Motor_Master/Motor_Master/Motor_Master/bldc.c b/2_Motor_Master/Motor_Master/Motor_Master/bldc.c index d5f80fc..966d875 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/bldc.c +++ b/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); diff --git a/2_Motor_Master/Motor_Master/Motor_Master/driver_init.c b/2_Motor_Master/Motor_Master/Motor_Master/driver_init.c index da2cb28..cf01c37 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/driver_init.c +++ b/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, + // Initial level + // pad_initial_level + // Low + // 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 diff --git a/2_Motor_Master/Motor_Master/Motor_Master/main.c b/2_Motor_Master/Motor_Master/Motor_Master/main.c index 2262acb..ba864c4 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/main.c +++ b/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); } diff --git a/2_Motor_Slave/Motor_Slave/Motor_Slave/AS5048.c b/2_Motor_Slave/Motor_Slave/Motor_Slave/AS5048.c index 047f6bf..18ae04e 100644 --- a/2_Motor_Slave/Motor_Slave/Motor_Slave/AS5048.c +++ b/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); diff --git a/2_Motor_Slave/Motor_Slave/Motor_Slave/AS5048.h b/2_Motor_Slave/Motor_Slave/Motor_Slave/AS5048.h index 77198e5..a5d389a 100644 --- a/2_Motor_Slave/Motor_Slave/Motor_Slave/AS5048.h +++ b/2_Motor_Slave/Motor_Slave/Motor_Slave/AS5048.h @@ -10,6 +10,17 @@ #include #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; diff --git a/2_Motor_Slave/Motor_Slave/Motor_Slave/Motor_Slave.cproj b/2_Motor_Slave/Motor_Slave/Motor_Slave/Motor_Slave.cproj index 5f069af..a7f9ae6 100644 --- a/2_Motor_Slave/Motor_Slave/Motor_Slave/Motor_Slave.cproj +++ b/2_Motor_Slave/Motor_Slave/Motor_Slave/Motor_Slave.cproj @@ -419,7 +419,7 @@ True Maximum (-g3) True - -std=gnu99 -mfloat-abi=hard -mfpu=fpv4-sp-d16 + -std=gnu11 -mfloat-abi=hard -mfpu=fpv4-sp-d16 True @@ -503,6 +503,12 @@ + + compile + + + compile + compile diff --git a/2_Motor_Slave/Motor_Slave/Motor_Slave/angle_sensors.c b/2_Motor_Slave/Motor_Slave/Motor_Slave/angle_sensors.c new file mode 100644 index 0000000..dcea02e --- /dev/null +++ b/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(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); +} diff --git a/2_Motor_Slave/Motor_Slave/Motor_Slave/angle_sensors.h b/2_Motor_Slave/Motor_Slave/Motor_Slave/angle_sensors.h new file mode 100644 index 0000000..fbee68d --- /dev/null +++ b/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_ */ \ No newline at end of file diff --git a/2_Motor_Slave/Motor_Slave/Motor_Slave/main.c b/2_Motor_Slave/Motor_Slave/Motor_Slave/main.c index 37665a3..b57f315 100644 --- a/2_Motor_Slave/Motor_Slave/Motor_Slave/main.c +++ b/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); diff --git a/Examples/AS5048-mbed/.hg_archival.txt b/Examples/AS5048-mbed/.hg_archival.txt new file mode 100644 index 0000000..a607457 --- /dev/null +++ b/Examples/AS5048-mbed/.hg_archival.txt @@ -0,0 +1,6 @@ +repo: 3edcf58e51e78b3cc029c791e3e7318cc6e2338f +node: 06b89a41109e5e51f534eb5a8b0067fe07d0e7dc +branch: default +latesttag: null +latesttagdistance: 5 +changessincelatesttag: 5 diff --git a/Examples/AS5048-mbed/as5048spi.cpp b/Examples/AS5048-mbed/as5048spi.cpp new file mode 100644 index 0000000..edb3418 --- /dev/null +++ b/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; +} + + + + + + \ No newline at end of file diff --git a/Examples/AS5048-mbed/as5048spi.h b/Examples/AS5048-mbed/as5048spi.h new file mode 100644 index 0000000..47175bb --- /dev/null +++ b/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); +}; + \ No newline at end of file diff --git a/Twincat/MotorData/.vs/MotorData/v15/.suo b/Twincat/MotorData/.vs/MotorData/v15/.suo index 63ea731..92ed812 100644 Binary files a/Twincat/MotorData/.vs/MotorData/v15/.suo and b/Twincat/MotorData/.vs/MotorData/v15/.suo differ