diff --git a/.vscode/settings.json b/.vscode/settings.json index 3370206..51c1d3c 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -128,6 +128,7 @@ "carr", "CIPO", "COPI", + "Dutycycle", "gast", "GNSS", "GPGGA", @@ -148,6 +149,7 @@ "Soln", "TPMOBIL", "UBLOX", + "wheelspeed", "ZLPJ" ], "cmake.configureOnOpen": false diff --git a/doc/TODO allgemein.txt b/doc/TODO allgemein.txt index d89abe9..771fbb1 100644 --- a/doc/TODO allgemein.txt +++ b/doc/TODO allgemein.txt @@ -33,7 +33,6 @@ Do later: Do now: Code: Doxygen Kommentare aktualisieren - magic numbers etc Fernbedienung! Latex: diff --git a/include/config.h b/include/config.h index d3828d3..5ea15ec 100644 --- a/include/config.h +++ b/include/config.h @@ -1,17 +1,22 @@ /** * @file config.h * @author Alexander Klein (alex@kleiax.de) - * @brief Some defines to configere the project. + * @brief Some defines to configure the project. * @version 0.1 * @date 2022-02-15 - * + * * @copyright Copyright (c) 2022 - * + * */ #pragma once -namespace PinNumbers { +/** + * @brief The numbers of pins to be used + * + */ +namespace PinNumbers +{ constexpr uint8_t spiCopi = 16; constexpr uint8_t spiCipo = 4; constexpr uint8_t spiSck = 5; @@ -20,24 +25,31 @@ namespace PinNumbers { constexpr uint8_t scl = 19; constexpr uint8_t battery = 35; - namespace LeftMotor { + namespace LeftMotor + { constexpr uint8_t dir1 = 27; constexpr uint8_t dir2 = 12; - constexpr uint8_t pwm = 13; + constexpr uint8_t pwm = 13; constexpr uint8_t encoder = 32; constexpr uint8_t pmwChannel = 0; } - namespace RightMotor { + namespace RightMotor + { constexpr uint8_t dir1 = 14; constexpr uint8_t dir2 = 23; - constexpr uint8_t pwm = 22; + constexpr uint8_t pwm = 22; constexpr uint8_t encoder = 33; constexpr uint8_t pmwChannel = 1; } } -namespace Settings { +/** + * @brief Default values for different components + * + */ +namespace Settings +{ constexpr uint32_t baudRate = 115200; constexpr uint32_t i2cSpeed = 400000; @@ -45,14 +57,17 @@ namespace Settings { constexpr float wheelDistance = 0.255; constexpr uint16_t encoderSteps = 384; - namespace Pid { - namespace Left { + namespace Pid + { + namespace Left + { constexpr uint8_t P = 5; constexpr uint8_t I = 0; constexpr uint8_t D = 0; } - namespace Right { + namespace Right + { constexpr uint8_t P = 5; constexpr uint8_t I = 0; constexpr uint8_t D = 0; diff --git a/include/moveControl.h b/include/moveControl.h index e274e9b..f2a1f08 100644 --- a/include/moveControl.h +++ b/include/moveControl.h @@ -22,6 +22,11 @@ #include "debugTimes.h" #include "component.h" + +/** + * @brief A struct to easy set the speed + * + */ struct DrivingSpeeds { double x; double rot; @@ -41,6 +46,11 @@ class MoveControl : public Component { * @brief used to set driving status * * When set to stop all motors are set to halt + * When set to Raw the PIDs are not used. The target + * values must be given withe @see setRawPowerLeft and + * @see setRawPowerRight. + * + * Drive is the normal working mode. * */ enum Status {Stop, @@ -52,7 +62,7 @@ class MoveControl : public Component { * @brief Construct a new Move Control object * * Initialize the motors, encoders and PIDs with given - * values in moveControlConfig.h + * values in config.h */ MoveControl(); @@ -66,6 +76,9 @@ class MoveControl : public Component { /** * @brief Set the Status * + * Control if this component should work normally or with + * raw values for the motors. + * * @see Status * @param status */ @@ -87,29 +100,39 @@ class MoveControl : public Component { void setSpeed(double speed); /** - * @brief Set the rotationspeed + * @brief Set the rotationSpeed * - * If the given rotationspeed is close to zero, then the - * target rotationspeed is set to zero. + * If the given rotationSpeed is close to zero, then the + * target rotationSpeed is set to zero. * @param speed rad/s */ void setRotationSpeed(double speed); + /** + * @brief Set the speeds + * + * This function is a combination of @see setSpeed and + * @see setRotationSpeed. The struct @see DrivingSpeeds + * is used for this. + * + * @param drivingSpeeds + */ void setSpeeds(DrivingSpeeds drivingSpeeds); /** - * @brief Set Raw Power Left + * @brief Set raw power left * - * This value has only an effect if Status is raw. + * This value has only an effect if the status is raw. + * @see setDrivingStatus * * @param power between -100 and 100 */ void setRawPowerLeft(int16_t power); /** - * @brief Set the Raw Power Right + * @brief Set the raw power right * - * This value has only an effect if Status is raw. + * This value has only an effect if status is raw. * * @param power between -100 and 100 */ @@ -119,9 +142,9 @@ class MoveControl : public Component { * @brief Set the pid tunings * * @param side 0 -> left, 1 -> right - * @param p - * @param i - * @param d + * @param pPart + * @param iPart + * @param dPart */ void setPidTunings(uint8_t side, double pPart, double iPart, double dPart); @@ -133,22 +156,11 @@ class MoveControl : public Component { */ PID* getPID(uint8_t side) const; - /** - * @brief Get the Speedometer Left object - * - * @return Speedometer* - */ - Speedometer* getSpeedometerLeft() const { return this->left_speedometer; } + Speedometer* getSpeedometerLeft() const { return this->leftSpeedometer; } + Speedometer* getSpeedometerRight() const { return this->rightSpeedometer; } - /** - * @brief Get the Speedometer Right object - * - * @return Speedometer* - */ - Speedometer* getSpeedometerRight() const { return this->right_speedometer; } - - uint16_t getDutycycleLeft() const { return this->left_motor->getDutycycle(); } - uint16_t getDutycycleRight() const { return this->right_motor->getDutycycle(); } + uint16_t getDutycycleLeft() const { return this->leftMotor->getDutycycle(); } + uint16_t getDutycycleRight() const { return this->rightMotor->getDutycycle(); } private: void run() override; @@ -157,22 +169,22 @@ class MoveControl : public Component { void regulateMotors(); void updateCurrentWheelSpeed(); - MotorControl *left_motor; - MotorControl *right_motor; - Speedometer *left_speedometer; - Speedometer *right_speedometer; - PID *left_pid; - PID *right_pid; + MotorControl *leftMotor; + MotorControl *rightMotor; + Speedometer *leftSpeedometer; + Speedometer *rightSpeedometer; + PID *leftPid; + PID *rightPid; Status driving_status = Status::Stop; DrivingSpeeds drivingSpeeds = {0, 0}; - double wheelspeed_left_target = 0; - double wheelspeed_right_target = 0; - double wheelspeed_left = 0; - double wheelspeed_right = 0; - double left_pid_out = 0; - double right_pid_out = 0; + double wheelspeedLeftTarget = 0; + double wheelspeedRightTarget = 0; + double wheelspeedLeft = 0; + double wheelspeedRight = 0; + double leftPidOut = 0; + double rightPidOut = 0; uint8_t overTimeCounter = 0; uint8_t overTimeMax = 100; diff --git a/src/moveControl.cpp b/src/moveControl.cpp index 3f9fce5..145a786 100644 --- a/src/moveControl.cpp +++ b/src/moveControl.cpp @@ -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(this->left_pid_out)); - this->right_motor->setTargetPower(static_cast(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(this->leftPidOut)); + this->rightMotor->setTargetPower(static_cast(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(); }