File content as of revision 0:6a4d3264c067:

/*
 * Controller.cpp
 * Copyright (c) 2020, ZHAW
 * All rights reserved.
 */

#include "Controller.h"

using namespace std;

const float Controller::PERIOD = 0.001f;                    // period of 1 ms
const float Controller::PI = 3.14159265f;                   // the constant PI
const float Controller::WHEEL_DISTANCE = 0.185f;            // distance between wheels, given in [m]
const float Controller::WHEEL_RADIUS = 0.038f;              // radius of wheels, given in [m]
const float Controller::COUNTS_PER_TURN = 86016.0f;         // encoder resolution (pololu motors: 1200.0f, maxon motors: 86016.0f)
const float Controller::LOWPASS_FILTER_FREQUENCY = 300.0f;  // given in [rad/s]
const float Controller::KN = 45.0f;                         // speed constant in [rpm/V] (pololu motors: 40.0f, maxon motors: 45.0f)
const float Controller::KP = 0.1f;                          // speed control parameter
const float Controller::MAX_VOLTAGE = 12.0f;                // battery voltage in [V]
const float Controller::MIN_DUTY_CYCLE = 0.02f;             // minimum duty-cycle
const float Controller::MAX_DUTY_CYCLE = 0.98f;             // maximum duty-cycle

/**
 * Creates and initialises the robot controller.
 * @param pwmLeft a reference to the pwm output for the left motor.
 * @param pwmRight a reference to the pwm output for the right motor.
 * @param counterLeft a reference to the encoder counter of the left motor.
 * @param counterRight a reference to the encoder counter of the right motor.
 */
Controller::Controller(PwmOut& pwmLeft, PwmOut& pwmRight, EncoderCounter& counterLeft, EncoderCounter& counterRight) : pwmLeft(pwmLeft), pwmRight(pwmRight), counterLeft(counterLeft), counterRight(counterRight) {
    
    // initialise pwm outputs
    
    pwmLeft.period(0.00005f);  // pwm period of 50 us
    pwmLeft = 0.5f;            // duty-cycle of 50%
    
    pwmRight.period(0.00005f); // pwm period of 50 us
    pwmRight = 0.5f;           // duty-cycle of 50%
    
    // initialise local variables
    
    translationalMotion.setProfileVelocity(2.0f);
    translationalMotion.setProfileAcceleration(2.0f);
    translationalMotion.setProfileDeceleration(4.0f);
    
    rotationalMotion.setProfileVelocity(6.0f);
    rotationalMotion.setProfileAcceleration(12.0f);
    rotationalMotion.setProfileDeceleration(12.0f);
    
    translationalVelocity = 0.0f;
    rotationalVelocity = 0.0f;
    actualTranslationalVelocity = 0.0f;
    actualRotationalVelocity = 0.0f;
    
    previousValueCounterLeft = counterLeft.read();
    previousValueCounterRight = counterRight.read();
    
    speedLeftFilter.setPeriod(PERIOD);
    speedLeftFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
    
    speedRightFilter.setPeriod(PERIOD);
    speedRightFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
    
    desiredSpeedLeft = 0.0f;
    desiredSpeedRight = 0.0f;
    
    actualSpeedLeft = 0.0f;
    actualSpeedRight = 0.0f;
    
    x = 0.0f;
    y = 0.0f;
    alpha = 0.0f;
    
    // start the periodic task
    
    ticker.attach(callback(this, &Controller::run), PERIOD);
}

/**
 * Deletes this Controller object.
 */
Controller::~Controller() {
    
    ticker.detach(); // stop the periodic task
}

/**
 * Sets the desired translational velocity of the robot.
 * @param velocity the desired translational velocity, given in [m/s].
 */
void Controller::setTranslationalVelocity(float velocity) {
    
    this->translationalVelocity = velocity;
}

/**
 * Sets the desired rotational velocity of the robot.
 * @param velocity the desired rotational velocity, given in [rad/s].
 */
void Controller::setRotationalVelocity(float velocity) {
    
    this->rotationalVelocity = velocity;
}

/**
 * Gets the actual translational velocity of the robot.
 * @return the actual translational velocity, given in [m/s].
 */
float Controller::getActualTranslationalVelocity() {
    
    return actualTranslationalVelocity;
}

/**
 * Gets the actual rotational velocity of the robot.
 * @return the actual rotational velocity, given in [rad/s].
 */
float Controller::getActualRotationalVelocity() {
    
    return actualRotationalVelocity;
}

/**
 * Sets the desired speed of the left motor.
 * @param desiredSpeedLeft desired speed given in [rpm].
 */
void Controller::setDesiredSpeedLeft(float desiredSpeedLeft) {

    this->desiredSpeedLeft = desiredSpeedLeft;
}

/**
 * Sets the desired speed of the right motor.
 * @param desiredSpeedRight desired speed given in [rpm].
 */
void Controller::setDesiredSpeedRight(float desiredSpeedRight) {

    this->desiredSpeedRight = desiredSpeedRight;
}

/**
 * Gets the actual speed of the left motor.
 * @return the actual speed given in [rpm].
 */
