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

changes made with edmundo

Browse Source
This commit is contained in:
Nicolas Trimborn
2021-08-27 22:05:46 +02:00
parent c00c82e6e6
commit e98d8c5085
48 changed files with 585 additions and 2009 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
2_Motor_Slave/Motor_Slave/Motor_Slave/.atmelstart/atmel_start_config.atstart
View File

@@ -308,7 +308,7 @@ drivers:
functionality: System
api: HAL:HPL:DMAC
configuration:
dmac_beatsize_0: 8-bit bus transfer
dmac_beatsize_0: 32-bit bus transfer
dmac_beatsize_1: 16-bit bus transfer
dmac_beatsize_10: 8-bit bus transfer
dmac_beatsize_11: 8-bit bus transfer
@@ -331,7 +331,7 @@ drivers:
dmac_beatsize_27: 8-bit bus transfer
dmac_beatsize_28: 8-bit bus transfer
dmac_beatsize_29: 8-bit bus transfer
dmac_beatsize_3: 8-bit bus transfer
dmac_beatsize_3: 32-bit bus transfer
dmac_beatsize_30: 8-bit bus transfer
dmac_beatsize_31: 8-bit bus transfer
dmac_beatsize_4: 8-bit bus transfer

4
2_Motor_Slave/Motor_Slave/Motor_Slave/Config/hpl_dmac_config.h
View File

@@ -301,7 +301,7 @@
// <i> Defines the size of one beat
// <id> dmac_beatsize_0
#ifndef CONF_DMAC_BEATSIZE_0
#define CONF_DMAC_BEATSIZE_0 0
#define CONF_DMAC_BEATSIZE_0 2
#endif
// <o> Block Action
@@ -973,7 +973,7 @@
// <i> Defines the size of one beat
// <id> dmac_beatsize_3
#ifndef CONF_DMAC_BEATSIZE_3
#define CONF_DMAC_BEATSIZE_3 0
#define CONF_DMAC_BEATSIZE_3 2
#endif
// <o> Block Action

218
2_Motor_Slave/Motor_Slave/Motor_Slave/Ethercat_QSPI.c
View File

