Bachelorarbeit-Rover/lib/MotorControl/motorControl.cpp

/**
 * @file motorControl.cpp
 * @author Alexander Klein (alex@kleiax.de)
 * @brief Implemention of the class motorControl.h.
 * @see motorControl.h
 * @version 0.1
 * @date 2021-12-13
 * 
 * @copyright Copyright (c) 2021
 * 
 */

#include "motorControl.h"

MotorControl::MotorControl() {
    this->setMinPwm(PWMMIN);
    this->setMaxPwm(PWMMAX);
    this->loopDelay = DELAY;
}

void MotorControl::init(uint8_t pwmPin, uint8_t pwmChannel, uint8_t dir_1, uint8_t dir_2) {
    this->pwmPin = pwmPin;
    this->pwmChannel = pwmChannel;
    this->dir_1 = dir_1;
    this->dir_2 = dir_2;

    pinMode(this->dir_1, OUTPUT);
    pinMode(this->dir_2, OUTPUT);

    digitalWrite(this->dir_1, LOW);
    digitalWrite(this->dir_2, LOW);

    ledcSetup(this->pwmChannel, PWMFREQ, this->pwmRes);
    ledcAttachPin(this->pwmPin, this->pwmChannel);
    ledcWrite(this->pwmChannel, 0);
}

void MotorControl::run() {
    // Absolute difference between targetPower and power
    uint8_t abs_difference = abs(this->targetPower - this->power);

    // Difference between targetPower and power
    int16_t difference = this->targetPower - this->power;

    // Check that the target speed is close to 0 and that the abs_difference is lower than powersteps
    if (abs(this->targetPower) <  powersteps && abs_difference < powersteps) {
        this->setRealPower(0);
        return;
    }

    // Correct speed
    if (abs_difference < powersteps) {
        return;
    }

    // Positive or negative tagret speed
    if (this->targetPower >= 0) {
        // Positive or negative speed
        if (this->power >= 0) {
            if (difference > 0) {
                this->increasePower(powersteps);
            } else {
                this->increasePower(-powersteps);
            }
        } else {
            this->increasePower(powersteps);
        }

    } else {
        // Positive or negative speed
        if (this->power >= 0) {
            this->increasePower(-powersteps);
        } else {
            if (difference > 0) {
                this->increasePower(powersteps);
            } else {
                this->increasePower(-powersteps);
            }
        }
    }
}

void MotorControl::setMinPwm(uint8_t min) {
    if (min > 80) min = 80;
    //transform percentage to real pwm value
    min = (uint8_t) (((1 << pwmRes) - 1) * (min / 100.0));
    this->dutycycleMin = min;
}

void MotorControl::setMaxPwm(uint8_t max) {
    if (max > 100) max = 100;
    //transform percentage to real pwm value
    max = (uint8_t) (((1 << pwmRes) - 1) * (max / 100.0));
    this->dutycycleMax = max;
}

uint16_t MotorControl::setPowerSteps(uint8_t increment) {
    this->powersteps = increment;
    return (uint16_t) (this->loopDelay * ( 100 / powersteps ));
}

void MotorControl::setTargetPower(int8_t power) {
    if (power <= 100 && power >= -100)
        this->targetPower = power;
    else
        std::cout << " MotorControl::setTargetPower: Invalid Argument - Power: " << power << std::endl;
}

void MotorControl::stop() {
    this->targetPower = 0;
}

void MotorControl::emergencyStop() {
    setRealPower(0);
}

bool MotorControl::isTargetPowerReached() const {
    if (this->targetPower == this->power)
        return true;
    return false;
}

bool MotorControl::isAccelerationPositive() const {
    if (power < targetPower)
        return true;
    return false;
}

bool MotorControl::isAccelerationNegative() const {
    if (power > targetPower)
        return true;
    return false;
}

void MotorControl::setRealPower(int8_t power) {
    //TODO: Exceptionhandling
    if (power <= 100 && power >= -100) {
        this->power = power;
    } else {
        return;
    }

    if (this->power == 0) {
        this->direction = 0;
        digitalWrite(this->dir_1, LOW);
        digitalWrite(this->dir_2, LOW);
        ledcWrite(this->pwmChannel, 0);
        this->dutycycle = 0;
        return;
    }

    uint8_t pwm_val = map(abs(power), 0, 100, this->dutycycleMin, this->dutycycleMax);

    if ((this->direction == 1 || this->direction == 0) && power < 0){  // new direction backward
        this->direction = 2;
        digitalWrite(this->dir_1, LOW);
        digitalWrite(this->dir_2, HIGH);
    } else if ((this->direction == 2 || this->direction == 0) && power > 0){  // new direction forward
        this->direction = 1;
        digitalWrite(this->dir_1, HIGH);
        digitalWrite(this->dir_2, LOW);
    }

    ledcWrite(this->pwmChannel, pwm_val);
    this->dutycycle = pwm_val;
}

void MotorControl::increasePower(int8_t power) {
    //TODO: Exceptionhandling
    //TODO: make a stop befor a direction change
    if (abs(power) > 2 * powersteps) {
        Serial.println("Invalid Argument in MotorControl::increasePower");
        return;
    }

    this->setRealPower(this->power + power);
}