Bachelorarbeit-Rover/src/moveControl.cpp

/**
 * @file moveControl.cpp
 * @author Alexander Klein (alex@kleiax.de)
 * @brief Contains an implementation of the class MoveControl
 * @version 0.1
 * @date 2022-02-15
 * 
 * @copyright Copyright (c) 2022
 * 
 */
#include "moveControl.h"

MoveControl::MoveControl() {
    this->loopDelay = 20;
    this->left_motor = new MotorControl();
    this->right_motor = new MotorControl();
    this->left_speedometer = new Speedometer(PinNumbers::LeftMotor::encoder, Settings::wheelDiameter, Settings::encoderSteps);
    this->right_speedometer = new Speedometer(PinNumbers::RightMotor::encoder, Settings::wheelDiameter, Settings::encoderSteps);

    this->left_pid = new PID(   &this->wheelspeed_left,
                                &this->left_pid_out,
                                &this->wheelspeed_left_target,
                                Settings::Pid::Left::P,
                                Settings::Pid::Left::I,
                                Settings::Pid::Left::D,
                                DIRECT);
    this->right_pid = new PID(  &this->wheelspeed_right,
                                &this->right_pid_out,
                                &this->wheelspeed_right_target,
                                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->right_pid->SetOutputLimits(Settings::Pid::outMin, Settings::Pid::outMax);
    this->right_pid->SetSampleTime(Settings::Pid::sampleTime);
    this->right_pid->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->addChildComponent(this->left_motor);
    this->addChildComponent(this->right_motor); 
    this->addChildComponent(this->left_speedometer);
    this->addChildComponent(this->right_speedometer);                             
}

MoveControl::~MoveControl() {
    this->left_motor->emergencyStop();
    this->right_motor->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;

}

void MoveControl::run() {
    this->updateCurrentWheelSpeed();
    this->calcTargetWheelSpeed();

    this->right_pid->Compute();
    this->left_pid->Compute();

    this->regulateMotors();
}

void MoveControl::setDrivingStatus(Status status) {
    this->setSpeed(0);
    this->setRotationSpeed(0);
    this->setRawPowerLeft(0);
    this->setRawPowerRight(0);
    this->driving_status = status;
    // switch (this->driving_status) {
    //     case Status::Stop :
    //         Serial.println("New drivingState = Stop in MoveControl::setDrivingStatus");
    //         break;

    //     case Status::Drive :
    //         Serial.println("New drivingState = Drive in MoveControl::setDrivingStatus");
    //         break;

    //     default:
    //         Serial.println("Wrong drivingState in MoveControl::regulateMotors");
    //         break;
    // }
}

void MoveControl::emergencyStop() {
    this->left_motor->emergencyStop();
    this->right_motor->emergencyStop();
    this->setSpeed(0);
    this->setRotationSpeed(0);
    this->setRawPowerLeft(0);
    this->setRawPowerRight(0);
    this->driving_status = Status::Stop;
}

void MoveControl::setSpeed(double speed) {
    if (speed < 0.2 && speed > -0.2) {
        this->x_speed = 0;
        return;
    }

    this->x_speed = speed;
}

void MoveControl::setRotationSpeed(double speed) {
    if (speed < 0.1 && speed > -0.1){
        this->rotation_speed = 0;
        return;
    }

    this->rotation_speed = speed;
}

void MoveControl::setRawPowerLeft(int16_t power) {
    if (power <= 100 && power >= -100)
        this->rawPowerLeft = power;
}

void MoveControl::setRawPowerRight(int16_t power) {
    if (power <= 100 && power >= -100)
        this->rawPowerRight = power;
}

void MoveControl::setPidTunings(uint8_t side, double p, double i, double d) {
    PID* selectedPID = nullptr;
    if (side == 0)
        selectedPID = this->left_pid;
    else if (side == 1)
        selectedPID = this->right_pid;

    selectedPID->SetTunings(p, i, d);
}

PID* MoveControl::getPID(uint8_t side) const {
    if (side == 0)
        return this->left_pid;
    else if (side == 1)
        return this->right_pid;
    else
        return nullptr;
}

void MoveControl::setSpeedometerDirection(Speedometer *speedometer, double value) {
    if (value > 0)
        speedometer->setDirection(Speedometer::Direction::Forward);
    else if (value < 0)
        speedometer->setDirection(Speedometer::Direction::Backward);
    else
        speedometer->setDirection(Speedometer::Direction::None);
}

void MoveControl::calcTargetWheelSpeed() {
    /* original formula:
       (1 / r) / 1  b \ / x \ = / Xl \
               \ 1 -b / \ T /   \ Xr /  */

    // (1 / r) * 1
    constexpr double A = 15.82278481;
    // (1 / r) * b
    constexpr double B = 2.096518987;

    this->wheelspeed_right_target = (A * this->x_speed + B * this->rotation_speed) * (Settings::wheelDiameter / 2);
    this->wheelspeed_left_target  = (A * this->x_speed + (-B) * this->rotation_speed) * (Settings::wheelDiameter / 2);
}

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);
            break;

        case Status::Drive :
            this->left_motor->setTargetPower( (int8_t) this->left_pid_out);
            this->right_motor->setTargetPower( (int8_t) this->right_pid_out);
            this->setSpeedometerDirection(this->left_speedometer, this->left_motor->getPower());
            this->setSpeedometerDirection(this->right_speedometer, this->right_motor->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);
            break;

        default:
            Serial.println("Wrong drivingState in MoveControl::regulateMotors");
            break;
    }
}

void MoveControl::updateCurrentWheelSpeed() {
    this->wheelspeed_left = this->left_speedometer->getSpeed();
    this->wheelspeed_right = this->right_speedometer->getSpeed();
}