float Controller::getActualSpeedLeft() {

    return actualSpeedLeft;
}

/**
 * Gets the actual speed of the right motor.
 * @return the actual speed given in [rpm].
 */
float Controller::getActualSpeedRight() {

    return actualSpeedRight;
}

/**
 * Sets the actual x coordinate of the robots position.
 * @param x the x coordinate of the position, given in [m].
 */
void Controller::setX(float x) {
    
    this->x = x;
}

/**
 * Gets the actual x coordinate of the robots position.
 * @return the x coordinate of the position, given in [m].
 */
float Controller::getX() {
    
    return x;
}

/**
 * Sets the actual y coordinate of the robots position.
 * @param y the y coordinate of the position, given in [m].
 */
void Controller::setY(float y) {
    
    this->y = y;
}

/**
 * Gets the actual y coordinate of the robots position.
 * @return the y coordinate of the position, given in [m].
 */
float Controller::getY() {
    
    return y;
}

/**
 * Sets the actual orientation of the robot.
 * @param alpha the orientation, given in [rad].
 */
void Controller::setAlpha(float alpha) {
    
    this->alpha = alpha;
}

/**
 * Gets the actual orientation of the robot.
 * @return the orientation, given in [rad].
 */
float Controller::getAlpha() {
    
    return alpha;
}

/**
 * This is an internal method of the controller that is running periodically.
 */
void Controller::run() {
    
    // calculate the planned velocities using the motion planner
    
    translationalMotion.incrementToVelocity(translationalVelocity, PERIOD);
    rotationalMotion.incrementToVelocity(rotationalVelocity, PERIOD);
    
    // calculate the values 'desiredSpeedLeft' and 'desiredSpeedRight' using the kinematic model
    
    desiredSpeedLeft = (translationalMotion.velocity-WHEEL_DISTANCE/2.0f*rotationalMotion.velocity)/WHEEL_RADIUS*60.0f/2.0f/PI;
    desiredSpeedRight = -(translationalMotion.velocity+WHEEL_DISTANCE/2.0f*rotationalMotion.velocity)/WHEEL_RADIUS*60.0f/2.0f/PI;
    
    // calculate the actual speed of the motors in [rpm]

    short valueCounterLeft = counterLeft.read();
    short valueCounterRight = counterRight.read();

    short countsInPastPeriodLeft = valueCounterLeft-previousValueCounterLeft;
    short countsInPastPeriodRight = valueCounterRight-previousValueCounterRight;

    previousValueCounterLeft = valueCounterLeft;
    previousValueCounterRight = valueCounterRight;

    actualSpeedLeft = speedLeftFilter.filter((float)countsInPastPeriodLeft/COUNTS_PER_TURN/PERIOD*60.0f);
    actualSpeedRight = speedRightFilter.filter((float)countsInPastPeriodRight/COUNTS_PER_TURN/PERIOD*60.0f);

    // calculate desired motor voltages Uout

    float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+desiredSpeedLeft/KN;
    float voltageRight = KP*(desiredSpeedRight-actualSpeedRight)+desiredSpeedRight/KN;

    // calculate, limit and set the duty-cycle

    float dutyCycleLeft = 0.5f+0.5f*voltageLeft/MAX_VOLTAGE;
    if (dutyCycleLeft < MIN_DUTY_CYCLE) dutyCycleLeft = MIN_DUTY_CYCLE;
    else if (dutyCycleLeft > MAX_DUTY_CYCLE) dutyCycleLeft = MAX_DUTY_CYCLE;
    pwmLeft = dutyCycleLeft;

    float dutyCycleRight = 0.5f+0.5f*voltageRight/MAX_VOLTAGE;
    if (dutyCycleRight < MIN_DUTY_CYCLE) dutyCycleRight = MIN_DUTY_CYCLE;
    else if (dutyCycleRight > MAX_DUTY_CYCLE) dutyCycleRight = MAX_DUTY_CYCLE;
    pwmRight = dutyCycleRight;
    
    // calculate the values 'actualTranslationalVelocity' and 'actualRotationalVelocity' using the kinematic model
    
    actualTranslationalVelocity = (actualSpeedLeft-actualSpeedRight)*2.0f*PI/60.0f*WHEEL_RADIUS/2.0f;
    actualRotationalVelocity = (-actualSpeedRight-actualSpeedLeft)*2.0f*PI/60.0f*WHEEL_RADIUS/WHEEL_DISTANCE;
    
    // calculate the actual robot pose
    
    float deltaTranslation = translationalMotion.velocity*PERIOD; // with a real robot: actualTranslationalVelocity*PERIOD
    float deltaOrientation = rotationalMotion.velocity*PERIOD;    // with a real robot: actualRotationalVelocity*PERIOD
    
    float sinAlpha = sin(alpha+deltaOrientation);
    float cosAlpha = cos(alpha+deltaOrientation);
    
    x += cosAlpha*deltaTranslation;
    y += sinAlpha*deltaTranslation;
    float alpha = this->alpha+deltaOrientation;
    
    while (alpha > PI) alpha -= 2.0f*PI;
    while (alpha < -PI) alpha += 2.0f*PI;
    
    this->alpha = alpha;
}