mirror of
https://gitee.com/dashuaibran/jyker
synced 2025-09-27 03:09:12 +08:00
296 lines
12 KiB
C++
296 lines
12 KiB
C++
#include "common_inc.h"
|
||
#include "configurations.h"
|
||
#include <can.h>
|
||
|
||
|
||
extern Motor motor;
|
||
extern EncoderCalibrator encoderCalibrator;
|
||
|
||
CAN_TxHeaderTypeDef txHeader =
|
||
{
|
||
.StdId = 0x00,
|
||
.ExtId = 0x00,
|
||
.IDE = CAN_ID_STD,
|
||
.RTR = CAN_RTR_DATA,
|
||
.DLC = 8,
|
||
.TransmitGlobalTime = DISABLE
|
||
};
|
||
|
||
void SendOne() {
|
||
printf("send one begin\r\n");
|
||
uint8_t TxData[8] = {0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08};
|
||
CAN_TxHeaderTypeDef TxHeader;
|
||
|
||
TxHeader.StdId = 0x123;
|
||
TxHeader.ExtId = 0x00;
|
||
TxHeader.RTR = CAN_RTR_DATA;
|
||
TxHeader.IDE = CAN_ID_STD;
|
||
TxHeader.DLC = 8;
|
||
TxHeader.TransmitGlobalTime = DISABLE;
|
||
|
||
if (HAL_CAN_AddTxMessage(&hcan, &TxHeader, TxData, &TxMailbox) != HAL_OK)
|
||
{
|
||
Error_Handler();
|
||
}
|
||
printf("send one \r\n");
|
||
}
|
||
|
||
void OnCanCmd(uint8_t _cmd, uint8_t* _data, uint32_t _len)
|
||
{
|
||
float tmpF;
|
||
int32_t tmpI;
|
||
|
||
switch (_cmd)
|
||
{
|
||
// 0x00~0x0F No Memory CMDs
|
||
case 0x01: // Enable Motor
|
||
motor.controller->requestMode = (*(uint32_t*) (RxData) == 1) ?
|
||
Motor::MODE_COMMAND_VELOCITY : Motor::MODE_STOP;
|
||
break;
|
||
case 0x02: // Do Calibration
|
||
encoderCalibrator.isTriggered = true;
|
||
break;
|
||
case 0x03: // Set Current SetPoint
|
||
if (motor.controller->modeRunning != Motor::MODE_COMMAND_CURRENT)
|
||
motor.controller->SetCtrlMode(Motor::MODE_COMMAND_CURRENT);
|
||
motor.controller->SetCurrentSetPoint((int32_t) (*(float*) RxData * 1000));
|
||
break;
|
||
case 0x04: // Set Velocity SetPoint
|
||
if (motor.controller->modeRunning != Motor::MODE_COMMAND_VELOCITY)
|
||
{
|
||
motor.config.motionParams.ratedVelocity = boardConfig.velocityLimit;
|
||
motor.controller->SetCtrlMode(Motor::MODE_COMMAND_VELOCITY);
|
||
}
|
||
motor.controller->SetVelocitySetPoint(
|
||
(int32_t) (*(float*) RxData *
|
||
(float) motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS));
|
||
//printf("vel: %d\n", (int32_t) (*(float*) RxData));
|
||
break;
|
||
case 0x05: // Set Position SetPoint
|
||
if (motor.controller->modeRunning != Motor::MODE_COMMAND_POSITION)
|
||
{
|
||
motor.config.motionParams.ratedVelocity = boardConfig.velocityLimit;
|
||
motor.controller->SetCtrlMode(Motor::MODE_COMMAND_POSITION);
|
||
}
|
||
motor.controller->SetPositionSetPoint(
|
||
(int32_t) (*(float*) RxData * (float) motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS));
|
||
//printf("SET MOTOR[0x05] POSITION[]\r\n");
|
||
if (_data[4]) // Need Position & Finished ACK
|
||
{
|
||
tmpF = motor.controller->GetPosition();
|
||
auto* b = (unsigned char*) &tmpF;
|
||
for (int i = 0; i < 4; i++)
|
||
_data[i] = *(b + i);
|
||
_data[4] = motor.controller->state == Motor::STATE_FINISH ? 1 : 0;
|
||
txHeader.StdId = (boardConfig.canNodeId << 7) | 0x23;
|
||
CAN_Send(&txHeader, _data);
|
||
}
|
||
break;
|
||
case 0x06: // Set Position with Time
|
||
if (motor.controller->modeRunning != Motor::MODE_COMMAND_POSITION)
|
||
motor.controller->SetCtrlMode(Motor::MODE_COMMAND_POSITION);
|
||
motor.controller->SetPositionSetPointWithTime(
|
||
(int32_t) (*(float*) RxData * (float) motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS),
|
||
*(float*) (RxData + 4));
|
||
if (_data[4]) // Need Position & Finished ACK
|
||
{
|
||
tmpF = motor.controller->GetPosition();
|
||
auto* b = (unsigned char*) &tmpF;
|
||
for (int i = 0; i < 4; i++)
|
||
_data[i] = *(b + i);
|
||
_data[4] = motor.controller->state == Motor::STATE_FINISH ? 1 : 0;
|
||
txHeader.StdId = (boardConfig.canNodeId << 7) | 0x23;
|
||
CAN_Send(&txHeader, _data);
|
||
}
|
||
break;
|
||
case 0x07: // Set Position with Velocity-Limit
|
||
{
|
||
if (motor.controller->modeRunning != Motor::MODE_COMMAND_POSITION)
|
||
{
|
||
motor.config.motionParams.ratedVelocity = boardConfig.velocityLimit;
|
||
motor.controller->SetCtrlMode(Motor::MODE_COMMAND_POSITION);
|
||
}
|
||
motor.config.motionParams.ratedVelocity =
|
||
(int32_t) (*(float*) (RxData + 4) * (float) motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS);
|
||
motor.controller->SetPositionSetPoint(
|
||
(int32_t) (*(float*) RxData * (float) motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS));
|
||
// Always Need Position & Finished ACK
|
||
tmpF = motor.controller->GetPosition();
|
||
auto* b = (unsigned char*) &tmpF;
|
||
for (int i = 0; i < 4; i++)
|
||
_data[i] = *(b + i);
|
||
_data[4] = motor.controller->state == Motor::STATE_FINISH ? 1 : 0;
|
||
txHeader.StdId = (boardConfig.canNodeId << 7) | 0x23;
|
||
CAN_Send(&txHeader, _data);
|
||
}
|
||
break;
|
||
|
||
// 0x10~0x1F CMDs with Memory
|
||
case 0x11: // Set Node-ID and Store to EEPROM
|
||
boardConfig.canNodeId = *(uint32_t*) (RxData);
|
||
if (_data[4])
|
||
boardConfig.configStatus = CONFIG_COMMIT;
|
||
break;
|
||
case 0x12: // Set Current-Limit and Store to EEPROM
|
||
motor.config.motionParams.ratedCurrent = (int32_t) (*(float*) RxData * 1000);
|
||
boardConfig.currentLimit = motor.config.motionParams.ratedCurrent;
|
||
if (_data[4])
|
||
boardConfig.configStatus = CONFIG_COMMIT;
|
||
break;
|
||
case 0x13: // Set Velocity-Limit and Store to EEPROM
|
||
motor.config.motionParams.ratedVelocity =
|
||
(int32_t) (*(float*) RxData *
|
||
(float) motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS);
|
||
boardConfig.velocityLimit = motor.config.motionParams.ratedVelocity;
|
||
if (_data[4])
|
||
boardConfig.configStatus = CONFIG_COMMIT;
|
||
break;
|
||
case 0x14: // Set Acceleration (and Store to EEPROM)
|
||
tmpF = *(float*) RxData * (float) motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;
|
||
|
||
motor.config.motionParams.ratedVelocityAcc = (int32_t) tmpF;
|
||
motor.motionPlanner.velocityTracker.SetVelocityAcc((int32_t) tmpF);
|
||
motor.motionPlanner.positionTracker.SetVelocityAcc((int32_t) tmpF);
|
||
boardConfig.velocityAcc = motor.config.motionParams.ratedVelocityAcc;
|
||
if (_data[4])
|
||
boardConfig.configStatus = CONFIG_COMMIT;
|
||
break;
|
||
case 0x15: // Apply Home-Position and Store to EEPROM
|
||
motor.controller->ApplyPosAsHomeOffset();
|
||
boardConfig.encoderHomeOffset = motor.config.motionParams.encoderHomeOffset %
|
||
motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;
|
||
boardConfig.configStatus = CONFIG_COMMIT;
|
||
break;
|
||
case 0x16: // Set Auto-Enable and Store to EEPROM
|
||
boardConfig.enableMotorOnBoot = (*(uint32_t*) (RxData) == 1);
|
||
if (_data[4])
|
||
boardConfig.configStatus = CONFIG_COMMIT;
|
||
break;
|
||
case 0x17: // Set DCE Kp
|
||
motor.config.ctrlParams.dce.kp = *(int32_t*) (RxData);
|
||
boardConfig.dce_kp = motor.config.ctrlParams.dce.kp;
|
||
if (_data[4])
|
||
boardConfig.configStatus = CONFIG_COMMIT;
|
||
break;
|
||
case 0x18: // Set DCE Kv
|
||
motor.config.ctrlParams.dce.kv = *(int32_t*) (RxData);
|
||
boardConfig.dce_kv = motor.config.ctrlParams.dce.kv;
|
||
if (_data[4])
|
||
boardConfig.configStatus = CONFIG_COMMIT;
|
||
break;
|
||
case 0x19: // Set DCE Ki
|
||
motor.config.ctrlParams.dce.ki = *(int32_t*) (RxData);
|
||
boardConfig.dce_ki = motor.config.ctrlParams.dce.ki;
|
||
if (_data[4])
|
||
boardConfig.configStatus = CONFIG_COMMIT;
|
||
break;
|
||
case 0x1A: // Set DCE Kd
|
||
motor.config.ctrlParams.dce.kd = *(int32_t*) (RxData);
|
||
boardConfig.dce_kd = motor.config.ctrlParams.dce.kd;
|
||
if (_data[4])
|
||
boardConfig.configStatus = CONFIG_COMMIT;
|
||
break;
|
||
case 0x1B: // Set Enable Stall-Protect
|
||
motor.config.ctrlParams.stallProtectSwitch = (*(uint32_t*) (RxData) == 1);
|
||
boardConfig.enableStallProtect = motor.config.ctrlParams.stallProtectSwitch;
|
||
if (_data[4])
|
||
boardConfig.configStatus = CONFIG_COMMIT;
|
||
break;
|
||
|
||
|
||
// 0x20~0x2F Inquiry CMDs
|
||
case 0x21: // Get Current
|
||
{
|
||
tmpF = motor.controller->GetFocCurrent();
|
||
auto* b = (unsigned char*) &tmpF;
|
||
for (int i = 0; i < 4; i++)
|
||
_data[i] = *(b + i);
|
||
_data[4] = (motor.controller->state == Motor::STATE_FINISH ? 1 : 0);
|
||
|
||
txHeader.StdId = (boardConfig.canNodeId << 7) | 0x21;
|
||
CAN_Send(&txHeader, _data);
|
||
}
|
||
break;
|
||
case 0x22: // Get Velocity
|
||
{
|
||
tmpF = motor.controller->GetVelocity();
|
||
auto* b = (unsigned char*) &tmpF;
|
||
for (int i = 0; i < 4; i++)
|
||
_data[i] = *(b + i);
|
||
_data[4] = (motor.controller->state == Motor::STATE_FINISH ? 1 : 0);
|
||
|
||
txHeader.StdId = (boardConfig.canNodeId << 7) | 0x22;
|
||
CAN_Send(&txHeader, _data);
|
||
}
|
||
break;
|
||
case 0x23: // Get Position
|
||
{
|
||
tmpF = motor.controller->GetPosition();
|
||
auto* b = (unsigned char*) &tmpF;
|
||
for (int i = 0; i < 4; i++)
|
||
_data[i] = *(b + i);
|
||
// Finished ACK
|
||
_data[4] = motor.controller->state == Motor::STATE_FINISH ? 1 : 0;
|
||
txHeader.StdId = (boardConfig.canNodeId << 7) | 0x23;
|
||
CAN_Send(&txHeader, _data);
|
||
// printf("CAN SEND BACK to NODE[%d]\n", boardConfig.canNodeId );
|
||
}
|
||
break;
|
||
case 0x24: // Get Offset
|
||
{
|
||
tmpI = motor.config.motionParams.encoderHomeOffset;
|
||
auto* b = (unsigned char*) &tmpI;
|
||
for (int i = 0; i < 4; i++)
|
||
_data[i] = *(b + i);
|
||
txHeader.StdId = (boardConfig.canNodeId << 7) | 0x24;
|
||
CAN_Send(&txHeader, _data);
|
||
}
|
||
break;
|
||
|
||
case 0x25: // Get temperature
|
||
{
|
||
auto* b = (unsigned char*) &boardConfig.motor_temperature;
|
||
for (int i = 0; i < 4; i++)
|
||
_data[i] = *(b + i);
|
||
_data[4] = 0;
|
||
_data[5] = 0;
|
||
_data[6] = 0;
|
||
_data[7] = 0;
|
||
txHeader.StdId = (boardConfig.canNodeId << 7) | 0x25;
|
||
CAN_Send(&txHeader, _data);
|
||
}
|
||
break;
|
||
|
||
|
||
case 0x26: // Get Limit-Current
|
||
{
|
||
float currentLimitFloat = (float)boardConfig.currentLimit;
|
||
// 将浮点数转换为字节序列
|
||
unsigned char* b = (unsigned char*)¤tLimitFloat;
|
||
for (int i = 0; i < 4; i++)
|
||
_data[i] = *(b + i);
|
||
_data[4] = 0;
|
||
_data[5] = 0;
|
||
_data[6] = 0;
|
||
_data[7] = 0;
|
||
txHeader.StdId = (boardConfig.canNodeId << 7) | 0x26;
|
||
CAN_Send(&txHeader, _data);
|
||
}
|
||
break;
|
||
|
||
case 0x7d: // enable motor temperature watch
|
||
boardConfig.enableTempWatch = true;
|
||
break;
|
||
case 0x7e: // Erase Configs
|
||
boardConfig.configStatus = CONFIG_RESTORE;
|
||
break;
|
||
case 0x7f: // Reboot
|
||
HAL_NVIC_SystemReset();
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
|
||
}
|
||
|