编译程序烧录到电机的时候,电机驱动板有两个按键k1和k2,但看不懂程序,不知道这两个按键具体怎么用,现在烧录到电机上后,按按键电机也不转,什么反应没有
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));
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));
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);
}
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 0x7e: // Erase Configs
boardConfig.configStatus = CONFIG_RESTORE;
break;
case 0x7f: // Reboot
HAL_NVIC_SystemReset();
break;
default:
break;
}
}