@@ -1,218 +0,0 @@
/*
* Ethercat_QSPI.c
*
* Created: 31/07/2021 17:52:21
* Author: Nick-XMG
*/
#include "Ethercat_QSPI.h"
#include <hpl_qspi_config.h>
#include <hpl_sercom_config.h>
#define SQI_READ (0x0B<<16)
#define SQI_WRITE (0x02<<16)
#define SQI_INC (0x40<<8)
#define SQI_DEC (0x80<<8)
#define ECAT_PRAM_RD_DATA 0x0000
#define ECAT_PRAM_WR_DATA 0x0020
#define HW_CFG 0x0074
#define BYTE_TEST 0x0064
#define ECAT_PRAM_RD_ADDR_LEN 0x0308
#define ECAT_PRAM_RD_CMD 0x030C
#define ECAT_PRAM_WR_ADDR_LEN 0x0310
#define ECAT_PRAM_WR_CMD 0x0314
#define LAN9252_RDY (1<<27)
#define PDRAM_RD_ADDRESS 0x1100
#define PDRAM_RD_LENGTH ECAT_SIZE_RD //do the math later to automatize. fixed to 64 for now.
#define PDRAM_WR_ADDRESS 0x1800
#define PDRAM_WR_LENGTH ECAT_SIZE_WR
#define PDRAM_LENGTH_MAX 64
#define PDRAM_LENGTH_SHORT 32
#define FIFO_DEPTH 16
#define PDRAM_REG_MAX PDRAM_LENGTH_SHORT/4
#define PRAM_X_ABORT (1<<30)
#define CSR_BUSY 0x80000000
COMPILER_ALIGNED(16)
DmacDescriptor dummy_rx_descriptor;
DmacDescriptor dummy_tx_descriptor;
volatile uint32_t QSPI_tx_buffer[buffer_size]={0};
volatile uint32_t QSPI_rx_buffer[buffer_size]={0};
volatile enum ecat_states ecat_state = wait;
volatile enum ecat_states next_ecat_state = wait;
volatile uint8_t wr_cnt = 0;
volatile uint8_t rd_cnt = 0;
static uint8_t sync_rx_buffer[2*2*buffer_size/3]={0};
static uint8_t sync_tx_buffer[2*2*buffer_size/3]={0xED};
static uint32_t QSPI_cmds[]={0,PRAM_X_ABORT,0,PRAM_X_ABORT,
((PDRAM_LENGTH_MAX)<<16)+PDRAM_RD_ADDRESS, CSR_BUSY,(PDRAM_LENGTH_MAX<<16)+PDRAM_WR_ADDRESS,CSR_BUSY,
((PDRAM_LENGTH_MAX)<<16)+PDRAM_RD_ADDRESS+PDRAM_LENGTH_MAX, CSR_BUSY,(PDRAM_LENGTH_MAX<<16)+PDRAM_WR_ADDRESS+PDRAM_LENGTH_MAX,CSR_BUSY,
((PDRAM_LENGTH_MAX)<<16)+PDRAM_RD_ADDRESS+PDRAM_LENGTH_MAX*2, CSR_BUSY,(PDRAM_LENGTH_MAX<<16)+PDRAM_WR_ADDRESS+PDRAM_LENGTH_MAX*2,CSR_BUSY,
};
static uint32_t zero[FIFO_DEPTH]={0};
volatile uint32_t status = 0;
struct _qspi_command rd_cmd = {
.inst_frame.bits.width = QSPI_INST4_ADDR4_DATA4,
.inst_frame.bits.inst_en = 0,
.inst_frame.bits.data_en = 1,
.inst_frame.bits.addr_en = 1,
.inst_frame.bits.dummy_cycles = 6,
.inst_frame.bits.tfr_type = QSPI_READMEM_ACCESS,
};
struct _qspi_command wr_cmd = {
.inst_frame.bits.width = QSPI_INST4_ADDR4_DATA4,
.inst_frame.bits.inst_en = 0,
.inst_frame.bits.data_en = 1,
.inst_frame.bits.addr_en = 1,
.inst_frame.bits.dummy_cycles = 0,
.inst_frame.bits.tfr_type = QSPI_WRITEMEM_ACCESS,
};
struct _qspi_command qspi_cmd = {
.inst_frame.bits.width = QSPI_INST1_ADDR1_DATA1,
.inst_frame.bits.inst_en = 1,
.inst_frame.bits.data_en = 0,
.inst_frame.bits.addr_en = 0,
.inst_frame.bits.dummy_cycles = 0,
.instruction = 0x38,
.inst_frame.bits.tfr_type = QSPI_WRITE_ACCESS,
};
struct _qspi_command *spi_cmd= &wr_cmd;
volatile uint8_t tx_complete =3;
volatile uint32_t *ECAT_BYTE_TEST= QSPI_AHB+BYTE_TEST+SQI_READ;
volatile uint32_t *ECAT_FIFO_RD_RD= QSPI_AHB+SQI_READ +ECAT_PRAM_RD_DATA;
//volatile uint32_t *ECAT_FIFO_RD_RD= QSPI_AHB+SQI_READ +SQI_INC+ECAT_PRAM_RD_DATA;
volatile uint32_t *ECAT_FIFO_WR_WR= QSPI_AHB+SQI_WRITE+ECAT_PRAM_WR_DATA;
//volatile uint32_t *ECAT_FIFO_WR_WR= QSPI_AHB+SQI_WRITE+SQI_INC+ECAT_PRAM_WR_DATA;
volatile uint32_t *ECAT_HW_CFG_RD= QSPI_AHB+SQI_READ+HW_CFG;
volatile uint32_t *ECAT_FIFO_RD_ADLEN_WR= QSPI_AHB+SQI_WRITE+SQI_INC+ECAT_PRAM_RD_ADDR_LEN;
volatile uint32_t *ECAT_FIFO_WR_ADLEN_WR= QSPI_AHB+SQI_WRITE+SQI_INC+ECAT_PRAM_WR_ADDR_LEN;
volatile uint32_t *ECAT_FIFO_WR_ADLEN_RD= QSPI_AHB+SQI_READ+SQI_INC+ECAT_PRAM_WR_ADDR_LEN;
volatile uint32_t *ECAT_FIFO_WR_CMD_RD= QSPI_AHB+SQI_READ+ECAT_PRAM_WR_CMD;
volatile uint32_t *ECAT_FIFO_WR_CMD_WR= QSPI_AHB+SQI_WRITE+ECAT_PRAM_WR_CMD;
volatile uint32_t *INPUT_ADDRESS ;
volatile uint32_t *OUTPUT_ADDRESS;
volatile uint32_t read_buffer =0;
volatile uint8_t ecat_length= 0;
volatile bool run_ECAT = false;
volatile bool synced = false;
void ECAT_STATE_MACHINE(void){
if ((ecat_state != wait)&(ecat_state != wait2)){
run_ECAT = false;
switch(ecat_state){
case en_SQI:
spi_cmd = &qspi_cmd;
QSPI->INSTRCTRL.bit.INSTR=spi_cmd->instruction;
OUTPUT_ADDRESS = ECAT_FIFO_RD_ADLEN_WR;
INPUT_ADDRESS = &QSPI_cmds[0];
ecat_length = 0;
next_ecat_state = rd_rdy;
break;
case rd_rdy:
if (QSPI_rx_buffer[0]==LAN9252_RDY){
next_ecat_state = abort_fifo;
}
else if (QSPI_rx_buffer[0]== 0xFFFFFFFF){
next_ecat_state = en_SQI;
QSPI_rx_buffer[0] = 0;
}
else{
spi_cmd = &rd_cmd;
OUTPUT_ADDRESS = &QSPI_rx_buffer[0];
INPUT_ADDRESS = ECAT_HW_CFG_RD;
ecat_length = 1;
wr_cnt=0;
}
break;
case abort_fifo:
spi_cmd = &wr_cmd;
OUTPUT_ADDRESS = ECAT_FIFO_WR_CMD_WR;
INPUT_ADDRESS = &QSPI_cmds[1];
ecat_length = 1;
next_ecat_state = dlt_rdram;
wr_cnt=0;
rd_cnt=0;
break;
case dlt_rdram:
spi_cmd = &wr_cmd;
OUTPUT_ADDRESS = ECAT_FIFO_WR_WR;
INPUT_ADDRESS = &zero[0];
//if (wr_cnt >= 1) ecat_length = FIFO_DEPTH/2;
//else ecat_length = FIFO_DEPTH;
ecat_length = FIFO_DEPTH;
next_ecat_state = cf_dlt_rdram;
break;
case cf_dlt_rdram:
spi_cmd = &wr_cmd;
OUTPUT_ADDRESS = ECAT_FIFO_WR_ADLEN_WR;
INPUT_ADDRESS = &QSPI_cmds[4*(wr_cnt+1)];
ecat_length = 2;
if (wr_cnt >= 2) {
next_ecat_state = wait;
wr_cnt =0;
} else {
next_ecat_state = dlt_rdram;
wr_cnt++;
}
break;
case write_fifo:
spi_cmd = &wr_cmd;
OUTPUT_ADDRESS = ECAT_FIFO_WR_WR;
INPUT_ADDRESS = &QSPI_tx_buffer[wr_cnt*FIFO_DEPTH];
/*if (wr_cnt >= 1) ecat_length = FIFO_DEPTH/2;
else ecat_length = FIFO_DEPTH;*/
ecat_length = FIFO_DEPTH;
next_ecat_state = config_fifo;
break;
case config_fifo:
spi_cmd = &wr_cmd;
OUTPUT_ADDRESS = ECAT_FIFO_RD_ADLEN_WR;
INPUT_ADDRESS = &QSPI_cmds[4*(wr_cnt+1)];
ecat_length = 4;
next_ecat_state = read_fifo;
break;
case read_fifo:
spi_cmd = &rd_cmd;
OUTPUT_ADDRESS = &QSPI_rx_buffer[wr_cnt*FIFO_DEPTH];
INPUT_ADDRESS = ECAT_FIFO_RD_RD;
ecat_length = FIFO_DEPTH;
if (wr_cnt >= 2) {
// ecat_length = FIFO_DEPTH/2;
next_ecat_state = wait2;
wr_cnt=0;
}
else {
// ecat_length = FIFO_DEPTH;
next_ecat_state = write_fifo;
wr_cnt++;
}
break;
}
qspi_dma_enable(&ECAT_QSPI);
QSPI->INSTRFRAME.reg = ((*spi_cmd).inst_frame.word);
for (uint8_t i=0;i<ecat_length;i++)
{
*(OUTPUT_ADDRESS+i) = *(INPUT_ADDRESS+i);
}
QSPI->CTRLA.bit.LASTXFER = 1;

38
2_Motor_Slave/Motor_Slave/Motor_Slave/Ethercat_QSPI.h
View File

@@ -1,38 +0,0 @@
/*
* EtherCAT_QSPI.h
*
* Created: 31/07/2021 17:51:24
* Author: Nick-XMG
*/
#ifndef ETHERCAT_QSPI_H_
#define ETHERCAT_QSPI_H_
#include "atmel_start.h"
#define ECAT_SIZE_WR 64 //max fifo size
#define ECAT_SIZE_RD ECAT_SIZE_WR
#define ECAT_SIZE_WR_REG ECAT_SIZE_WR/4
#define ECAT_SIZE_RD_REG ECAT_SIZE_WR/4
#define buffer_size 3*ECAT_SIZE_WR_REG //changed to double
#define motor_buffer_size buffer_size/3
extern enum ecat_states {abort_fifo,dlt_rdram,cf_dlt_rdram,write_fifo,config_fifo,read_fifo,wait,wait2,en_SQI,temp,rd_rdy};
extern volatile enum ecat_states ecat_state;
extern volatile enum ecat_states next_ecat_state;
volatile uint32_t QSPI_tx_buffer[buffer_size];
volatile uint32_t QSPI_rx_buffer[buffer_size];
//static uint32_t QSPI_tx_buffer[buffer_size] = {47,46,45,44,43,42,41,40,39,38,37,36,35,34,33,32,
//31,30,29,28,27,26,25,24,23,22,21,20,19,18,17,16,
//15,14,13,12,11,10, 9,8,7,6,5,4,3,2,1,0};
//static uint32_t QSPI_rx_buffer[buffer_size] = {0};
extern volatile bool run_ECAT;
void config_qspi(void);
#endif /* ETHERCAT_QSPI_H_ */

297
2_Motor_Slave/Motor_Slave/Motor_Slave/Ethercat_SlaveDef.h
View File

@@ -1,297 +0,0 @@
/*
* EtherCAT_SlaveDef.h
*
* Created: 01/08/2021 12:56:16
* Author: Nick-XMG
*/
#ifndef ETHERCAT_SLAVEDEF_H_
#define ETHERCAT_SLAVEDEF_H_
#include "Ethercat_QSPI.h"
//Write To Ecat Total Bytes (XX bytes)
/* Motor 1*/
static volatile uint8_t *M1_Status = (uint8_t *)&QSPI_tx_buffer[0];
static volatile uint8_t *M1_Mode = (((uint8_t *)&QSPI_tx_buffer[0])+1);
static volatile int16_t *M1_Joint_rel_position = (((int16_t *)&QSPI_tx_buffer[0])+1);
static volatile int16_t *M1_Joint_abs_position = ((int16_t *)&QSPI_tx_buffer[1]);
static volatile int16_t *M1_Motor_speed = (((int16_t *)&QSPI_tx_buffer[1])+1);
static volatile int16_t *M1_Motor_current_bus = ((int16_t *)&QSPI_tx_buffer[2]);
static volatile int16_t *M1_Motor_currentPhA = (((int16_t *)&QSPI_tx_buffer[2])+1);
static volatile int16_t *M1_Motor_currentPhB = ((int16_t *)&QSPI_tx_buffer[3]);
static volatile int16_t *M1_Motor_currentPhC = (((int16_t *)&QSPI_tx_buffer[3])+1);
static volatile int16_t *M1_Motor__hallState = ((int16_t *)&QSPI_tx_buffer[4]);
static volatile int16_t *M1_Motor_dutyCycle = (((int16_t *)&QSPI_tx_buffer[4])+1);
/* Motor 2*/
static volatile uint8_t *M2_Status = (uint8_t *)&QSPI_tx_buffer[5];
static volatile uint8_t *M2_Mode = (((uint8_t *)&QSPI_tx_buffer[5])+1);
static volatile int16_t *M2_Joint_rel_position = (((int16_t *)&QSPI_tx_buffer[5])+1);
static volatile int16_t *M2_Joint_abs_position = ((int16_t *)&QSPI_tx_buffer[6]);
static volatile int16_t *M2_Motor_speed = (((int16_t *)&QSPI_tx_buffer[6])+1);
static volatile int16_t *M2_Motor_current_bus = ((int16_t *)&QSPI_tx_buffer[7]);
static volatile int16_t *M2_Motor_currentPhA = (((int16_t *)&QSPI_tx_buffer[7])+1);
static volatile int16_t *M2_Motor_currentPhB = ((int16_t *)&QSPI_tx_buffer[8]);
static volatile int16_t *M2_Motor_currentPhC = (((int16_t *)&QSPI_tx_buffer[8])+1);
static volatile int16_t *M2_Motor__hallState = ((int16_t *)&QSPI_tx_buffer[9]);
static volatile int16_t *M2_Motor_dutyCycle = (((int16_t *)&QSPI_tx_buffer[9])+1);
/* EMG */
static volatile int16_t *EMG_CH1 = (((int16_t *)&QSPI_tx_buffer[10]));
static volatile int16_t *EMG_CH2 = (((int16_t *)&QSPI_tx_buffer[10])+1);
static volatile int16_t *EMG_CH3 = (((int16_t *)&QSPI_tx_buffer[11]));
static volatile int16_t *EMG_CH4 = (((int16_t *)&QSPI_tx_buffer[11])+1);
static volatile int16_t *EMG_CH5 = (((int16_t *)&QSPI_tx_buffer[12]));
static volatile int16_t *EMG_CH6 = (((int16_t *)&QSPI_tx_buffer[12])+1);
static volatile int16_t *EMG_CH7 = (((int16_t *)&QSPI_tx_buffer[13]));
static volatile int16_t *EMG_CH8 = (((int16_t *)&QSPI_tx_buffer[13])+1);
/* Motor 3*/
static volatile uint8_t *M3_Status = (uint8_t *)&QSPI_tx_buffer[14];
static volatile uint8_t *M3_Mode = (((uint8_t *)&QSPI_tx_buffer[14])+1);
static volatile int16_t *M3_Joint_rel_position = (((int16_t *)&QSPI_tx_buffer[14])+1);
static volatile int16_t *M3_Joint_abs_position = ((int16_t *)&QSPI_tx_buffer[15]);
static volatile int16_t *M3_Motor_speed = (((int16_t *)&QSPI_tx_buffer[15])+1);
static volatile int16_t *M3_Motor_current_bus = ((int16_t *)&QSPI_tx_buffer[16]);
static volatile int16_t *M3_Motor_currentPhA = (((int16_t *)&QSPI_tx_buffer[16])+1);
static volatile int16_t *M3_Motor_currentPhB = ((int16_t *)&QSPI_tx_buffer[17]);
static volatile int16_t *M3_Motor_currentPhC = (((int16_t *)&QSPI_tx_buffer[17])+1);
static volatile int16_t *M3_Motor__hallState = ((int16_t *)&QSPI_tx_buffer[18]);
static volatile int16_t *M3_Motor_dutyCycle = (((int16_t *)&QSPI_tx_buffer[18])+1);
/* Motor 4*/
static volatile uint8_t *M4_Status = (uint8_t *)&QSPI_tx_buffer[19];
static volatile uint8_t *M4_Mode = (((uint8_t *)&QSPI_tx_buffer[19])+1);
static volatile int16_t *M4_Joint_rel_position = (((int16_t *)&QSPI_tx_buffer[19])+1);
static volatile int16_t *M4_Joint_abs_position = ((int16_t *)&QSPI_tx_buffer[20]);
static volatile int16_t *M4_Motor_speed = (((int16_t *)&QSPI_tx_buffer[20])+1);
static volatile int16_t *M4_Motor_current_bus = ((int16_t *)&QSPI_tx_buffer[21]);
static volatile int16_t *M4_Motor_currentPhA = (((int16_t *)&QSPI_tx_buffer[21])+1);
static volatile int16_t *M4_Motor_currentPhB = ((int16_t *)&QSPI_tx_buffer[22]);
static volatile int16_t *M4_Motor_currentPhC = (((int16_t *)&QSPI_tx_buffer[22])+1);
static volatile int16_t *M4_Motor__hallState = ((int16_t *)&QSPI_tx_buffer[23]);
static volatile int16_t *M4_Motor_dutyCycle = (((int16_t *)&QSPI_tx_buffer[23])+1);
/* IMU */
static volatile int16_t *q_x0 = (((int16_t *)&QSPI_tx_buffer[24]));
static volatile int16_t *q_y0 = (((int16_t *)&QSPI_tx_buffer[24])+1);
static volatile int16_t *q_z0 = (((int16_t *)&QSPI_tx_buffer[25]));
static volatile int16_t *q_w0 = (((int16_t *)&QSPI_tx_buffer[25])+1);
/* EMG */
static volatile int16_t *FSR_CH1 = (((int16_t *)&QSPI_tx_buffer[26]));
static volatile int16_t *FSR_CH2 = (((int16_t *)&QSPI_tx_buffer[26])+1);
static volatile int16_t *FSR_CH3 = (((int16_t *)&QSPI_tx_buffer[27]));
static volatile int16_t *FSR_CH4 = (((int16_t *)&QSPI_tx_buffer[27])+1);
static volatile int16_t *FSR_CH5 = (((int16_t *)&QSPI_tx_buffer[28]));
static volatile int16_t *Pressure_CH1 = (((int16_t *)&QSPI_tx_buffer[28])+1);
static volatile int16_t *Pressure_CH2 = (((int16_t *)&QSPI_tx_buffer[29]));
static volatile int16_t *Pressure_CH3 = (((int16_t *)&QSPI_tx_buffer[29])+1);
//Read From Ecat Total (XX Bytes)
//QSPI_rx_buffer
/* Motor 1*/
static volatile uint8_t *M1_Control_mode = ((uint8_t *)&QSPI_rx_buffer[0]);
static volatile uint8_t *M1_Control_set = (((uint8_t *)&QSPI_rx_buffer[0])+1);
static volatile int16_t *M1_Desired_pos = ((int16_t *)&QSPI_rx_buffer[0]+1);
static volatile int16_t *M1_Desired_speed = ((int16_t *)&QSPI_rx_buffer[1]);
static volatile int16_t *M1_Desired_current = ((int16_t *)&QSPI_rx_buffer[1]+1);
static volatile int16_t *M1_Max_pos = ((int16_t *)&QSPI_rx_buffer[2]);
static volatile int16_t *M1_Max_velocity = ((int16_t *)&QSPI_rx_buffer[2]+1);
static volatile int16_t *M1_Max_current = ((int16_t *)&QSPI_rx_buffer[3]);
static volatile int16_t *M1_Desired_dc = ((int16_t *)&QSPI_rx_buffer[3]+1); //Spare
///* Motor 2*/
static volatile uint8_t *M2_Control_mode = ((uint8_t *)&QSPI_rx_buffer[4]);
static volatile uint8_t *M2_Control_set = (((uint8_t *)&QSPI_rx_buffer[4])+1);
static volatile int16_t *M2_Desired_pos = ((int16_t *)&QSPI_rx_buffer[4]+1);
static volatile int16_t *M2_Desired_speed = ((int16_t *)&QSPI_rx_buffer[5]);
static volatile int16_t *M2_Desired_current = ((int16_t *)&QSPI_rx_buffer[5]+1);
static volatile int16_t *M2_Max_pos = ((int16_t *)&QSPI_rx_buffer[6]);
static volatile int16_t *M2_Max_velocity = ((int16_t *)&QSPI_rx_buffer[6]+1);
static volatile int16_t *M2_Max_current = ((int16_t *)&QSPI_rx_buffer[7]);
static volatile int16_t *M2_Desired_dc = ((int16_t *)&QSPI_rx_buffer[7]+1); //Spare
///* Motor 3*/
static volatile uint8_t *M3_Control_mode = ((uint8_t *)&QSPI_rx_buffer[8]);
static volatile uint8_t *M3_Control_set = (((uint8_t *)&QSPI_rx_buffer[8])+1);
static volatile int16_t *M3_Desired_pos = ((int16_t *)&QSPI_rx_buffer[8]+1);
static volatile int16_t *M3_Desired_speed = ((int16_t *)&QSPI_rx_buffer[9]);
static volatile int16_t *M3_Desired_current = ((int16_t *)&QSPI_rx_buffer[9]+1);
static volatile int16_t *M3_Max_pos = ((int16_t *)&QSPI_rx_buffer[10]);
static volatile int16_t *M3_Max_velocity = ((int16_t *)&QSPI_rx_buffer[10]+1);
static volatile int16_t *M3_Max_current = ((int16_t *)&QSPI_rx_buffer[11]);
static volatile int16_t *M3_Spare = ((int16_t *)&QSPI_rx_buffer[11]+1); //Spare
///* Motor 4*/
static volatile uint8_t *M4_Control_mode = ((uint8_t *)&QSPI_rx_buffer[12]);
static volatile uint8_t *M4_Control_set = (((uint8_t *)&QSPI_rx_buffer[12])+1);
static volatile int16_t *M4_Desired_pos = ((int16_t *)&QSPI_rx_buffer[12]+1);
static volatile int16_t *M4_Desired_speed = ((int16_t *)&QSPI_rx_buffer[13]);
static volatile int16_t *M4_Desired_current = ((int16_t *)&QSPI_rx_buffer[13]+1);
static volatile int16_t *M4_Max_pos = ((int16_t *)&QSPI_rx_buffer[14]);
static volatile int16_t *M4_Max_velocity = ((int16_t *)&QSPI_rx_buffer[14]+1);
static volatile int16_t *M4_Max_current = ((int16_t *)&QSPI_rx_buffer[15]);
static volatile int16_t *M4_Spare = ((int16_t *)&QSPI_rx_buffer[15]+1);//Spare
static void update_telemetry(void)
{
inline int16_t convert_to_mA(volatile float32_t current_PU)
{
return (int16_t)(current_PU*1000.0f);
}
//
//*M1_Status = 0;
//*M1_Mode = 0;
/* Motor 1 */
*M1_Status = Motor1.motor_state.currentstate;
*M1_Joint_rel_position = Motor1.motor_status.Num_Steps;
//*M1_Joint_abs_position = ((int16_t *)&QSPI_tx_buffer[1]);
//*M1_Motor_speed = (((int16_t *)&QSPI_tx_buffer[1])+1);
*M1_Motor_current_bus = convert_to_mA(Motor1.Iphase_pu.Bus);
*M1_Motor_currentPhA = convert_to_mA(Motor1.Iphase_pu.A);
*M1_Motor_currentPhB = convert_to_mA(Motor1.Iphase_pu.B);
*M1_Motor_currentPhC = convert_to_mA(Motor1.Iphase_pu.C);
*M1_Motor__hallState = Motor1.motor_status.currentHallPattern;
*M1_Motor_dutyCycle = Motor1.motor_status.duty_cycle;
*M1_Motor_speed = (int16_t)Motor1.motor_status.calc_rpm;
*M1_Joint_abs_position = Motor1.motor_status.actualDirection;
/* Motor 2 */
*M2_Status = Motor2.motor_state.currentstate;
*M2_Joint_rel_position = Motor2.motor_status.Num_Steps;
//*M1_Joint_abs_position = ((int16_t *)&QSPI_tx_buffer[1]);
//*M1_Motor_speed = (((int16_t *)&QSPI_tx_buffer[1])+1);
*M2_Motor_current_bus = convert_to_mA( Motor2.Iphase_pu.Bus);
*M2_Motor_currentPhA = convert_to_mA( Motor2.Iphase_pu.A);
*M2_Motor_currentPhB = convert_to_mA( Motor2.Iphase_pu.B);
*M2_Motor_currentPhC = convert_to_mA(Motor2.Iphase_pu.C);
*M2_Motor__hallState = Motor2.motor_status.currentHallPattern;
*M2_Motor_dutyCycle = Motor2.motor_status.duty_cycle;
*M2_Motor_speed = (int16_t)Motor2.motor_status.calc_rpm;
*M2_Joint_abs_position = Motor2.motor_status.actualDirection;
}
static void update_setpoints(void)
{
Motor1.motor_setpoints.desired_position = *M1_Desired_pos;
Motor1.motor_setpoints.desired_speed = *M1_Desired_speed;
Motor1.motor_setpoints.desired_torque = *M1_Desired_current;
Motor1.motor_setpoints.max_current = *M1_Max_current;
Motor1.motor_setpoints.max_torque = *M1_Max_current;
Motor1.motor_setpoints.max_velocity = *M1_Max_velocity;
Motor2.motor_setpoints.desired_position = *M2_Desired_pos;
Motor2.motor_setpoints.desired_speed = *M2_Desired_speed;
Motor2.motor_setpoints.desired_torque = *M2_Desired_current;
Motor2.motor_setpoints.max_current = *M2_Max_current;
Motor2.motor_setpoints.max_torque = *M2_Max_current;
Motor2.motor_setpoints.max_velocity = *M2_Max_velocity;
//volatile uint8_t a = *M1_Control_mode;
//volatile uint8_t b = *M1_Control_set;
//volatile int16_t c = *M1_Desired_pos;
//volatile int16_t d = *M1_Desired_speed;
//volatile int16_t e = *M1_Desired_current;
//volatile int16_t f = *M1_Max_pos;
//volatile int16_t g = *M1_Max_velocity;
//volatile int16_t h = *M1_Max_current;
//volatile int16_t i = *M1_Spare;
//inline float32_t convert_int_to_PU(volatile int16_t input)
//{
//return ((float32_t)(input/1000.0f));
//}
////Motor1.des_mode = 0;
////Motor1.set = 0;
//Motor1.motor_setpoints.desired_position = *desired_position;
//Motor1.motor_setpoints.desired_speed = *desired_speed;
////Motor1.desired_speed = 1500;
//Motor1.motor_setpoints.desired_torque = convert_int_to_PU(*desired_torque);
////Motor1.controllerParam.I_kp = 0;
////Motor1.controllerParam.I_ki = 0;
////Motor1.controllerParam.V_kp = 0;
////Motor1.controllerParam.V_kd = 0;
////Motor1.controllerParam.V_kd = 0;
////Motor1.controllerParam.P_kp = 0;
////Motor1.controllerParam.P_ki = 0;
////Motor1.reductionRatio = 0;
//Motor1.motor_setpoints.max_velocity = *max_velocity;
//Motor1.motor_setpoints.max_current = convert_int_to_PU(*max_current);
//Motor1.motor_setpoints.max_torque = convert_int_to_PU(*max_torque);
////Motor1.Spare1 = 0;
////Motor1.Spare2 = 0;
////Motor1.Spare3 = 0;
////Motor1.Spare4 = 0;
}
static inline void comms_check(void)
{
/* Motor 1*/
*M1_Status = 1;
*M1_Mode = 2;
*M1_Joint_rel_position = -3;
*M1_Joint_abs_position = 4;
*M1_Motor_speed = -5;
*M1_Motor_current_bus = 6;
*M1_Motor_currentPhA = -7;
*M1_Motor_currentPhB = 8;
*M1_Motor_currentPhC = -9;
*M1_Motor__hallState = 10;
*M1_Motor_dutyCycle = -11;
/* Motor 2*/
*M2_Status = 12;
*M2_Mode = 13;
*M2_Joint_rel_position = 14;
*M2_Joint_abs_position = -15;
*M2_Motor_speed = 16;
*M2_Motor_current_bus = -17;
*M2_Motor_currentPhA = 18;
*M2_Motor_currentPhB = -19;
*M2_Motor_currentPhC = 20;
*M2_Motor__hallState = -21;
*M3_Motor_dutyCycle = 22;
/* EMG */
*EMG_CH1 = -23;
*EMG_CH2 = 24;
*EMG_CH3 = -25;
*EMG_CH4 = 26;
*EMG_CH5 = -27;
*EMG_CH6 = 28;
*EMG_CH7 = -29;
*EMG_CH8 = 30;
/* Motor 3*/
*M3_Status = 1;
*M3_Mode = 2;
*M3_Joint_rel_position = -3;
*M3_Joint_abs_position = 4;
*M3_Motor_speed = -5;
*M3_Motor_current_bus = 6;
*M3_Motor_currentPhA = -7;
*M3_Motor_currentPhB = 8;
*M3_Motor_currentPhC = -9;
*M3_Motor__hallState = 10;
*M3_Motor_dutyCycle = -11;
/* Motor 4*/
*M4_Status = 12;
*M4_Mode = 13;
*M4_Joint_rel_position = 14;
*M4_Joint_abs_position = -15;
*M4_Motor_speed = 16;
*M4_Motor_current_bus = -17;
*M4_Motor_currentPhA = 18;
*M4_Motor_currentPhB = -19;
*M4_Motor_currentPhC = 20;
*M4_Motor__hallState = -21;
*M4_Motor_dutyCycle = 22;
/* IMU */
*q_x0 = 23;
*q_y0 = -24;
*q_z0 = 25;
*q_w0 = -26;
/* EMG */
*FSR_CH1 = 27;
*FSR_CH2 = -28;
*FSR_CH3 = 29;
*FSR_CH4 = -30;
*FSR_CH5 = 31;
*Pressure_CH1 = -32;
*Pressure_CH2 = 33;
*Pressure_CH3 = -34;
}
#endif /* ETHERCAT_SLAVEDEF_H_ */

183
2_Motor_Slave/Motor_Slave/Motor_Slave/MSIF_slave.h
View File

@@ -9,75 +9,141 @@
#ifndef MASTER_SLAVE_IF_H_
#define MASTER_SLAVE_IF_H_
#define SLAVE_BUFFER_SIZE 64
#define SLAVE_BUFFER_SIZE_BYTES 64
#define SLAVE_BUFFER_SIZE_LONG SLAVE_BUFFER_SIZE_BYTES/4
static uint8_t SPI_rx_buffer[SLAVE_BUFFER_SIZE] = {0};
static uint8_t SPI_tx_buffer[SLAVE_BUFFER_SIZE] = {0};
static uint32_t SPI_rx_buffer[SLAVE_BUFFER_SIZE_LONG] = {0};
static uint32_t SPI_tx_buffer[SLAVE_BUFFER_SIZE_LONG] = {0};
//static uint8_t SPI_tx_buffer[SLAVE_BUFFER_SIZE] = {0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,
//18,19,20,21,22,23,24,25,26,27,28,29,30,31};
//tx_buffer
/* Motor 3*/
static volatile uint8_t *M3_Status = (uint8_t *)&SPI_tx_buffer[0]; //1 byte - 0 of 64
static volatile uint8_t *M3_Mode = (uint8_t *)&SPI_tx_buffer[1]; //1 byte - 1 of 64
static volatile int16_t *M3_Joint_rel_position = (int16_t *)&SPI_tx_buffer[2]; //2 byte - 2 of 64
static volatile int16_t *M3_Joint_abs_position = (int16_t *)&SPI_tx_buffer[4]; //2 byte - 4 of 64
static volatile int16_t *M3_Motor_speed = (int16_t *)&SPI_tx_buffer[6]; //2 byte - 6 of 64
static volatile int16_t *M3_Motor_current_bus = (int16_t *)&SPI_tx_buffer[8]; //2 byte - 8 of 64
static volatile int16_t *M3_Motor_currentPhA = (int16_t *)&SPI_tx_buffer[10]; //2 byte - 10 of 64
static volatile int16_t *M3_Motor_currentPhB = (int16_t *)&SPI_tx_buffer[12]; //2 byte - 12 of 64
static volatile int16_t *M3_Motor_currentPhC = (int16_t *)&SPI_tx_buffer[14]; //2 byte - 14 of 64
static volatile int16_t *M3_Motor__hallState = (int16_t *)&SPI_tx_buffer[16]; //2 byte - 16 of 64
static volatile int16_t *M3_Motor_dutyCycle = (int16_t *)&SPI_tx_buffer[18]; //2 byte - 18 of 64
static volatile uint8_t *M3_Status = (uint8_t *)&SPI_tx_buffer[0];
static volatile uint8_t *M3_Mode = (((uint8_t *)&SPI_tx_buffer[0])+1);
static volatile int16_t *M3_Joint_rel_position = (((int16_t *)&SPI_tx_buffer[0])+1);
static volatile int16_t *M3_Joint_abs_position = ((int16_t *)&SPI_tx_buffer[1]);
static volatile int16_t *M3_Motor_speed = (((int16_t *)&SPI_tx_buffer[1])+1);
static volatile int16_t *M3_Motor_current_bus = ((int16_t *)&SPI_tx_buffer[2]);
static volatile int16_t *M3_Motor_currentPhA = (((int16_t *)&SPI_tx_buffer[2])+1);
static volatile int16_t *M3_Motor_currentPhB = ((int16_t *)&SPI_tx_buffer[3]);
static volatile int16_t *M3_Motor_currentPhC = (((int16_t *)&SPI_tx_buffer[3])+1);
static volatile int16_t *M3_Motor__hallState = ((int16_t *)&SPI_tx_buffer[4]);
static volatile int16_t *M3_Motor_dutyCycle = (((int16_t *)&SPI_tx_buffer[4])+1);
/* Motor 4*/
static volatile uint8_t *M4_Status = (uint8_t *)&SPI_tx_buffer[20]; //1 byte - 20 of 64
static volatile uint8_t *M4_Mode = (uint8_t *)&SPI_tx_buffer[21]; //1 byte - 21 of 64
static volatile int16_t *M4_Joint_rel_position = (int16_t *)&SPI_tx_buffer[22]; //2 byte - 22 of 64
static volatile int16_t *M4_Joint_abs_position = (int16_t *)&SPI_tx_buffer[24]; //2 byte - 24 of 64
static volatile int16_t *M4_Motor_speed = (int16_t *)&SPI_tx_buffer[26]; //2 byte - 26 of 64
static volatile int16_t *M4_Motor_current_bus = (int16_t *)&SPI_tx_buffer[28]; //2 byte - 28 of 64
static volatile int16_t *M4_Motor_currentPhA = (int16_t *)&SPI_tx_buffer[30]; //2 byte - 30 of 64
static volatile int16_t *M4_Motor_currentPhB = (int16_t *)&SPI_tx_buffer[32]; //2 byte - 32 of 64
static volatile int16_t *M4_Motor_currentPhC = (int16_t *)&SPI_tx_buffer[34]; //2 byte - 34 of 64
static volatile int16_t *M4_Motor__hallState = (int16_t *)&SPI_tx_buffer[36]; //2 byte - 36 of 64
static volatile int16_t *M4_Motor_dutyCycle = (int16_t *)&SPI_tx_buffer[38]; //2 byte - 38 of 64
static volatile uint8_t *M4_Status = (uint8_t *)&SPI_tx_buffer[5];
static volatile uint8_t *M4_Mode = (((uint8_t *)&SPI_tx_buffer[5])+1);
static volatile int16_t *M4_Joint_rel_position = (((int16_t *)&SPI_tx_buffer[5])+1);
static volatile int16_t *M4_Joint_abs_position = ((int16_t *)&SPI_tx_buffer[6]);
static volatile int16_t *M4_Motor_speed = (((int16_t *)&SPI_tx_buffer[6])+1);
static volatile int16_t *M4_Motor_current_bus = ((int16_t *)&SPI_tx_buffer[7]);
static volatile int16_t *M4_Motor_currentPhA = (((int16_t *)&SPI_tx_buffer[7])+1);
static volatile int16_t *M4_Motor_currentPhB = ((int16_t *)&SPI_tx_buffer[8]);
static volatile int16_t *M4_Motor_currentPhC = (((int16_t *)&SPI_tx_buffer[8])+1);
static volatile int16_t *M4_Motor__hallState = ((int16_t *)&SPI_tx_buffer[9]);
static volatile int16_t *M4_Motor_dutyCycle = (((int16_t *)&SPI_tx_buffer[9])+1);
/* IMU */
static volatile int16_t *q_x0 = (int16_t *)&SPI_tx_buffer[40]; //2 byte - 40 of 64
static volatile int16_t *q_y0 = (int16_t *)&SPI_tx_buffer[42]; //2 byte - 42 of 64
static volatile int16_t *q_z0 = (int16_t *)&SPI_tx_buffer[44]; //2 byte - 44 of 64
static volatile int16_t *q_w0 = (int16_t *)&SPI_tx_buffer[46]; //2 byte - 46 of 64
static volatile int16_t *q_x0 = (int16_t *)&SPI_tx_buffer[10];
static volatile int16_t *q_y0 = (((int16_t *)&SPI_tx_buffer[10])+1);
static volatile int16_t *q_z0 = (int16_t *)&SPI_tx_buffer[11];
static volatile int16_t *q_w0 = (((int16_t *)&SPI_tx_buffer[11])+1);
/* EMG */
static volatile int16_t *FSR_CH1 = (int16_t *)&SPI_tx_buffer[48]; //2 byte - 48 of 64
static volatile int16_t *FSR_CH2 = (int16_t *)&SPI_tx_buffer[50]; //2 byte - 50 of 64
static volatile int16_t *FSR_CH3 = (int16_t *)&SPI_tx_buffer[52]; //2 byte - 52 of 64
static volatile int16_t *FSR_CH4 = (int16_t *)&SPI_tx_buffer[54]; //2 byte - 54 of 64
static volatile int16_t *FSR_CH5 = (int16_t *)&SPI_tx_buffer[56]; //2 byte - 56 of 64
static volatile int16_t *Pressure_CH1 = (int16_t *)&SPI_tx_buffer[58]; //2 byte - 58 of 64
static volatile int16_t *Pressure_CH2 = (int16_t *)&SPI_tx_buffer[60]; //2 byte - 60 of 64
static volatile int16_t *Pressure_CH3 = (int16_t *)&SPI_tx_buffer[62]; //2 byte - 62 of 64
static volatile int16_t *FSR_CH1 = (int16_t *)&SPI_tx_buffer[12]; //2 byte - 48 of 64
static volatile int16_t *FSR_CH2 = (((int16_t *)&SPI_tx_buffer[12])+1);
static volatile int16_t *FSR_CH3 = (int16_t *)&SPI_tx_buffer[13]; //2 byte - 52 of 64
static volatile int16_t *FSR_CH4 = (((int16_t *)&SPI_tx_buffer[13])+1);
static volatile int16_t *FSR_CH5 = (int16_t *)&SPI_tx_buffer[14]; //2 byte - 56 of 64
static volatile int16_t *Pressure_CH1 = (((int16_t *)&SPI_tx_buffer[14])+1);
static volatile int16_t *Pressure_CH2 = (int16_t *)&SPI_tx_buffer[15]; //2 byte - 60 of 64
static volatile int16_t *Pressure_CH3 = (((int16_t *)&SPI_tx_buffer[15])+1);
//rx_buffer
///* Motor 3*/
static volatile uint8_t *M3_Control_mode = (uint8_t *)&SPI_rx_buffer[0]; //1 byte - 0 of 32
static volatile uint8_t *M3_Control_set = (uint8_t *)&SPI_rx_buffer[1]; //1 byte - 1 of 32
static volatile int16_t *M3_Desired_pos = (int16_t *)&SPI_rx_buffer[2]; //2 byte - 2 of 32
static volatile int16_t *M3_Desired_speed = (int16_t *)&SPI_rx_buffer[4]; //2 byte - 4 of 32
static volatile int16_t *M3_Desired_current = (int16_t *)&SPI_rx_buffer[6]; //2 byte - 6 of 32
static volatile int16_t *M3_Max_pos = (int16_t *)&SPI_rx_buffer[8]; //2 byte - 8 of 32
static volatile int16_t *M3_Max_velocity = (int16_t *)&SPI_rx_buffer[10]; //2 byte - 10 of 32
static volatile int16_t *M3_Max_current = (int16_t *)&SPI_rx_buffer[12]; //2 byte - 12 of 32
static volatile int16_t *M3_Spare = (int16_t *)&SPI_rx_buffer[14]; //2 byte - 14 of 32
static volatile uint8_t *M3_Control_mode = (uint8_t *)&SPI_rx_buffer[0]; //1 byte - 0 of 32
static volatile uint8_t *M3_Control_set = (((uint8_t *)&SPI_rx_buffer[0])+1); //1 byte - 1 of 32
static volatile int16_t *M3_Desired_pos = ((int16_t *)&SPI_rx_buffer[0]+1); //2 byte - 2 of 32
static volatile int16_t *M3_Desired_speed = (int16_t *)&SPI_rx_buffer[1]; //2 byte - 4 of 32
static volatile int16_t *M3_Desired_current = ((int16_t *)&SPI_rx_buffer[1]+1); //2 byte - 6 of 32
static volatile int16_t *M3_Max_pos = (int16_t *)&SPI_rx_buffer[2]; //2 byte - 8 of 32
static volatile int16_t *M3_Max_velocity = ((int16_t *)&SPI_rx_buffer[2]+1); //2 byte - 10 of 32
static volatile int16_t *M3_Max_current = (int16_t *)&SPI_rx_buffer[3]; //2 byte - 12 of 32
static volatile int16_t *M3_Spare = ((int16_t *)&SPI_rx_buffer[3]+1); //2 byte - 14 of 32
///* Motor 4*/
static volatile uint8_t *M4_Control_mode = (int16_t *)&SPI_rx_buffer[16]; //1 byte - 16 of 32
static volatile uint8_t *M4_Control_set = (int16_t *)&SPI_rx_buffer[17]; //1 byte - 17 of 32
static volatile int16_t *M4_Desired_pos = (int16_t *)&SPI_rx_buffer[18]; //2 byte - 18 of 32
static volatile int16_t *M4_Desired_speed = (int16_t *)&SPI_rx_buffer[20]; //2 byte - 20 of 32
static volatile int16_t *M4_Desired_current = (int16_t *)&SPI_rx_buffer[22]; //2 byte - 22 of 32
static volatile int16_t *M4_Max_pos = (int16_t *)&SPI_rx_buffer[24]; //2 byte - 24 of 32
static volatile int16_t *M4_Max_velocity = (int16_t *)&SPI_rx_buffer[26]; //2 byte - 26 of 32
static volatile int16_t *M4_Max_current = (int16_t *)&SPI_rx_buffer[28]; //2 byte - 28 of 32
static volatile int16_t *M4_Spare = (int16_t *)&SPI_rx_buffer[30]; //2 byte - 30 of 32
static volatile uint8_t *M4_Control_mode = (uint8_t *)&SPI_rx_buffer[4]; //1 byte - 16 of 32
static volatile uint8_t *M4_Control_set = (((uint8_t *)&SPI_rx_buffer[4])+1); //1 byte - 17 of 32
static volatile int16_t *M4_Desired_pos = ((int16_t *)&SPI_rx_buffer[4]+1); //2 byte - 18 of 32
static volatile int16_t *M4_Desired_speed = (int16_t *)&SPI_rx_buffer[5]; //2 byte - 20 of 32
static volatile int16_t *M4_Desired_current = ((int16_t *)&SPI_rx_buffer[5]+1); //2 byte - 22 of 32
static volatile int16_t *M4_Max_pos = (int16_t *)&SPI_rx_buffer[6]; //2 byte - 24 of 32
static volatile int16_t *M4_Max_velocity = ((int16_t *)&SPI_rx_buffer[6]+1); //2 byte - 26 of 32
static volatile int16_t *M4_Max_current = (int16_t *)&SPI_rx_buffer[7]; //2 byte - 28 of 32
static volatile int16_t *M4_Spare = ((int16_t *)&SPI_rx_buffer[7]+1); //2 byte - 30 of 32
////tx_buffer
///* Motor 3*/
//static volatile uint8_t *M3_Status = (uint8_t *)&SPI_tx_buffer[0]; //1 byte - 0 of 64
//static volatile uint8_t *M3_Mode = (uint8_t *)&SPI_tx_buffer[1]; //1 byte - 1 of 64
//static volatile int16_t *M3_Joint_rel_position = (int16_t *)&SPI_tx_buffer[2]; //2 byte - 2 of 64
//static volatile int16_t *M3_Joint_abs_position = (int16_t *)&SPI_tx_buffer[4]; //2 byte - 4 of 64
//static volatile int16_t *M3_Motor_speed = (int16_t *)&SPI_tx_buffer[6]; //2 byte - 6 of 64
//static volatile int16_t *M3_Motor_current_bus = (int16_t *)&SPI_tx_buffer[8]; //2 byte - 8 of 64
//static volatile int16_t *M3_Motor_currentPhA = (int16_t *)&SPI_tx_buffer[10]; //2 byte - 10 of 64
//static volatile int16_t *M3_Motor_currentPhB = (int16_t *)&SPI_tx_buffer[12]; //2 byte - 12 of 64
//static volatile int16_t *M3_Motor_currentPhC = (int16_t *)&SPI_tx_buffer[14]; //2 byte - 14 of 64
//static volatile int16_t *M3_Motor__hallState = (int16_t *)&SPI_tx_buffer[16]; //2 byte - 16 of 64
//static volatile int16_t *M3_Motor_dutyCycle = (int16_t *)&SPI_tx_buffer[18]; //2 byte - 18 of 64
///* Motor 4*/
//static volatile uint8_t *M4_Status = (uint8_t *)&SPI_tx_buffer[20]; //1 byte - 20 of 64
//static volatile uint8_t *M4_Mode = (uint8_t *)&SPI_tx_buffer[21]; //1 byte - 21 of 64
//static volatile int16_t *M4_Joint_rel_position = (int16_t *)&SPI_tx_buffer[22]; //2 byte - 22 of 64
//static volatile int16_t *M4_Joint_abs_position = (int16_t *)&SPI_tx_buffer[24]; //2 byte - 24 of 64
//static volatile int16_t *M4_Motor_speed = (int16_t *)&SPI_tx_buffer[26]; //2 byte - 26 of 64
//static volatile int16_t *M4_Motor_current_bus = (int16_t *)&SPI_tx_buffer[28]; //2 byte - 28 of 64
//static volatile int16_t *M4_Motor_currentPhA = (int16_t *)&SPI_tx_buffer[30]; //2 byte - 30 of 64
//static volatile int16_t *M4_Motor_currentPhB = (int16_t *)&SPI_tx_buffer[32]; //2 byte - 32 of 64
//static volatile int16_t *M4_Motor_currentPhC = (int16_t *)&SPI_tx_buffer[34]; //2 byte - 34 of 64
//static volatile int16_t *M4_Motor__hallState = (int16_t *)&SPI_tx_buffer[36]; //2 byte - 36 of 64
//static volatile int16_t *M4_Motor_dutyCycle = (int16_t *)&SPI_tx_buffer[38]; //2 byte - 38 of 64
///* IMU */
//static volatile int16_t *q_x0 = (int16_t *)&SPI_tx_buffer[40]; //2 byte - 40 of 64
//static volatile int16_t *q_y0 = (int16_t *)&SPI_tx_buffer[42]; //2 byte - 42 of 64
//static volatile int16_t *q_z0 = (int16_t *)&SPI_tx_buffer[44]; //2 byte - 44 of 64
//static volatile int16_t *q_w0 = (int16_t *)&SPI_tx_buffer[46]; //2 byte - 46 of 64
///* EMG */
//static volatile int16_t *FSR_CH1 = (int16_t *)&SPI_tx_buffer[48]; //2 byte - 48 of 64
//static volatile int16_t *FSR_CH2 = (int16_t *)&SPI_tx_buffer[50]; //2 byte - 50 of 64
//static volatile int16_t *FSR_CH3 = (int16_t *)&SPI_tx_buffer[52]; //2 byte - 52 of 64
//static volatile int16_t *FSR_CH4 = (int16_t *)&SPI_tx_buffer[54]; //2 byte - 54 of 64
//static volatile int16_t *FSR_CH5 = (int16_t *)&SPI_tx_buffer[56]; //2 byte - 56 of 64
//static volatile int16_t *Pressure_CH1 = (int16_t *)&SPI_tx_buffer[58]; //2 byte - 58 of 64
//static volatile int16_t *Pressure_CH2 = (int16_t *)&SPI_tx_buffer[60]; //2 byte - 60 of 64
//static volatile int16_t *Pressure_CH3 = (int16_t *)&SPI_tx_buffer[62]; //2 byte - 62 of 64
//
////rx_buffer
/////* Motor 3*/
//static volatile uint8_t *M3_Control_mode = (uint8_t *)&SPI_rx_buffer[0]; //1 byte - 0 of 32
//static volatile uint8_t *M3_Control_set = (uint8_t *)&SPI_rx_buffer[1]; //1 byte - 1 of 32
//static volatile int16_t *M3_Desired_pos = (int16_t *)&SPI_rx_buffer[2]; //2 byte - 2 of 32
//static volatile int16_t *M3_Desired_speed = (int16_t *)&SPI_rx_buffer[4]; //2 byte - 4 of 32
//static volatile int16_t *M3_Desired_current = (int16_t *)&SPI_rx_buffer[6]; //2 byte - 6 of 32
//static volatile int16_t *M3_Max_pos = (int16_t *)&SPI_rx_buffer[8]; //2 byte - 8 of 32
//static volatile int16_t *M3_Max_velocity = (int16_t *)&SPI_rx_buffer[10]; //2 byte - 10 of 32
//static volatile int16_t *M3_Max_current = (int16_t *)&SPI_rx_buffer[12]; //2 byte - 12 of 32
//static volatile int16_t *M3_Spare = (int16_t *)&SPI_rx_buffer[14]; //2 byte - 14 of 32
/////* Motor 4*/
//static volatile uint8_t *M4_Control_mode = (int16_t *)&SPI_rx_buffer[16]; //1 byte - 16 of 32
//static volatile uint8_t *M4_Control_set = (int16_t *)&SPI_rx_buffer[17]; //1 byte - 17 of 32
//static volatile int16_t *M4_Desired_pos = (int16_t *)&SPI_rx_buffer[18]; //2 byte - 18 of 32
//static volatile int16_t *M4_Desired_speed = (int16_t *)&SPI_rx_buffer[20]; //2 byte - 20 of 32
//static volatile int16_t *M4_Desired_current = (int16_t *)&SPI_rx_buffer[22]; //2 byte - 22 of 32
//static volatile int16_t *M4_Max_pos = (int16_t *)&SPI_rx_buffer[24]; //2 byte - 24 of 32
//static volatile int16_t *M4_Max_velocity = (int16_t *)&SPI_rx_buffer[26]; //2 byte - 26 of 32
//static volatile int16_t *M4_Max_current = (int16_t *)&SPI_rx_buffer[28]; //2 byte - 28 of 32
//static volatile int16_t *M4_Spare = (int16_t *)&SPI_rx_buffer[30]; //2 byte - 30 of 32
static void update_telemetry(void)
@@ -91,7 +157,8 @@ static void update_telemetry(void)
//*M3_Mode = 0;
/* Motor 1 */
*M3_Status = Motor1.motor_state.currentstate;
*M3_Status = Motor1.motor_state.fault;
*M3_Mode = Motor1.motor_state.currentstate;
*M3_Joint_rel_position = Motor1.motor_status.Num_Steps;
//*M3_Joint_abs_position = ((int16_t *)&QSPI_tx_buffer[1];
//*M3_Motor_speed = (((int16_t *)&QSPI_tx_buffer[1]+1);
@@ -104,7 +171,8 @@ static void update_telemetry(void)
*M3_Motor_speed = (int16_t)Motor1.motor_status.calc_rpm;
//*M3_Joint_abs_position = ;
/* Motor 2 */
*M4_Status = Motor2.motor_state.currentstate;
*M4_Status = Motor2.motor_state.fault;
*M4_Mode = Motor2.motor_state.currentstate;
*M4_Joint_rel_position = Motor2.motor_status.Num_Steps;
//*M3_Joint_abs_position = ((int16_t *)&QSPI_tx_buffer[1];
//*M3_Motor_speed = (((int16_t *)&QSPI_tx_buffer[1]+1);
@@ -134,6 +202,7 @@ static void update_setpoints(void)
Motor2.motor_setpoints.max_torque = *M4_Max_current;
Motor2.motor_setpoints.max_velocity = *M4_Max_velocity;
volatile int y = 0;
//volatile uint8_t a = *M3_Control_mode;
//volatile uint8_t b = *M3_Control_set;
//volatile int16_t c = *M3_Desired_pos;

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

@@ -212,7 +212,7 @@
<AcmeProjectActionInfo Action="File" Source="config/hpl_adc_config.h" IsConfig="true" Hash="xpUWpQK+5c6kRc3rDlR+Qw" />
<AcmeProjectActionInfo Action="File" Source="config/hpl_ccl_config.h" IsConfig="true" Hash="Q1yijLwNXjFOsGrwEEma+g" />
<AcmeProjectActionInfo Action="File" Source="config/hpl_cmcc_config.h" IsConfig="true" Hash="bmtxQ8rLloaRtAo2HeXZRQ" />
<AcmeProjectActionInfo Action="File" Source="config/hpl_dmac_config.h" IsConfig="true" Hash="fD/O4h+JMsc7H2g0bo8JyQ" />
<AcmeProjectActionInfo Action="File" Source="config/hpl_dmac_config.h" IsConfig="true" Hash="h12kdZpbVL8Ar9UXmU32Vg" />
<AcmeProjectActionInfo Action="File" Source="config/hpl_eic_config.h" IsConfig="true" Hash="xGTWc3ZL07K/tP/YF7YzPw" />
<AcmeProjectActionInfo Action="File" Source="config/hpl_evsys_config.h" IsConfig="true" Hash="/3bNiu/UgpvPbmvfRA+w3g" />
<AcmeProjectActionInfo Action="File" Source="config/hpl_gclk_config.h" IsConfig="true" Hash="fvc5nhPTGTNHCTNlzs6nhA" />
@@ -389,6 +389,7 @@
<armgcc.compiler.directories.IncludePaths>
<ListValues>
<Value>%24(PackRepoDir)\arm\CMSIS\5.4.0\CMSIS\Core\Include\</Value>
<Value>%24(PackRepoDir)\atmel\SAME51_DFP\1.1.139\include</Value>
<Value>../Config</Value>
<Value>../</Value>
<Value>../examples</Value>
@@ -413,7 +414,6 @@
<Value>../hpl/tcc</Value>
<Value>../hri</Value>
<Value>../bosch_sensor</Value>
<Value>%24(PackRepoDir)\atmel\SAME51_DFP\1.1.139\include</Value>
</ListValues>
</armgcc.compiler.directories.IncludePaths>
<armgcc.compiler.optimization.PrepareFunctionsForGarbageCollection>True</armgcc.compiler.optimization.PrepareFunctionsForGarbageCollection>
@@ -440,6 +440,7 @@
<armgcc.assembler.general.IncludePaths>
<ListValues>
<Value>%24(PackRepoDir)\arm\CMSIS\5.4.0\CMSIS\Core\Include\</Value>
<Value>%24(PackRepoDir)\atmel\SAME51_DFP\1.1.139\include</Value>
<Value>../Config</Value>
<Value>../</Value>
<Value>../examples</Value>
@@ -464,13 +465,13 @@
<Value>../hpl/tcc</Value>
<Value>../hri</Value>
<Value>../bosch_sensor</Value>
<Value>%24(PackRepoDir)\atmel\SAME51_DFP\1.1.139\include</Value>
</ListValues>
</armgcc.assembler.general.IncludePaths>
<armgcc.assembler.debugging.DebugLevel>Default (-g)</armgcc.assembler.debugging.DebugLevel>
<armgcc.preprocessingassembler.general.IncludePaths>
<ListValues>
<Value>%24(PackRepoDir)\arm\CMSIS\5.4.0\CMSIS\Core\Include\</Value>
<Value>%24(PackRepoDir)\atmel\SAME51_DFP\1.1.139\include</Value>
<Value>../Config</Value>
<Value>../</Value>
<Value>../examples</Value>
@@ -495,7 +496,6 @@
<Value>../hpl/tcc</Value>
<Value>../hri</Value>
<Value>../bosch_sensor</Value>
<Value>%24(PackRepoDir)\atmel\SAME51_DFP\1.1.139\include</Value>
</ListValues>
</armgcc.preprocessingassembler.general.IncludePaths>
<armgcc.preprocessingassembler.debugging.DebugLevel>Default (-Wa,-g)</armgcc.preprocessingassembler.debugging.DebugLevel>

114
2_Motor_Slave/Motor_Slave/Motor_Slave/bldc.c
View File

@@ -9,7 +9,7 @@
#include "statemachine.h"
#include "utilities.h"
#include "Ethercat_SlaveDef.h"
void motor_StateMachine(BLDCMotor_t* const motor)
{
@@ -38,7 +38,7 @@ void motor_StateMachine(BLDCMotor_t* const motor)
motor->motor_state.currentstate = MOTOR_PVI_CTRL_STATE;
break;
case MOTOR_OPEN_LOOP_STATE:
BLDC_runOpenLoop(motor, 0);
BLDC_runOpenLoop(motor, 350);
calculate_motor_speed(motor);
motor->motor_state.previousstate = motor->motor_state.currentstate;
break;
@@ -77,6 +77,9 @@ void motor_StateMachine(BLDCMotor_t* const motor)
if(motor->regulation_loop_count > 23) motor->regulation_loop_count = 0;
else motor->regulation_loop_count++;
break;
case MOTOR_FAULT:
disable_phases(motor);
break;
} //end switch (motor->motor_state.currentstate)
// ----------------------------------------------------------------------
@@ -357,11 +360,25 @@ void calculate_motor_speed(BLDCMotor_t* const motor)
}
void disable_phases(BLDCMotor_t* const motor)
{
Tcc * tmp = (Tcc *)motor->motor_param->pwm_desc->device.hw;
tmp->PATTBUF.reg = DISABLE_PATTERN;
}
//------------------------------------------------------------------------------
// pi current control
//------------------------------------------------------------------------------
void BLDC_runCurrentCntl(BLDCMotor_t *motor, const float32_t curfbk, const float32_t curRef)
{
if (curfbk > DEVICE_SHUNT_CURRENT_A)
{
motor->motor_state.currentstate = MOTOR_FAULT;
motor->motor_state.fault = MOTOR_CURRENT_OVERSCALE;
}
motor->controllers.Pi_Idc.Fbk_pu = f_clamp(curfbk, -DEVICE_SHUNT_CURRENT_A, DEVICE_SHUNT_CURRENT_A); // Clamped to max current sensor readingspeedfbk;
motor->controllers.Pi_Idc.Ref_pu = f_clamp(curRef, -motor->motor_param->motor_Max_Current_IDC_A,
motor->motor_param->motor_Max_Current_IDC_A); // Clamp desired to Motor Max Current i_ref_clamped;
@@ -460,6 +477,16 @@ volatile uint8_t readHallSensorM1(void)
motor_read = (motor_read & M1_HALL_A_MASK) | (uint8_t)((PORT->Group[M1_HALL_A_GROUP].IN.reg & M1_HALL_A_PORT)>>(M1_HALL_A_LSR));
motor_read = (motor_read & M1_HALL_B_MASK) | (uint8_t)((PORT->Group[M1_HALL_B_GROUP].IN.reg & M1_HALL_B_PORT)>>(M1_HALL_B_LSR));
motor_read = (motor_read & M1_HALL_C_MASK) | (uint8_t)((PORT->Group[M1_HALL_C_GROUP].IN.reg & M1_HALL_C_PORT)>>(M1_HALL_C_LSR));
//if(motor_read == INVALID_HALL_7) {
//Motor1.motor_state.currentstate = MOTOR_FAULT;
//Motor1.motor_state.fault = MOTOR_HALLSENSORINVALID;
////applicationStatus.currentstate = APP_FAULT;
//}
return motor_read;
//volatile uint8_t a = gpio_get_pin_level(M1_HALL_A_PIN);
@@ -479,13 +506,14 @@ volatile uint8_t readHallSensorM2(void)
motor_read = (motor_read & M2_HALL_B_MASK) | (uint8_t)((PORT->Group[M2_HALL_B_GROUP].IN.reg & M2_HALL_B_PORT)>>(M2_HALL_B_LSR));
motor_read = (motor_read & M2_HALL_C_MASK) | (uint8_t)((PORT->Group[M2_HALL_C_GROUP].IN.reg & M2_HALL_C_PORT)>>(M2_HALL_C_LSR));
return motor_read;
//if(((motor_read == INVALID_HALL_0) || (motor_read == INVALID_HALL_7))) {
//Motor2.motor_state.fault = MOTOR_HALLSENSORINVALID;
//if(motor_read == INVALID_HALL_7) {
//Motor2.motor_state.currentstate = MOTOR_FAULT;
//Motor2.motor_state.fault = MOTOR_HALLSENSORINVALID;
////applicationStatus.currentstate = APP_FAULT;
//}
return motor_read;
//volatile uint8_t a = gpio_get_pin_level(M2_HALL_A_PIN);
//volatile uint8_t b = gpio_get_pin_level(M2_HALL_B_PIN);
//volatile uint8_t c = gpio_get_pin_level(M2_HALL_C_PIN);
@@ -503,15 +531,17 @@ volatile uint8_t readHallSensorM2(void)
// ----------------------------------------------------------------------
void read_zero_current_offset_value(BLDCMotor_t *motor1, BLDCMotor_t *motor2)
{
uint32_t phase_A_zero_current_offset_temp = 0;
uint32_t phase_B_zero_current_offset_temp = 0;
volatile uint16_t zero_current_offset_temp[2] = {0,0};
uint8_t samples = 32;
volatile int32_t phase_A_zero_current_offset_temp = 0;
volatile int32_t phase_B_zero_current_offset_temp = 0;
volatile int16_t zero_current_offset_temp[2] = {0,0};
const uint8_t samples = 16;
uint8_t i;
// ------------------------- Motor 1 ---------------------------------
// ------------------------------------------------------------------
// Motor 1
// -------------------------------------------------------------------
adc_sync_enable_channel(&ADC_1, 9);
//adc_sync_enable_channel(&ADC_1, 0);
/* Single ended */
//ADC1->INPUTCTRL.reg = 0x1809;
@@ -521,35 +551,33 @@ void read_zero_current_offset_value(BLDCMotor_t *motor1, BLDCMotor_t *motor2)
for (i=0; i<samples; i++)
{
volatile uint16_t zero_current_offset_temp[2] = {0,0};
while (ADC1->STATUS.bit.ADCBUSY) {}; /* Wait for bus synchronization. */
ADC1->SWTRIG.bit.START = true; /* Start the ADC using a software trigger. */
while (ADC1->INTFLAG.bit.RESRDY == 0); /* Wait for the result ready flag to be set. */
while (ADC1->INTFLAG.bit.RESRDY == 0){}; /* Wait for the result ready flag to be set. */
zero_current_offset_temp[0] = (int16_t)ADC1->RESULT.reg; /* Read the value. */
ADC1->INTFLAG.reg = ADC_INTFLAG_RESRDY; /* Clear the flag. */
zero_current_offset_temp[0] = ADC1->RESULT.reg; /* Read the value. */
phase_A_zero_current_offset_temp += zero_current_offset_temp[0];
phase_A_zero_current_offset_temp += (int32_t)zero_current_offset_temp[0];
}
/* Set Motor Variables */
motor1->Voffset_lsb.A = phase_A_zero_current_offset_temp/samples;
adc_sync_disable_channel(&ADC_1, 9);
adc_sync_enable_channel(&ADC_1, 8);
/* Single ended */
//ADC1->INPUTCTRL.reg = 0x1808;
/* Differential */
ADC1->INPUTCTRL.reg = 0x0088;
while (ADC1->STATUS.bit.ADCBUSY) {}; /* Wait for bus synchronization. */
for (i=0; i<samples; i++)
{
while (ADC1->STATUS.bit.ADCBUSY) {}; /* Wait for bus synchronization. */
ADC1->SWTRIG.bit.START = true; /* Start the ADC using a software trigger. */
while (ADC1->INTFLAG.bit.RESRDY == 0); /* Wait for the result ready flag to be set. */
ADC1->INTFLAG.reg = ADC_INTFLAG_RESRDY; /* Clear the flag. */
zero_current_offset_temp[1] = ADC1->RESULT.reg; /* Read the value. */
phase_B_zero_current_offset_temp += zero_current_offset_temp[1];
while (ADC1->INTFLAG.bit.RESRDY == 0){}; /* Wait for the result ready flag to be set. */
zero_current_offset_temp[1] = (int16_t)ADC1->RESULT.reg; /* Read the value. */
ADC1->INTFLAG.reg = ADC_INTFLAG_RESRDY; /* Clear the flag. */
phase_B_zero_current_offset_temp += (int32_t)zero_current_offset_temp[1];
}
@@ -557,13 +585,23 @@ void read_zero_current_offset_value(BLDCMotor_t *motor1, BLDCMotor_t *motor2)
motor1->Voffset_lsb.B = phase_B_zero_current_offset_temp/samples;
adc_sync_disable_channel(&ADC_1, 8);
adc_sync_enable_channel(&ADC_1, 7);
//adc_sync_enable_channel(&ADC_1, 0);
if ((abs(motor1->Voffset_lsb.A) > MAX_CUR_SENSE_OFFSET) || (abs(motor1->Voffset_lsb.B) > MAX_CUR_SENSE_OFFSET))
{
motor1->motor_state.currentstate = MOTOR_FAULT;
motor1->motor_state.fault = MOTOR_CURRENT_SENSOR;
}
// ------------------------------------------------------------------
// Motor 2
// -------------------------------------------------------------------
phase_A_zero_current_offset_temp = 0;
phase_B_zero_current_offset_temp = 0;
phase_B_zero_current_offset_temp = 0;
adc_sync_enable_channel(&ADC_1, 7);
/* Single ended */
//ADC1->INPUTCTRL.reg = 0x1807;
/* Differential */
@@ -572,39 +610,47 @@ void read_zero_current_offset_value(BLDCMotor_t *motor1, BLDCMotor_t *motor2)
for (i=0; i<samples; i++)
{
volatile uint16_t zero_current_offset_temp[2] = {0,0};
while (ADC1->STATUS.bit.ADCBUSY) {}; /* Wait for bus synchronization. */
ADC1->SWTRIG.bit.START = true; /* Start the ADC using a software trigger. */
while (ADC1->INTFLAG.bit.RESRDY == 0); /* Wait for the result ready flag to be set. */
ADC1->INTFLAG.reg = ADC_INTFLAG_RESRDY; /* Clear the flag. */
zero_current_offset_temp[0] = ADC1->RESULT.reg; /* Read the value. */
zero_current_offset_temp[0] = (int16_t)ADC1->RESULT.reg; /* Read the value. */
phase_A_zero_current_offset_temp += zero_current_offset_temp[0];
phase_A_zero_current_offset_temp += (int32_t)zero_current_offset_temp[0];
}
/* Set Motor Variables */
motor2->Voffset_lsb.A = phase_A_zero_current_offset_temp/samples;
adc_sync_disable_channel(&ADC_1, 7);
adc_sync_enable_channel(&ADC_1, 6);
/* Single ended */
//ADC1->INPUTCTRL.reg = 0x1806;
/* Differential */
ADC1->INPUTCTRL.reg = 0x0086;
while (ADC1->STATUS.bit.ADCBUSY) {}; /* Wait for bus synchronization. */
for (i=0; i<samples; i++)
{
while (ADC1->STATUS.bit.ADCBUSY) {}; /* Wait for bus synchronization. */
ADC1->SWTRIG.bit.START = true; /* Start the ADC using a software trigger. */
while (ADC1->INTFLAG.bit.RESRDY == 0); /* Wait for the result ready flag to be set. */
ADC1->INTFLAG.reg = ADC_INTFLAG_RESRDY; /* Clear the flag. */
zero_current_offset_temp[1] = ADC1->RESULT.reg; /* Read the value. */
zero_current_offset_temp[1] = (int16_t)ADC1->RESULT.reg; /* Read the value. */
phase_B_zero_current_offset_temp += zero_current_offset_temp[1];
phase_B_zero_current_offset_temp += (int32_t)zero_current_offset_temp[1];
}
/* Set Motor Variables */
motor2->Voffset_lsb.B = phase_B_zero_current_offset_temp/samples;
adc_sync_disable_channel(&ADC_1, 6);
//adc_sync_disable_channel(&ADC_1, 0);
if ((abs(motor2->Voffset_lsb.A) > MAX_CUR_SENSE_OFFSET) || (abs(motor2->Voffset_lsb.B) > MAX_CUR_SENSE_OFFSET))
{
motor2->motor_state.currentstate = MOTOR_FAULT;
motor2->motor_state.fault = MOTOR_CURRENT_SENSOR;
}
}

4
2_Motor_Slave/Motor_Slave/Motor_Slave/bldc.h
View File

@@ -35,7 +35,7 @@
// ----------------------------------------------------------------------
// ADC Parameters
// ----------------------------------------------------------------------
#define ADC_VOLTAGE_REFERENCE (3.3f)
#define ADC_VOLTAGE_REFERENCE (3.0f)
#define ADC_RESOLUTION (12)
#define ADC_MAX_COUNTS (1<<ADC_RESOLUTION)
#define ADC_LSB_SIZE (ADC_VOLTAGE_REFERENCE/ADC_MAX_COUNTS)
@@ -61,6 +61,7 @@
#define DEVICE_SHUNT_CURRENT_A 2.5f // phase current(PEAK) [A]
#define CURRENT_SENSOR_SENSITIVITY 0.4f //V/A
#define ONEON_CURRENT_SENSOR_SENSITIVITY 2.5f //V/A
#define MAX_CUR_SENSE_OFFSET 150
// ----------------------------------------------------------------------
// global variables
@@ -84,6 +85,7 @@ void BldcInitStruct(BLDCMotor_t* const motor, BLDCMotor_param_t* constmotor_para
void exec_commutation(BLDCMotor_t* const motor);
void select_active_phase(BLDCMotor_t* const Motor);
void calculate_motor_speed(BLDCMotor_t* const motor);
void disable_phases(BLDCMotor_t* const motor);
// ----------------------------------------------------------------------
// Static Functions
// ----------------------------------------------------------------------

25
2_Motor_Slave/Motor_Slave/Motor_Slave/configuration.h
View File

@@ -173,6 +173,9 @@ static void spi_slave_tx_complete_cb(struct _dma_resource *const resource)
void boardToBoardTransferInit(void)
{
hri_sercomspi_set_CTRLB_PLOADEN_bit(SPI_1_MSIF.dev.prvt);
SERCOM1->SPI.CTRLC.bit.ICSPACE = 5;
SERCOM1->SPI.CTRLC.bit.DATA32B= true;
spi_s_sync_enable(&SPI_1_MSIF);
}
@@ -181,24 +184,24 @@ void init_spi_slave_dma_descriptors()
_dma_set_source_address(CONF_SERCOM_1_RECEIVE_DMA_CHANNEL,
(uint32_t *)&(((SercomSpi *)(SPI_1_MSIF.dev.prvt))->DATA.reg));
_dma_set_destination_address(CONF_SERCOM_1_RECEIVE_DMA_CHANNEL, &SPI_rx_buffer[0]);
_dma_set_data_amount(CONF_SERCOM_1_RECEIVE_DMA_CHANNEL, SLAVE_BUFFER_SIZE);
_dma_set_data_amount(CONF_SERCOM_1_RECEIVE_DMA_CHANNEL, SLAVE_BUFFER_SIZE_LONG);
_dma_set_source_address(CONF_SERCOM_1_TRANSMIT_DMA_CHANNEL, &SPI_tx_buffer[0]);
_dma_set_destination_address(CONF_SERCOM_1_TRANSMIT_DMA_CHANNEL,
(uint32_t *)&(((SercomSpi *)(SPI_1_MSIF.dev.prvt))->DATA.reg));
_dma_set_data_amount(CONF_SERCOM_1_TRANSMIT_DMA_CHANNEL, SLAVE_BUFFER_SIZE);
_dma_set_data_amount(CONF_SERCOM_1_TRANSMIT_DMA_CHANNEL, SLAVE_BUFFER_SIZE_LONG);
hri_dmacdescriptor_set_BTCTRL_VALID_bit(&_descriptor_section[CONF_SERCOM_1_RECEIVE_DMA_CHANNEL]);
hri_dmacdescriptor_set_BTCTRL_VALID_bit(&_descriptor_section[CONF_SERCOM_1_TRANSMIT_DMA_CHANNEL]);
/* callback */
//struct _dma_resource *resource_rx, *resource_tx;
//_dma_get_channel_resource(&resource_rx, CONF_SERCOM_1_RECEIVE_DMA_CHANNEL);
//_dma_get_channel_resource(&resource_tx, CONF_SERCOM_1_TRANSMIT_DMA_CHANNEL);
//resource_rx->dma_cb.transfer_done = spi_slave_rx_complete_cb;
struct _dma_resource *resource_rx, *resource_tx;
_dma_get_channel_resource(&resource_rx, CONF_SERCOM_1_RECEIVE_DMA_CHANNEL);
_dma_get_channel_resource(&resource_tx, CONF_SERCOM_1_TRANSMIT_DMA_CHANNEL);
resource_rx->dma_cb.transfer_done = b2bTransferComplete_cb;
//resource_tx->dma_cb.transfer_done = spi_slave_tx_complete_cb;
/* Enable DMA transfer complete interrupt */
//_dma_set_irq_state(CONF_SERCOM_1_RECEIVE_DMA_CHANNEL, DMA_TRANSFER_COMPLETE_CB, true);
//
///* Enable DMA transfer complete interrupt */
_dma_set_irq_state(CONF_SERCOM_1_RECEIVE_DMA_CHANNEL, DMA_TRANSFER_COMPLETE_CB, true);
}
@@ -209,12 +212,12 @@ void spi_s_sync_enable_ss_detect(void *hw, bool state)
NVIC_ClearPendingIRQ((IRQn_Type)SERCOM1_1_IRQn);
NVIC_EnableIRQ((IRQn_Type)SERCOM1_1_IRQn);
if (state) {
hri_sercomspi_set_INTEN_TXC_bit(hw);
//hri_sercomspi_set_INTEN_TXC_bit(hw);
//hri_sercomspi_set_INTEN_SSL_bit(hw);
//hri_sercomspi_set_INTEN_SSL_bit(hw);
//SERCOM_SPI_INTENSET_SSL
} else {
hri_sercomspi_clear_INTEN_TXC_bit(hw);
//hri_sercomspi_clear_INTEN_TXC_bit(hw);
//hri_sercomspi_clear_INTEN_SSL_bit(hw);
}
}

4
2_Motor_Slave/Motor_Slave/Motor_Slave/hpl/dmac/hpl_dmac.c
View File

@@ -216,10 +216,10 @@ static void _dmac_handler(void)
uint8_t channel = hri_dmac_get_INTPEND_reg(DMAC, DMAC_INTPEND_ID_Msk);
struct _dma_resource *tmp_resource = &_resources[channel];
if (hri_dmac_get_INTPEND_TERR_bit(DMAC)) {
if (hri_dmac_get_CHINTFLAG_TERR_bit(DMAC, channel)) {
hri_dmac_clear_CHINTFLAG_TERR_bit(DMAC, channel);
tmp_resource->dma_cb.error(tmp_resource);
} else if (hri_dmac_get_INTPEND_TCMPL_bit(DMAC)) {
} else if (hri_dmac_get_CHINTFLAG_TCMPL_bit(DMAC, channel)) {
hri_dmac_clear_CHINTFLAG_TCMPL_bit(DMAC, channel);
tmp_resource->dma_cb.transfer_done(tmp_resource);
}

13
2_Motor_Slave/Motor_Slave/Motor_Slave/interrupts.h
View File

@@ -59,7 +59,18 @@ static void M2_RESET_BAR(void)
volatile int x = 0;
}
// ----------------------------------------------------------------------
// Master/Slave IF Callback
// ----------------------------------------------------------------------
static void b2bTransferComplete_cb(struct _dma_resource *resource)
{
DMAC->Channel[0].CHCTRLA.reg |= DMAC_CHCTRLA_ENABLE;
DMAC->Channel[3].CHCTRLA.reg |= DMAC_CHCTRLA_ENABLE;
volatile int x = 0;
//PORT->Group[GPIO_PORTB].OUTCLR.reg = (1<<Slave_1->SS_pin);
//gpio_set_pin_level(SPI1_CS, true);
}

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

@@ -86,9 +86,7 @@ void SERCOM1_1_Handler()
//SERCOM1->SPI.INTFLAG.bit.TXC = 0x01;
//SPI_tx_buffer[0] += 1;
//tx_buffer[31] += 1;
DMAC->Channel[CONF_SERCOM_1_RECEIVE_DMA_CHANNEL].CHCTRLA.reg |= DMAC_CHCTRLA_ENABLE;
DMAC->Channel[CONF_SERCOM_1_TRANSMIT_DMA_CHANNEL].CHCTRLA.reg |= DMAC_CHCTRLA_ENABLE;
//_dma_enable_transaction(CONF_SERCOM_1_RECEIVE_DMA_CHANNEL, false);
//_dma_enable_transaction(CONF_SERCOM_1_TRANSMIT_DMA_CHANNEL, false);
@@ -108,7 +106,7 @@ void SERCOM1_3_Handler()
//tx_buffer[0] += 1;
//tx_buffer[31] += 1;
//
//DMAC->Channel[0].CHCTRLA.reg |= DMAC_CHCTRLA_ENABLE;
//DMAC->Channel[1].CHCTRLA.reg |= DMAC_CHCTRLA_ENABLE;
//_dma_enable_transaction(CONF_SERCOM_1_RECEIVE_DMA_CHANNEL, false);
@@ -138,10 +136,10 @@ void enable_NVIC_IRQ(void)
NVIC_SetPriority(ADC1_0_IRQn, 3);
NVIC_EnableIRQ(TCC0_0_IRQn);
NVIC_EnableIRQ(TCC1_0_IRQn);
//NVIC_EnableIRQ(SERCOM1_3_IRQn);
NVIC_EnableIRQ(SERCOM1_3_IRQn);
//NVIC_SetPriority(SERCOM1_3_IRQn, 0);
NVIC_EnableIRQ(SERCOM1_1_IRQn);
NVIC_SetPriority(SERCOM1_1_IRQn, 1);
//NVIC_EnableIRQ(SERCOM1_1_IRQn);
//NVIC_SetPriority(SERCOM1_1_IRQn, 1);
//NVIC_EnableIRQ(SERCOM1_3_IRQn);
//NVIC_EnableIRQ(EIC_5_IRQn);
}
@@ -177,7 +175,7 @@ void APPLICATION_StateMachine(void)
//applicationStatus.currentstate ;
//comms_check();
motor_StateMachine(&Motor1);
//motor_StateMachine(&Motor2);
motor_StateMachine(&Motor2);
break;
case APP_FAULT:
//DisableGateDrivers(&Motor1);
@@ -216,16 +214,19 @@ int main(void)
{
/* Initializes MCU, drivers and middleware */
atmel_start_init();
BldcInitStruct(&Motor1, &FH_22mm24BXTR);
BldcInitStruct(&Motor2, &FH_32mm24BXTR);
BldcInitStruct(&Motor2, &FH_22mm24BXTR_temp);
Motor1.readHall = &readHallSensorM1;
Motor2.readHall = &readHallSensorM2;
read_zero_current_offset_value(&Motor1, &Motor2);
__disable_irq();
//config_qspi();
configure_tcc_pwm();
adc_sync_enable_channel(&ADC_1, 6);
//ECAT_STATE_MACHINE();
adc_init_dma();
//adc_init_dma();
boardToBoardTransferInit();
init_spi_slave_dma_descriptors();
@@ -233,6 +234,7 @@ int main(void)
One_ms_timer_init();
custom_logic_enable();
enable_NVIC_IRQ();
__enable_irq();
DMAC->Channel[CONF_SERCOM_1_RECEIVE_DMA_CHANNEL].CHCTRLA.reg |= DMAC_CHCTRLA_ENABLE;
DMAC->Channel[CONF_SERCOM_1_TRANSMIT_DMA_CHANNEL].CHCTRLA.reg |= DMAC_CHCTRLA_ENABLE;
@@ -250,10 +252,11 @@ int main(void)
/* Replace with your application code */
while (1) {
if (Motor1.timerflags.adc_readings_ready_tic) {process_currents();}
if (Motor1.timerflags.current_loop_tic) {
APPLICATION_StateMachine();
exec_commutation(&Motor1);
////exec_commutation(&Motor2);
exec_commutation(&Motor2);
}
if (Motor1.timerflags.motor_telemetry_flag) {

24
2_Motor_Slave/Motor_Slave/Motor_Slave/motorparameters.h
View File

@@ -163,6 +163,30 @@ const static BLDCMotor_param_t FH_22mm24BXTR = {
.motor_MaxPWM = 800.0,
};
/* Small Motor - 2214S024BXTR*/
const static BLDCMotor_param_t FH_22mm24BXTR_temp = {
.pwm_desc = &PWM_1,
.speedtimer_hw = TC4,
.motor_Poles = 14,
.motor_polePairs = 7,
.motor_commutationStates = 42, //polePairs * 6
.motor_RS_Ohm = 25.9,
.motor_LD_H = 0.003150,
.motor_LQ_H = 0.003150,
.motor_Flux_WB = 0.001575,
.motor_Max_Spd_RPM = 3000,
.motor_MeasureRange_RPM = 3000 * 1.2, //(1.2f * MOTOR_MAX_SPD_RPM)f // give 20% headroom
.motor_Max_Spd_ELEC = (3000/60)*7.0, //(MOTOR_MAX_SPD_RPM/60)*MOTOR_POLEPAIRS
//.motor_Max_Current_IDC_A = 0.368,
.motor_Max_Current_IDC_A = 0.180,
.controller_param.Pid_Speed.Kp = 0.00008f,
.controller_param.Pid_Speed.Ki = 0.0000001f,
//.controller_param.Pid_Speed.Ki = 0.0000001f,
.controller_param.Pi_Pos.Kp = 50.0f,
.controller_param.Pi_Pos.Ki = 0.0f,
.motor_MaxPWM = 800.0,
};
/* Big Motor - 3216W012BXTR */
const static BLDCMotor_param_t FH_32mm12BXTR = {

8
2_Motor_Slave/Motor_Slave/Motor_Slave/statemachine.h
View File

@@ -59,9 +59,11 @@ typedef enum
typedef enum
{
MOTOR_NOFAULT = 0xE1,
MOTOR_HALLSENSORINVALID = 0xE2,
MOTOR_DRIVER_OVER_CURRENT = 0xE3,
MOTOR_NOFAULT = 0x0E,
MOTOR_HALLSENSORINVALID = 0xE1,
MOTOR_DRIVER_OVER_CURRENT = 0xE2,
MOTOR_CURRENT_SENSOR = 0xE3,
MOTOR_CURRENT_OVERSCALE = 0xE4,
} MOTOR_FAULTS_t;
typedef struct MOTOR_STATE