From 7938bfa26c6475eac49a293e5b040c76df5f46cb Mon Sep 17 00:00:00 2001 From: Nicolas Trimborn Date: Sun, 1 Aug 2021 17:39:38 +0200 Subject: [PATCH] added slave files --- .../LAN9252-2_motor_master_mkIII.xml | 755 ++++++++++++++++++ .../Motor_Master/Motor_Master/EtherCAT_QSPI.h | 3 +- .../Motor_Master/EtherCAT_SlaveDef.h | 269 +++++++ .../Motor_Master/Motor_Master/Ethercat_QSPI.c | 2 + .../Motor_Master/Motor_Master/Ethercat_QSPI.h | 38 + .../Motor_Master/Motor_Master.cproj | 21 +- .../Motor_Master/Motor_Master/main.c | 16 +- 7 files changed, 1086 insertions(+), 18 deletions(-) create mode 100644 2_Motor_Master/LAN9252-2_motor_master_mkIII.xml create mode 100644 2_Motor_Master/Motor_Master/Motor_Master/EtherCAT_SlaveDef.h create mode 100644 2_Motor_Master/Motor_Master/Motor_Master/Ethercat_QSPI.h diff --git a/2_Motor_Master/LAN9252-2_motor_master_mkIII.xml b/2_Motor_Master/LAN9252-2_motor_master_mkIII.xml new file mode 100644 index 0000000..023e7a0 --- /dev/null +++ b/2_Motor_Master/LAN9252-2_motor_master_mkIII.xml @@ -0,0 +1,755 @@ + + + + #x000004D8 + Microchip Technology Inc + + + + + LAN9252-2_motor_Master + LAN9252-2_motor_Master + 424DD8020000000000003600000028000000100000000E0000000100180000000000A2020000120B0000120B000000000000000000001306E31306E3190CE42B1FE62B1FE61306E31F13E5190CE42519E51306E31306E3190CE42F24E7190CE41306E31306E31306E31306E35F56EC645CED645CED4137E91F13E5473DE95F57EC3227E71306E3473DE95A51EC271BE61306E31306E31409CA524CC68E8AD74F48C1615CC82218D03E36BF716BCE746FCE453DC01307CE3931BA7D78D27671D1150CB21409CA1712801B1D1D1B1D1D1B1D1D1B1D1D120B891B1D1D1B1D1D1B1D1D1B1D1D120B891B1D1D1B1D1D1B1D1D1B1D1D1712801712807F8080D4D5D5D4D5D5383939120B89545656D4D5D5D4D5D5626464130C89292B2BD4D5D5D4D5D56264641915801712804647471B1D1DAAAAAAD4D5D5130E82383939292B2B717272D4D5D5151183D4D5D57F80801B1D1D7172721E1C81191580464747D4D5D5D4D5D51B1D1D19158A292B2BD4D5D5D4D5D5292B2B1B1B8AD4D5D56264641B1D1D1B1D1D2427821E1D81D4D5D54647476264643839391E208BD4D5D57F8080464747545656242A8BD4D5D59B9C9C292B2BAAAAAA2D3683252882464747D4D5D5D4D5D51B1D1D272D85292B2BD4D5D5D4D5D5292B2B2E37861B1D1DD4D5D5D4D5D5464747394484323BB52324812122822426822526824554C0323883292B822A2D83353C84424CBF3238843940842E32834853865D6EBB5262EB3E43E83334E74147E94349E9535FEB4D56EA5662EB484DEA545DEB636FED545AEA5A63EC6671ED8CA0F290A5F2748AEF6B7BEE5D68EC6874ED788AEF8397F17684EF7986EF8C9FF2818FF1818EF08E9DF18A97F18791F19BA9F3B0C0F691A4F291A2F28390F192A1F29CACF3A3B3F498A6F3A4B3F4AEBDF5B0BEF59EA8F3A3ADF4BBC7F7C4D1F8CAD7F8CED9F9B4C4F6B8C8F6ACB8F59AA3F3B6C1F6C5D2F8C2CDF8CCD7F9D2DDF9D5E0FAD2DAF9D5DCF9DFE7FBE2E9FBE5EBFBE8EEFB0000 + + + + + LAN9252-2_motor_Master + + + + 4096 + 4 + 3 + + + MII + 0 + + + MII + 1 + + #x0012 + + LAN9252-2_motor_Master + Outputs + Inputs + Outputs + Inputs + + #x1a00 + ECAT2MCU + + #x3101 + 1 + 8 + M1__control_mode + BYTE + + + #x3102 + 1 + 8 + M1__control_set + BYTE + + + #x3103 + 1 + 16 + M1__desired_position + INT + + + #x3104 + 1 + 16 + M1__desired_speed + INT + + + #x3105 + 1 + 16 + M1__desired__current + INT + + + #x3106 + 1 + 16 + M1__Max_Pos + INT + + + #x3107 + 1 + 16 + M1__Max_velocity + INT + + + #x3108 + 1 + 16 + M1__Max_current + INT + + + #x3109 + 1 + 16 + M1__Spare + INT + + + #x310A + 1 + 8 + M2__control_mode + BYTE + + + #x310B + 1 + 8 + M2__control_set + BYTE + + + #x310C + 1 + 16 + M2__desired_position + INT + + + #x310D + 1 + 16 + M2__desired_speed + INT + + + #x310E + 1 + 16 + M2__desired__current + INT + + + #x310F + 1 + 16 + M2__Max_Pos + INT + + + #x3110 + 1 + 16 + M2__Max_velocity + INT + + + #x3111 + 1 + 16 + M2__Max_current + INT + + + #x3112 + 1 + 16 + M2__Spare + INT + + + #x3113 + 1 + 8 + M3__control_mode + BYTE + + + #x3114 + 1 + 8 + M3__control_set + BYTE + + + #x3115 + 1 + 16 + M3__desired_position + INT + + + #x3116 + 1 + 16 + M3__desired_speed + INT + + + #x3117 + 1 + 16 + M3__desired__current + INT + + + #x3118 + 1 + 16 + M3__Max_Pos + INT + + + #x3119 + 1 + 16 + M3__Max_velocity + INT + + + #x311A + 1 + 16 + M3__Max_current + INT + + + #x311B + 1 + 16 + M3__Spare + INT + + + #x311C + 1 + 8 + M4__control_mode + BYTE + + + #x311D + 1 + 8 + M4__control_set + BYTE + + + #x311E + 1 + 16 + M4__desired_position + INT + + + #x311F + 1 + 16 + M4__desired_speed + INT + + + #x3120 + 1 + 16 + M4__desired__current + INT + + + #x3121 + 1 + 16 + M4__Max_Pos + INT + + + #x3122 + 1 + 16 + M4__Max_velocity + INT + + + #x3123 + 1 + 16 + M4__Max_current + INT + + + #x3124 + 1 + 16 + M4__Spare + INT + + + + #x1600 + MCU2ECAT + + #x3001 + 1 + 8 + M1__status + BYTE + + + #x3002 + 1 + 8 + M1__mode + BYTE + + + #x3003 + 1 + 16 + M1__Joint_rel_position + INT + + + #x3004 + 1 + 16 + M1__Joint_abs_position + INT + + + #x3005 + 1 + 16 + M1__Motor_speed + INT + + + #x3006 + 1 + 16 + M1__Motor_current_bus + INT + + + #x3007 + 1 + 16 + M1__Motor_currentPhA + INT + + + #x3008 + 1 + 16 + M1__Motor_currentPhB + INT + + + #x3009 + 1 + 16 + M1__Motor_currentPhC + INT + + + #x300A + 1 + 16 + M1__Motor_hallState + INT + + + #x300B + 1 + 16 + M1__Motor_dutyCycle + INT + + + #x300C + 1 + 8 + M2__status + BYTE + + + #x300D + 1 + 8 + M2__mode + BYTE + + + #x300E + 1 + 16 + M2__Joint__rel_position + INT + + + #x300F + 1 + 16 + M2__Joint_abs_position + INT + + + #x3010 + 1 + 16 + M2__Motor_speed + INT + + + #x3011 + 1 + 16 + M2__Motor_current_bus + INT + + + #x3012 + 1 + 16 + M2__Motor_currentPhA + INT + + + #x3013 + 1 + 16 + M2__Motor_currentPhB + INT + + + #x3014 + 1 + 16 + M2__Motor_currentPhC + INT + + + #x3015 + 1 + 16 + M2__Motor_hallState + INT + + + #x3016 + 1 + 16 + M2__Motor_dutyCycle + INT + + + #x3017 + 1 + 16 + EMG__CH1 + INT + + + #x3018 + 1 + 16 + EMG__CH2 + INT + + + #x3019 + 1 + 16 + EMG__CH3 + INT + + + #x301A + 1 + 16 + EMG__CH4 + INT + + + #x301B + 1 + 16 + EMG__CH5 + INT + + + #x301C + 1 + 16 + EMG__CH6 + INT + + + #x301D + 1 + 16 + EMG__CH7 + INT + + + #x301E + 1 + 16 + EMG__CH8 + INT + + + #x301F + 1 + 8 + M3__status + BYTE + + + #x3020 + 1 + 8 + M3__mode + BYTE + + + #x3021 + 1 + 16 + M3__Joint__rel_position + INT + + + #x3022 + 1 + 16 + M3__Joint_abs_position + INT + + + #x3023 + 1 + 16 + M3__Motor_speed + INT + + + #x3024 + 1 + 16 + M3__Motor_current_bus + INT + + + #x3025 + 1 + 16 + M3__Motor_currentPhA + INT + + + #x3026 + 1 + 16 + M3__Motor_currentPhB + INT + + + #x3027 + 1 + 16 + M3__Motor_currentPhC + INT + + + #x3028 + 1 + 16 + M3__Motor_hallState + INT + + + #x3029 + 1 + 16 + M3__Motor_dutyCycle + INT + + + #x302A + 1 + 8 + M4__status + BYTE + + + #x302B + 1 + 8 + M4__mode + BYTE + + + #x302C + 1 + 16 + M4__Joint__rel_position + INT + + + #x302D + 1 + 16 + M4__Joint_abs_position + INT + + + #x302E + 1 + 16 + M4__Motor_speed + INT + + + #x302F + 1 + 16 + M4__Motor_current_bus + INT + + + #x3030 + 1 + 16 + M4__Motor_currentPhA + INT + + + #x3031 + 1 + 16 + M4__Motor_currentPhB + INT + + + #x3032 + 1 + 16 + M4__Motor_currentPhC + INT + + + #x3033 + 1 + 16 + M4__Motor_hallState + INT + + + #x3034 + 1 + 16 + M4__Motor_dutyCycle + INT + + + #x3035 + 1 + 16 + IMU__q_x0 + INT + + + #x3036 + 1 + 16 + IMU__q_y0 + INT + + + #x3037 + 1 + 16 + IMU__q_z0 + INT + + + #x3038 + 1 + 16 + IMU__q_w0 + INT + + + #x3039 + 1 + 16 + FSR__CH1 + INT + + + #x303A + 1 + 16 + FSR__CH2 + INT + + + #x303B + 1 + 16 + FSR__CH3 + INT + + + #x303C + 1 + 16 + FSR__CH4 + INT + + + #x303D + 1 + 16 + FSR__CH5 + INT + + + #x303E + 1 + 16 + Pressure__CH1 + INT + + + #x303F + 1 + 16 + Pressure__CH2 + INT + + + #x3040 + 1 + 16 + Pressure__CH3 + INT + + + + 2048 + 803100CC1000F0FF + + + + + \ No newline at end of file diff --git a/2_Motor_Master/Motor_Master/Motor_Master/EtherCAT_QSPI.h b/2_Motor_Master/Motor_Master/Motor_Master/EtherCAT_QSPI.h index 134675a..d64c935 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/EtherCAT_QSPI.h +++ b/2_Motor_Master/Motor_Master/Motor_Master/EtherCAT_QSPI.h @@ -8,8 +8,7 @@ #define ETHERCAT_QSPI_H_ #include "atmel_start.h" -#include -#include + #define ECAT_SIZE_WR 64 //max fifo size #define ECAT_SIZE_RD ECAT_SIZE_WR diff --git a/2_Motor_Master/Motor_Master/Motor_Master/EtherCAT_SlaveDef.h b/2_Motor_Master/Motor_Master/Motor_Master/EtherCAT_SlaveDef.h new file mode 100644 index 0000000..1448fd4 --- /dev/null +++ b/2_Motor_Master/Motor_Master/Motor_Master/EtherCAT_SlaveDef.h @@ -0,0 +1,269 @@ +/* + * 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_Spare = ((int16_t *)&QSPI_rx_buffer[3]+1); +///* 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_Spare = ((int16_t *)&QSPI_rx_buffer[7]+1); +///* 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); +///* 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); + +//void update_telemetry(void) +//{ + //inline int16_t convert_to_mA(volatile float32_t current_PU) + //{ + //return (int16_t)(current_PU*1000.0f); + //} + // + //*state = applicationStatus.currentstate; + //*status = applicationStatus.fault; + //*motor_rel_position = Motor1.motor_status.Num_Steps; + //*motor_abs_position = 0; + //*motor_dutyCycle = Motor1.motor_status.duty_cycle; + //*motor_speed = (int16_t)Motor1.motor_status.calc_rpm; + //*motor_torque = 0; + //*motor_currentPHA = convert_to_mA(Motor1.Iphase_pu.A); + //*motor_currentPHB = convert_to_mA(Motor1.Iphase_pu.B); + //*motor_currentPHC = convert_to_mA(Motor1.Iphase_pu.C); + //*motor_currentBUS = convert_to_mA(Motor1.Iphase_pu.Bus); + //*hall_state = Motor1.motor_status.currentHallPattern; + //*Spare_byte1 = Motor1.motor_setpoints.directionOffset; + //*Spare_1 = Motor1.motor_status.actualDirection; + ////*Spare_2 = 0; +//} +// +void update_setpoints(void) +{ + 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_ */ \ No newline at end of file diff --git a/2_Motor_Master/Motor_Master/Motor_Master/Ethercat_QSPI.c b/2_Motor_Master/Motor_Master/Motor_Master/Ethercat_QSPI.c index 49a51ed..f254423 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/Ethercat_QSPI.c +++ b/2_Motor_Master/Motor_Master/Motor_Master/Ethercat_QSPI.c @@ -6,6 +6,8 @@ */ #include "Ethercat_QSPI.h" +#include +#include #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 diff --git a/2_Motor_Master/Motor_Master/Motor_Master/Ethercat_QSPI.h b/2_Motor_Master/Motor_Master/Motor_Master/Ethercat_QSPI.h new file mode 100644 index 0000000..d64c935 --- /dev/null +++ b/2_Motor_Master/Motor_Master/Motor_Master/Ethercat_QSPI.h @@ -0,0 +1,38 @@ +/* + * 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_ */ \ No newline at end of file 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 ea39e6d..1421b05 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/Motor_Master.cproj +++ b/2_Motor_Master/Motor_Master/Motor_Master/Motor_Master.cproj @@ -30,15 +30,15 @@ - - - - - - - - - + + + + + + + + + gcc .atmelstart\atmel_start_config.atstart @@ -576,6 +576,9 @@ compile + + compile + compile diff --git a/2_Motor_Master/Motor_Master/Motor_Master/main.c b/2_Motor_Master/Motor_Master/Motor_Master/main.c index 0bf98eb..6e8a058 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/main.c +++ b/2_Motor_Master/Motor_Master/Motor_Master/main.c @@ -1,11 +1,12 @@ #include #include "bldc.h" #include "EtherCAT_QSPI.h" +#include "EtherCAT_SlaveDef.h" /** * Example of using TIMER_0. */ -static void onems_tick(const struct timer_task *const timer_task) +static void One_ms_cycle_callback(const struct timer_task *const timer_task) { if ((ecat_state == wait2)|(ecat_state == wait)) ecat_state= write_fifo; @@ -15,10 +16,10 @@ static void onems_tick(const struct timer_task *const timer_task) static struct timer_task Onems_task; -void onemstimer_init(void) +void One_ms_timer_init(void) { Onems_task.interval = 1; - Onems_task.cb = onems_tick; + Onems_task.cb = One_ms_cycle_callback; Onems_task.mode = TIMER_TASK_REPEAT; timer_add_task(&TIMER_0, &Onems_task); timer_start(&TIMER_0); @@ -32,16 +33,17 @@ int main(void) atmel_start_init(); config_qspi(); ECAT_STATE_MACHINE(); - onemstimer_init(); + One_ms_timer_init(); /* Replace with your application code */ while (1) { //delay_ms(100); - //m1_hall = readM1Hall(); - //m2_hall = readM2Hall(); + *M1_Motor__hallState = readM1Hall(); + *M2_Motor__hallState = readM2Hall(); //x += 1; - QSPI_tx_buffer[0] += 1; + //comms_check(); + //update_setpoints(); if (run_ECAT) ECAT_STATE_MACHINE(); }