kleiax/Bachelorarbeit-Rover
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases 1 Wiki Activity

update comments 1

Browse Source
This commit is contained in:
kleiax
2023-10-12 08:43:33 +02:00
parent a43e2db92b
commit 61a95e4601
5 changed files with 133 additions and 105 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/moveControl.cpp
+54 -54
View File
@@ -12,53 +12,53 @@
MoveControl::MoveControl()
: Component(MoveControl::loopDelay),
left_motor{new MotorControl()},
right_motor{new MotorControl()},
left_speedometer{new Speedometer(PinNumbers::LeftMotor::encoder, Settings::wheelDiameter, Settings::encoderSteps)},
right_speedometer{new Speedometer(PinNumbers::RightMotor::encoder, Settings::wheelDiameter, Settings::encoderSteps)},
left_pid{new PID(&this->wheelspeed_left,
&this->left_pid_out,
&this->wheelspeed_left_target,
leftMotor{new MotorControl()},
rightMotor{new MotorControl()},
leftSpeedometer{new Speedometer(PinNumbers::LeftMotor::encoder, Settings::wheelDiameter, Settings::encoderSteps)},
rightSpeedometer{new Speedometer(PinNumbers::RightMotor::encoder, Settings::wheelDiameter, Settings::encoderSteps)},
leftPid{new PID(&this->wheelspeedLeft,
&this->leftPidOut,
&this->wheelspeedLeftTarget,
Settings::Pid::Left::P,
Settings::Pid::Left::I,
Settings::Pid::Left::D,
DIRECT)},
right_pid{new PID(&this->wheelspeed_right,
&this->right_pid_out,
&this->wheelspeed_right_target,
rightPid{new PID(&this->wheelspeedRight,
&this->rightPidOut,
&this->wheelspeedRightTarget,
Settings::Pid::Right::P,
Settings::Pid::Right::I,
Settings::Pid::Right::D,
DIRECT)}
{
this->left_pid->SetOutputLimits(Settings::Pid::outMin, Settings::Pid::outMax);
this->left_pid->SetSampleTime(Settings::Pid::sampleTime);
this->left_pid->SetMode(AUTOMATIC);
this->leftPid->SetOutputLimits(Settings::Pid::outMin, Settings::Pid::outMax);
this->leftPid->SetSampleTime(Settings::Pid::sampleTime);
this->leftPid->SetMode(AUTOMATIC);
this->right_pid->SetOutputLimits(Settings::Pid::outMin, Settings::Pid::outMax);
this->right_pid->SetSampleTime(Settings::Pid::sampleTime);
this->right_pid->SetMode(AUTOMATIC);
this->rightPid->SetOutputLimits(Settings::Pid::outMin, Settings::Pid::outMax);
this->rightPid->SetSampleTime(Settings::Pid::sampleTime);
this->rightPid->SetMode(AUTOMATIC);
this->left_motor->init(PinNumbers::LeftMotor::pwm, PinNumbers::LeftMotor::pmwChannel, PinNumbers::LeftMotor::dir1, PinNumbers::LeftMotor::dir2);
this->right_motor->init(PinNumbers::RightMotor::pwm, PinNumbers::RightMotor::pmwChannel, PinNumbers::RightMotor::dir1, PinNumbers::RightMotor::dir2);
this->leftMotor->init(PinNumbers::LeftMotor::pwm, PinNumbers::LeftMotor::pmwChannel, PinNumbers::LeftMotor::dir1, PinNumbers::LeftMotor::dir2);
this->rightMotor->init(PinNumbers::RightMotor::pwm, PinNumbers::RightMotor::pmwChannel, PinNumbers::RightMotor::dir1, PinNumbers::RightMotor::dir2);
this->addChildComponent(this->left_motor);
this->addChildComponent(this->right_motor);
this->addChildComponent(this->left_speedometer);
this->addChildComponent(this->right_speedometer);
this->addChildComponent(this->leftMotor);
this->addChildComponent(this->rightMotor);
this->addChildComponent(this->leftSpeedometer);
this->addChildComponent(this->rightSpeedometer);
}
MoveControl::~MoveControl()
{
this->left_motor->emergencyStop();
this->right_motor->emergencyStop();
this->leftMotor->emergencyStop();
this->rightMotor->emergencyStop();
delete this->left_motor;
delete this->right_motor;
delete this->left_speedometer;
delete this->right_speedometer;
delete this->left_pid;
delete this->right_pid;
delete this->leftMotor;
delete this->rightMotor;
delete this->leftSpeedometer;
delete this->rightSpeedometer;
delete this->leftPid;
delete this->rightPid;
}
void MoveControl::run()
@@ -66,8 +66,8 @@ void MoveControl::run()
this->updateCurrentWheelSpeed();
this->calcTargetWheelSpeed();
this->right_pid->Compute();
this->left_pid->Compute();
this->rightPid->Compute();
this->leftPid->Compute();
this->regulateMotors();
}
@@ -100,8 +100,8 @@ void MoveControl::setDrivingStatus(Status status)
void MoveControl::emergencyStop()
{
this->left_motor->emergencyStop();
this->right_motor->emergencyStop();
this->leftMotor->emergencyStop();
this->rightMotor->emergencyStop();
this->setSpeed(0);
this->setRotationSpeed(0);
this->setRawPowerLeft(0);
@@ -158,11 +158,11 @@ void MoveControl::setPidTunings(uint8_t side, double pPart, double iPart, double
PID *selectedPID = nullptr;
if (side == 0)
{
selectedPID = this->left_pid;
selectedPID = this->leftPid;
}
else if (side == 1)
{
selectedPID = this->right_pid;
selectedPID = this->rightPid;
}
selectedPID->SetTunings(pPart, iPart, dPart);
@@ -172,11 +172,11 @@ PID *MoveControl::getPID(uint8_t side) const
{
if (side == 0)
{
return this->left_pid;
return this->leftPid;
}
if (side == 1)
{
return this->right_pid;
return this->rightPid;
}
return nullptr;
}
@@ -208,8 +208,8 @@ void MoveControl::calcTargetWheelSpeed()
// (1 / r) * b
constexpr double B1rb = (1.0 / (Settings::wheelDiameter / 2)) * (Settings::wheelDistance / 2);
this->wheelspeed_right_target = (A1r1 * this->drivingSpeeds.x + B1rb * this->drivingSpeeds.rot);
this->wheelspeed_left_target = (A1r1 * this->drivingSpeeds.x + (-B1rb) * this->drivingSpeeds.rot);
this->wheelspeedRightTarget = (A1r1 * this->drivingSpeeds.x + B1rb * this->drivingSpeeds.rot);
this->wheelspeedLeftTarget = (A1r1 * this->drivingSpeeds.x + (-B1rb) * this->drivingSpeeds.rot);
}
void MoveControl::regulateMotors()
@@ -217,24 +217,24 @@ void MoveControl::regulateMotors()
switch (this->driving_status)
{
case Status::Stop:
this->left_motor->setTargetPower(0);
this->right_motor->setTargetPower(0);
this->setSpeedometerDirection(this->left_speedometer, 0);
this->setSpeedometerDirection(this->right_speedometer, 0);
this->leftMotor->setTargetPower(0);
this->rightMotor->setTargetPower(0);
this->setSpeedometerDirection(this->leftSpeedometer, 0);
this->setSpeedometerDirection(this->rightSpeedometer, 0);
break;
case Status::Drive:
this->left_motor->setTargetPower(static_cast<int8_t>(this->left_pid_out));
this->right_motor->setTargetPower(static_cast<int8_t>(this->right_pid_out));
this->setSpeedometerDirection(this->left_speedometer, this->left_motor->getPower());
this->setSpeedometerDirection(this->right_speedometer, this->right_motor->getPower());
this->leftMotor->setTargetPower(static_cast<int8_t>(this->leftPidOut));
this->rightMotor->setTargetPower(static_cast<int8_t>(this->rightPidOut));
this->setSpeedometerDirection(this->leftSpeedometer, this->leftMotor->getPower());
this->setSpeedometerDirection(this->rightSpeedometer, this->rightMotor->getPower());
break;
case Status::Raw:
this->left_motor->setTargetPower(this->rawPowerLeft);
this->right_motor->setTargetPower(this->rawPowerRight);
this->setSpeedometerDirection(this->left_speedometer, this->rawPowerLeft);
this->setSpeedometerDirection(this->right_speedometer, this->rawPowerRight);
this->leftMotor->setTargetPower(this->rawPowerLeft);
this->rightMotor->setTargetPower(this->rawPowerRight);
this->setSpeedometerDirection(this->leftSpeedometer, this->rawPowerLeft);
this->setSpeedometerDirection(this->rightSpeedometer, this->rawPowerRight);
break;
default:
@@ -245,6 +245,6 @@ void MoveControl::regulateMotors()
void MoveControl::updateCurrentWheelSpeed()
{
this->wheelspeed_left = this->left_speedometer->getSpeedRad();
this->wheelspeed_right = this->right_speedometer->getSpeedRad();
this->wheelspeedLeft = this->leftSpeedometer->getSpeedRad();
this->wheelspeedRight = this->rightSpeedometer->getSpeedRad();
}