slavedef
This commit is contained in:
Nicolas Trimborn
parent
5234c7031b
commit
7fe6ef2ab9
|
|
@ -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_ */
|
||||||
Loading…
Reference in New Issue