mirror of
https://gitee.com/peng_zhihui/Dummy-Robot
synced 2025-11-04 20:49:40 +08:00
640 lines
22 KiB
C++
640 lines
22 KiB
C++
#include "configurations.h"
|
|
#include "motor.h"
|
|
|
|
#include <cmath>
|
|
|
|
|
|
void Motor::Tick20kHz()
|
|
{
|
|
// 1.Encoder data Update
|
|
encoder->UpdateAngle();
|
|
|
|
// 2.Motor Control Update
|
|
CloseLoopControlTick();
|
|
}
|
|
|
|
|
|
void Motor::AttachEncoder(EncoderBase* _encoder)
|
|
{
|
|
encoder = _encoder;
|
|
}
|
|
|
|
|
|
void Motor::AttachDriver(DriverBase* _driver)
|
|
{
|
|
driver = _driver;
|
|
}
|
|
|
|
|
|
void Motor::CloseLoopControlTick()
|
|
{
|
|
/************************************ First Called ************************************/
|
|
static bool isFirstCalled = true;
|
|
if (isFirstCalled)
|
|
{
|
|
int32_t angle;
|
|
if (config.motionParams.encoderHomeOffset < MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS / 2)
|
|
{
|
|
angle =
|
|
encoder->angleData.rectifiedAngle >
|
|
config.motionParams.encoderHomeOffset + MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS / 2 ?
|
|
encoder->angleData.rectifiedAngle - MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS :
|
|
encoder->angleData.rectifiedAngle;
|
|
} else
|
|
{
|
|
angle =
|
|
encoder->angleData.rectifiedAngle <
|
|
config.motionParams.encoderHomeOffset - MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS / 2 ?
|
|
encoder->angleData.rectifiedAngle + MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS :
|
|
encoder->angleData.rectifiedAngle;
|
|
}
|
|
|
|
controller->realLapPosition = angle;
|
|
controller->realLapPositionLast = angle;
|
|
controller->realPosition = angle;
|
|
controller->realPositionLast = angle;
|
|
|
|
isFirstCalled = false;
|
|
return;
|
|
}
|
|
|
|
/********************************* Update Data *********************************/
|
|
int32_t deltaLapPosition;
|
|
|
|
// Read Encoder data
|
|
controller->realLapPositionLast = controller->realLapPosition;
|
|
controller->realLapPosition = encoder->angleData.rectifiedAngle;
|
|
|
|
// Lap-Position calculate
|
|
deltaLapPosition = controller->realLapPosition - controller->realLapPositionLast;
|
|
if (deltaLapPosition > MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS >> 1)
|
|
deltaLapPosition -= MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;
|
|
else if (deltaLapPosition < -MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS >> 1)
|
|
deltaLapPosition += MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;
|
|
|
|
// Naive-Position calculate
|
|
controller->realPositionLast = controller->realPosition;
|
|
controller->realPosition += deltaLapPosition;
|
|
|
|
/********************************* Estimate Data *********************************/
|
|
// Estimate Velocity
|
|
controller->estVelocityIntegral += (
|
|
(controller->realPosition - controller->realPositionLast) * motionPlanner.CONTROL_FREQUENCY
|
|
+ ((controller->estVelocity << 5) - controller->estVelocity)
|
|
);
|
|
controller->estVelocity = controller->estVelocityIntegral >> 5;
|
|
controller->estVelocityIntegral -= (controller->estVelocity << 5);
|
|
|
|
// Estimate Position
|
|
controller->estLeadPosition = Controller::CompensateAdvancedAngle(controller->estVelocity);
|
|
controller->estPosition = controller->realPosition + controller->estLeadPosition;
|
|
|
|
// Estimate Error
|
|
controller->estError = controller->softPosition - controller->estPosition;
|
|
|
|
/************************************ Ctrl Loop ************************************/
|
|
if (controller->isStalled ||
|
|
controller->softDisable ||
|
|
!encoder->IsCalibrated())
|
|
{
|
|
controller->ClearIntegral(); // clear integrals
|
|
controller->focPosition = 0; // clear outputs
|
|
controller->focCurrent = 0;
|
|
driver->Sleep();
|
|
} else if (controller->softBrake)
|
|
{
|
|
controller->ClearIntegral();
|
|
controller->focPosition = 0;
|
|
controller->focCurrent = 0;
|
|
driver->Brake();
|
|
} else
|
|
{
|
|
switch (controller->modeRunning)
|
|
{
|
|
case MODE_STEP_DIR:
|
|
controller->CalcDceToOutput(controller->softPosition, controller->softVelocity);
|
|
break;
|
|
case MODE_STOP:
|
|
driver->Sleep();
|
|
break;
|
|
case MODE_COMMAND_Trajectory:
|
|
controller->CalcDceToOutput(controller->softPosition, controller->softVelocity);
|
|
break;
|
|
case MODE_COMMAND_CURRENT:
|
|
controller->CalcCurrentToOutput(controller->softCurrent);
|
|
break;
|
|
case MODE_COMMAND_VELOCITY:
|
|
controller->CalcPidToOutput(controller->softVelocity);
|
|
break;
|
|
case MODE_COMMAND_POSITION:
|
|
controller->CalcDceToOutput(controller->softPosition, controller->softVelocity);
|
|
break;
|
|
case MODE_PWM_CURRENT:
|
|
controller->CalcCurrentToOutput(controller->softCurrent);
|
|
break;
|
|
case MODE_PWM_VELOCITY:
|
|
controller->CalcPidToOutput(controller->softVelocity);
|
|
break;
|
|
case MODE_PWM_POSITION:
|
|
controller->CalcDceToOutput(controller->softPosition, controller->softVelocity);
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
/******************************* Mode Change Handle *******************************/
|
|
if (controller->modeRunning != controller->requestMode)
|
|
{
|
|
controller->modeRunning = controller->requestMode;
|
|
controller->softNewCurve = true;
|
|
}
|
|
|
|
/******************************* Update Hard-Goal *******************************/
|
|
if (controller->goalVelocity > config.motionParams.ratedVelocity)
|
|
controller->goalVelocity = config.motionParams.ratedVelocity;
|
|
else if (controller->goalVelocity < -config.motionParams.ratedVelocity)
|
|
controller->goalVelocity = -config.motionParams.ratedVelocity;
|
|
if (controller->goalCurrent > config.motionParams.ratedCurrent)
|
|
controller->goalCurrent = config.motionParams.ratedCurrent;
|
|
else if (controller->goalCurrent < -config.motionParams.ratedCurrent)
|
|
controller->goalCurrent = -config.motionParams.ratedCurrent;
|
|
|
|
/******************************** Motion Plan *********************************/
|
|
if ((controller->softDisable && !controller->goalDisable) ||
|
|
(controller->softBrake && !controller->goalBrake))
|
|
{
|
|
controller->softNewCurve = true;
|
|
}
|
|
|
|
if (controller->softNewCurve)
|
|
{
|
|
controller->softNewCurve = false;
|
|
controller->ClearIntegral();
|
|
controller->ClearStallFlag();
|
|
|
|
switch (controller->modeRunning)
|
|
{
|
|
case MODE_STOP:
|
|
break;
|
|
case MODE_COMMAND_POSITION:
|
|
motionPlanner.positionTracker.NewTask(controller->estPosition, controller->estVelocity);
|
|
break;
|
|
case MODE_COMMAND_VELOCITY:
|
|
motionPlanner.velocityTracker.NewTask(controller->estVelocity);
|
|
break;
|
|
case MODE_COMMAND_CURRENT:
|
|
motionPlanner.currentTracker.NewTask(controller->focCurrent);
|
|
break;
|
|
case MODE_COMMAND_Trajectory:
|
|
motionPlanner.trajectoryTracker.NewTask(controller->estPosition, controller->estVelocity);
|
|
break;
|
|
case MODE_PWM_POSITION:
|
|
motionPlanner.positionTracker.NewTask(controller->estPosition, controller->estVelocity);
|
|
break;
|
|
case MODE_PWM_VELOCITY:
|
|
motionPlanner.velocityTracker.NewTask(controller->estVelocity);
|
|
break;
|
|
case MODE_PWM_CURRENT:
|
|
motionPlanner.currentTracker.NewTask(controller->focCurrent);
|
|
break;
|
|
case MODE_STEP_DIR:
|
|
motionPlanner.positionInterpolator.NewTask(controller->estPosition, controller->estVelocity);
|
|
// step/dir mode uses delta-position, so stay where we are
|
|
controller->goalPosition = controller->estPosition;
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
/******************************* Update Soft Goal *******************************/
|
|
switch (controller->modeRunning)
|
|
{
|
|
case MODE_STOP:
|
|
break;
|
|
case MODE_COMMAND_POSITION:
|
|
motionPlanner.positionTracker.CalcSoftGoal(controller->goalPosition);
|
|
controller->softPosition = motionPlanner.positionTracker.go_location;
|
|
controller->softVelocity = motionPlanner.positionTracker.go_velocity;
|
|
break;
|
|
case MODE_COMMAND_VELOCITY:
|
|
motionPlanner.velocityTracker.CalcSoftGoal(controller->goalVelocity);
|
|
controller->softVelocity = motionPlanner.velocityTracker.goVelocity;
|
|
break;
|
|
case MODE_COMMAND_CURRENT:
|
|
motionPlanner.currentTracker.CalcSoftGoal(controller->goalCurrent);
|
|
controller->softCurrent = motionPlanner.currentTracker.goCurrent;
|
|
break;
|
|
case MODE_COMMAND_Trajectory:
|
|
motionPlanner.trajectoryTracker.CalcSoftGoal(controller->goalPosition, controller->goalVelocity);
|
|
controller->softPosition = motionPlanner.trajectoryTracker.goPosition;
|
|
controller->softVelocity = motionPlanner.trajectoryTracker.goVelocity;
|
|
break;
|
|
case MODE_PWM_POSITION:
|
|
motionPlanner.positionTracker.CalcSoftGoal(controller->goalPosition);
|
|
controller->softPosition = motionPlanner.positionTracker.go_location;
|
|
controller->softVelocity = motionPlanner.positionTracker.go_velocity;
|
|
break;
|
|
case MODE_PWM_VELOCITY:
|
|
motionPlanner.velocityTracker.CalcSoftGoal(controller->goalVelocity);
|
|
controller->softVelocity = motionPlanner.velocityTracker.goVelocity;
|
|
break;
|
|
case MODE_PWM_CURRENT:
|
|
motionPlanner.currentTracker.CalcSoftGoal(controller->goalCurrent);
|
|
controller->softCurrent = motionPlanner.currentTracker.goCurrent;
|
|
break;
|
|
case MODE_STEP_DIR:
|
|
motionPlanner.positionInterpolator.CalcSoftGoal(controller->goalPosition);
|
|
controller->softPosition = motionPlanner.positionInterpolator.goPosition;
|
|
controller->softVelocity = motionPlanner.positionInterpolator.goVelocity;
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
|
|
controller->softDisable = controller->goalDisable;
|
|
controller->softBrake = controller->goalBrake;
|
|
|
|
/******************************** State Check ********************************/
|
|
int32_t current = abs(controller->focCurrent);
|
|
|
|
// Stall detect
|
|
if (controller->config->stallProtectSwitch)
|
|
{
|
|
if (// Current Mode
|
|
((controller->modeRunning == MODE_COMMAND_CURRENT ||
|
|
controller->modeRunning == MODE_PWM_CURRENT) &&
|
|
(current != 0))
|
|
|| // Other Mode
|
|
current == config.motionParams.ratedCurrent)
|
|
{
|
|
if (abs(controller->estVelocity) < MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS / 5)
|
|
{
|
|
if (controller->stalledTime >= 1000 * 1000)
|
|
controller->isStalled = true;
|
|
else
|
|
controller->stalledTime += motionPlanner.CONTROL_PERIOD;
|
|
}
|
|
} else // can ONLY clear stall flag MANUALLY
|
|
{
|
|
controller->stalledTime = 0;
|
|
}
|
|
}
|
|
|
|
// Overload detect
|
|
if ((controller->modeRunning != MODE_COMMAND_CURRENT) &&
|
|
(controller->modeRunning != MODE_PWM_CURRENT) &&
|
|
(current == config.motionParams.ratedCurrent))
|
|
{
|
|
if (controller->overloadTime >= 1000 * 1000)
|
|
controller->overloadFlag = true;
|
|
else
|
|
controller->overloadTime += motionPlanner.CONTROL_PERIOD;
|
|
} else // auto clear overload flag when released
|
|
{
|
|
controller->overloadTime = 0;
|
|
controller->overloadFlag = false;
|
|
}
|
|
|
|
/******************************** Update State ********************************/
|
|
if (!encoder->IsCalibrated())
|
|
controller->state = STATE_NO_CALIB;
|
|
else if (controller->modeRunning == MODE_STOP)
|
|
controller->state = STATE_STOP;
|
|
else if (controller->isStalled)
|
|
controller->state = STATE_STALL;
|
|
else if (controller->overloadFlag)
|
|
controller->state = STATE_OVERLOAD;
|
|
else
|
|
{
|
|
if (controller->modeRunning == MODE_COMMAND_POSITION)
|
|
{
|
|
if ((controller->softPosition == controller->goalPosition)
|
|
&& (controller->softVelocity == 0))
|
|
controller->state = STATE_FINISH;
|
|
else
|
|
controller->state = STATE_RUNNING;
|
|
} else if (controller->modeRunning == MODE_COMMAND_VELOCITY)
|
|
{
|
|
if (controller->softVelocity == controller->goalVelocity)
|
|
controller->state = STATE_FINISH;
|
|
else
|
|
controller->state = STATE_RUNNING;
|
|
} else if (controller->modeRunning == MODE_COMMAND_CURRENT)
|
|
{
|
|
if (controller->softCurrent == controller->goalCurrent)
|
|
controller->state = STATE_FINISH;
|
|
else
|
|
controller->state = STATE_RUNNING;
|
|
} else
|
|
{
|
|
controller->state = STATE_FINISH;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
void Motor::Controller::CalcCurrentToOutput(int32_t current)
|
|
{
|
|
focCurrent = current;
|
|
|
|
if (focCurrent > 0)
|
|
focPosition = estPosition + context->SOFT_DIVIDE_NUM; // ahead phase of 90°
|
|
else if (focCurrent < 0)
|
|
focPosition = estPosition - context->SOFT_DIVIDE_NUM; // behind phase of 90°
|
|
else focPosition = estPosition;
|
|
|
|
context->driver->SetFocCurrentVector(focPosition, focCurrent);
|
|
}
|
|
|
|
|
|
void Motor::Controller::CalcPidToOutput(int32_t _speed)
|
|
{
|
|
config->pid.vErrorLast = config->pid.vError;
|
|
config->pid.vError = _speed - estVelocity;
|
|
if (config->pid.vError > (1024 * 1024)) config->pid.vError = (1024 * 1024);
|
|
if (config->pid.vError < (-1024 * 1024)) config->pid.vError = (-1024 * 1024);
|
|
config->pid.outputKp = ((config->pid.kp) * (config->pid.vError));
|
|
|
|
config->pid.integralRound += (config->pid.ki * config->pid.vError);
|
|
config->pid.integralRemainder = config->pid.integralRound >> 10;
|
|
config->pid.integralRound -= (config->pid.integralRemainder << 10);
|
|
config->pid.outputKi += config->pid.integralRemainder;
|
|
// integralRound limitation is ratedCurrent*1024
|
|
if (config->pid.outputKi > context->config.motionParams.ratedCurrent << 10)
|
|
config->pid.outputKi = context->config.motionParams.ratedCurrent << 10;
|
|
else if (config->pid.outputKi < -(context->config.motionParams.ratedCurrent << 10))
|
|
config->pid.outputKi = -(context->config.motionParams.ratedCurrent << 10);
|
|
|
|
config->pid.outputKd = config->pid.kd * (config->pid.vError - config->pid.vErrorLast);
|
|
|
|
config->pid.output = (config->pid.outputKp + config->pid.outputKi + config->pid.outputKd) >> 10;
|
|
if (config->pid.output > context->config.motionParams.ratedCurrent)
|
|
config->pid.output = context->config.motionParams.ratedCurrent;
|
|
else if (config->pid.output < -context->config.motionParams.ratedCurrent)
|
|
config->pid.output = -context->config.motionParams.ratedCurrent;
|
|
|
|
CalcCurrentToOutput(config->pid.output);
|
|
}
|
|
|
|
|
|
void Motor::Controller::CalcDceToOutput(int32_t _location, int32_t _speed)
|
|
{
|
|
config->dce.pError = _location - estPosition;
|
|
if (config->dce.pError > (3200)) config->dce.pError = (3200); // limited pError to 1/16r (51200/16)
|
|
if (config->dce.pError < (-3200)) config->dce.pError = (-3200);
|
|
config->dce.vError = (_speed - estVelocity) >> 7;
|
|
if (config->dce.vError > (4000)) config->dce.vError = (4000); // limited vError
|
|
if (config->dce.vError < (-4000)) config->dce.vError = (-4000);
|
|
|
|
config->dce.outputKp = config->dce.kp * config->dce.pError;
|
|
|
|
config->dce.integralRound += (config->dce.ki * config->dce.pError + config->dce.kv * config->dce.vError);
|
|
config->dce.integralRemainder = config->dce.integralRound >> 7;
|
|
config->dce.integralRound -= (config->dce.integralRemainder << 7);
|
|
config->dce.outputKi += config->dce.integralRemainder;
|
|
// limited to ratedCurrent * 1024, should be TUNED when use different scene
|
|
if (config->dce.outputKi > context->config.motionParams.ratedCurrent << 10)
|
|
config->dce.outputKi = context->config.motionParams.ratedCurrent << 10;
|
|
else if (config->dce.outputKi < -(context->config.motionParams.ratedCurrent << 10))
|
|
config->dce.outputKi = -(context->config.motionParams.ratedCurrent << 10);
|
|
|
|
config->dce.outputKd = ((config->dce.kd) * (config->dce.vError));
|
|
|
|
config->dce.output = (config->dce.outputKp + config->dce.outputKi + config->dce.outputKd) >> 10;
|
|
if (config->dce.output > context->config.motionParams.ratedCurrent)
|
|
config->dce.output = context->config.motionParams.ratedCurrent;
|
|
else if (config->dce.output < -context->config.motionParams.ratedCurrent)
|
|
config->dce.output = -context->config.motionParams.ratedCurrent;
|
|
|
|
CalcCurrentToOutput(config->dce.output);
|
|
}
|
|
|
|
|
|
void Motor::Controller::SetCtrlMode(Motor::Mode_t _mode)
|
|
{
|
|
requestMode = _mode;
|
|
}
|
|
|
|
|
|
void Motor::Controller::AddTrajectorySetPoint(int32_t _pos, int32_t _vel)
|
|
{
|
|
SetPositionSetPoint(_pos);
|
|
SetVelocitySetPoint(_vel);
|
|
}
|
|
|
|
|
|
void Motor::Controller::SetPositionSetPoint(int32_t _pos)
|
|
{
|
|
goalPosition = _pos + context->config.motionParams.encoderHomeOffset;
|
|
}
|
|
|
|
|
|
bool Motor::Controller::SetPositionSetPointWithTime(int32_t _pos, float _time)
|
|
{
|
|
int32_t deltaPos = abs(_pos - realPosition + context->config.motionParams.encoderHomeOffset);
|
|
|
|
float pMax = (float) context->config.motionParams.ratedVelocityAcc * _time * _time / 4;
|
|
if ((float) deltaPos > pMax)
|
|
{
|
|
context->config.motionParams.ratedVelocity = boardConfig.velocityLimit;
|
|
SetPositionSetPoint(_pos);
|
|
|
|
return false;
|
|
} else
|
|
{
|
|
float vMax = _time * (float) context->config.motionParams.ratedVelocityAcc;
|
|
vMax -= (float) context->config.motionParams.ratedVelocityAcc *
|
|
(sqrtf(_time * _time - 4 * (float) deltaPos /
|
|
(float) context->config.motionParams.ratedVelocityAcc));
|
|
vMax /= 2;
|
|
|
|
context->config.motionParams.ratedVelocity = (int32_t) vMax;
|
|
SetPositionSetPoint(_pos);
|
|
|
|
return true;
|
|
}
|
|
}
|
|
|
|
|
|
void Motor::Controller::SetVelocitySetPoint(int32_t _vel)
|
|
{
|
|
if ((_vel >= -context->config.motionParams.ratedVelocity) &&
|
|
(_vel <= context->config.motionParams.ratedVelocity))
|
|
{
|
|
goalVelocity = _vel;
|
|
}
|
|
}
|
|
|
|
|
|
float Motor::Controller::GetPosition(bool _isLap)
|
|
{
|
|
return _isLap ?
|
|
(float) (realLapPosition - context->config.motionParams.encoderHomeOffset) /
|
|
(float) (context->MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS)
|
|
:
|
|
(float) (realPosition - context->config.motionParams.encoderHomeOffset) /
|
|
(float) (context->MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS);
|
|
}
|
|
|
|
|
|
float Motor::Controller::GetVelocity()
|
|
{
|
|
return (float) estVelocity / (float) context->MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;
|
|
}
|
|
|
|
|
|
float Motor::Controller::GetFocCurrent()
|
|
{
|
|
return (float) focCurrent / 1000.f;
|
|
}
|
|
|
|
|
|
void Motor::Controller::SetCurrentSetPoint(int32_t _cur)
|
|
{
|
|
if (_cur > context->config.motionParams.ratedCurrent)
|
|
goalCurrent = context->config.motionParams.ratedCurrent;
|
|
else if (_cur < -context->config.motionParams.ratedCurrent)
|
|
goalCurrent = -context->config.motionParams.ratedCurrent;
|
|
else
|
|
goalCurrent = _cur;
|
|
}
|
|
|
|
|
|
void Motor::Controller::SetDisable(bool _disable)
|
|
{
|
|
goalDisable = _disable;
|
|
}
|
|
|
|
|
|
void Motor::Controller::SetBrake(bool _brake)
|
|
{
|
|
goalBrake = _brake;
|
|
|
|
}
|
|
|
|
|
|
void Motor::Controller::ClearStallFlag()
|
|
{
|
|
stalledTime = 0;
|
|
isStalled = false;
|
|
}
|
|
|
|
|
|
int32_t Motor::Controller::CompensateAdvancedAngle(int32_t _vel)
|
|
{
|
|
/*
|
|
* The code is for DPS series sensors, need to measured and renew for TLE5012/MT6816.
|
|
*/
|
|
|
|
int32_t compensate;
|
|
|
|
if (_vel < 0)
|
|
{
|
|
if (_vel > -100000) compensate = 0;
|
|
else if (_vel > -1300000) compensate = (((_vel + 100000) * 262) >> 20) - 0;
|
|
else if (_vel > -2200000) compensate = (((_vel + 1300000) * 105) >> 20) - 300;
|
|
else compensate = (((_vel + 2200000) * 52) >> 20) - 390;
|
|
|
|
if (compensate < -430) compensate = -430;
|
|
} else
|
|
{
|
|
if (_vel < 100000) compensate = 0;
|
|
else if (_vel < 1300000) compensate = (((_vel - 100000) * 262) >> 20) + 0;
|
|
else if (_vel < 2200000) compensate = (((_vel - 1300000) * 105) >> 20) + 300;
|
|
else compensate = (((_vel - 2200000) * 52) >> 20) + 390;
|
|
|
|
if (compensate > 430) compensate = 430;
|
|
}
|
|
|
|
return compensate;
|
|
}
|
|
|
|
|
|
void Motor::Controller::Init()
|
|
{
|
|
requestMode = boardConfig.enableMotorOnBoot ? static_cast<Mode_t>(boardConfig.defaultMode) : MODE_STOP;
|
|
|
|
modeRunning = MODE_STOP;
|
|
state = STATE_STOP;
|
|
|
|
realLapPosition = 0;
|
|
realLapPositionLast = 0;
|
|
realPosition = 0;
|
|
realPositionLast = 0;
|
|
|
|
estVelocityIntegral = 0;
|
|
estVelocity = 0;
|
|
estLeadPosition = 0;
|
|
estPosition = 0;
|
|
estError = 0;
|
|
|
|
goalPosition = context->config.motionParams.encoderHomeOffset;
|
|
goalVelocity = 0;
|
|
goalCurrent = 0;
|
|
goalDisable = false;
|
|
goalBrake = false;
|
|
|
|
softPosition = 0;
|
|
softVelocity = 0;
|
|
softCurrent = 0;
|
|
softDisable = false;
|
|
softBrake = false;
|
|
softNewCurve = false;
|
|
|
|
focPosition = 0;
|
|
focCurrent = 0;
|
|
|
|
stalledTime = 0;
|
|
isStalled = false;
|
|
|
|
overloadTime = 0;
|
|
overloadFlag = false;
|
|
|
|
config->pid.vError = 0;
|
|
config->pid.vErrorLast = 0;
|
|
config->pid.outputKp = 0;
|
|
config->pid.outputKi = 0;
|
|
config->pid.outputKd = 0;
|
|
config->pid.integralRound = 0;
|
|
config->pid.integralRemainder = 0;
|
|
config->pid.output = 0;
|
|
|
|
config->dce.pError = 0;
|
|
config->dce.vError = 0;
|
|
config->dce.outputKp = 0;
|
|
config->dce.outputKi = 0;
|
|
config->dce.outputKd = 0;
|
|
config->dce.integralRound = 0;
|
|
config->dce.integralRemainder = 0;
|
|
config->dce.output = 0;
|
|
}
|
|
|
|
|
|
void Motor::Controller::ApplyPosAsHomeOffset()
|
|
{
|
|
context->config.motionParams.encoderHomeOffset = realPosition %
|
|
context->MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;
|
|
}
|
|
|
|
|
|
void Motor::Controller::AttachConfig(Motor::Controller::Config_t* _config)
|
|
{
|
|
config = _config;
|
|
}
|
|
|
|
|
|
void Motor::Controller::ClearIntegral() const
|
|
{
|
|
config->pid.integralRound = 0;
|
|
config->pid.integralRemainder = 0;
|
|
config->pid.outputKi = 0;
|
|
|
|
config->dce.integralRound = 0;
|
|
config->dce.integralRemainder = 0;
|
|
config->dce.outputKi = 0;
|
|
}
|
|
|
|
|