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..8cb4e8e --- /dev/null +++ b/2_Motor_Master/Motor_Master/Motor_Master/Ethercat_SlaveDef.h @@ -0,0 +1,279 @@ +/* + * 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); + } + // + //*M1_Status = 0; + //*M1_Mode = 0; + + /* Motor 1 */ + *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; + // + /* Motor 2 */ + *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; + +} + +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