Nim leo niiiim
Diff: Controller.cpp
- Revision:
- 0:da791f233257
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Controller.cpp Fri May 11 12:21:19 2018 +0000 @@ -0,0 +1,243 @@ +/* + * Controller.cpp + * Copyright (c) 2018, ZHAW + * All rights reserved. + */ + +#include <cmath> +#include "Controller.h" + +using namespace std; + +const float Controller::PERIOD = 0.001f; // period of control task, given in [s] +const float Controller::PI = 3.14159265f; // the constant PI +const float Controller::WHEEL_DISTANCE = 0.170f; // distance between wheels, given in [m] +const float Controller::WHEEL_RADIUS = 0.0375f; // radius of wheels, given in [m] +const float Controller::COUNTS_PER_TURN = 1200.0f; // resolution of encoder counter +const float Controller::LOWPASS_FILTER_FREQUENCY = 300.0f; // frequency of lowpass filter for actual speed values, given in [rad/s] +const float Controller::KN = 40.0f; // speed constant of motor, given in [rpm/V] +const float Controller::KP = 0.2f; // speed controller gain, given in [V/rpm] +const float Controller::MAX_VOLTAGE = 12.0f; // supply voltage for power stage in [V] +const float Controller::MIN_DUTY_CYCLE = 0.02f; // minimum allowed value for duty cycle (2%) +const float Controller::MAX_DUTY_CYCLE = 0.98f; // maximum allowed value for duty cycle (98%) + +/** + * Creates and initializes a Controller object. + * @param pwmLeft a pwm output object to set the duty cycle for the left motor. + * @param pwmRight a pwm output object to set the duty cycle for the right motor. + * @param counterLeft an encoder counter object to read the position of the left motor. + * @param counterRight an encoder counter object to read the position of the right motor. + */ +Controller::Controller(PwmOut& pwmLeft, PwmOut& pwmRight, EncoderCounter& counterLeft, EncoderCounter& counterRight) : pwmLeft(pwmLeft), pwmRight(pwmRight), counterLeft(counterLeft), counterRight(counterRight) { + + // initialize periphery drivers + + pwmLeft.period(0.00005f); + pwmLeft.write(0.5f); + + pwmRight.period(0.00005f); + pwmRight.write(0.5f); + + // initialize local variables + + translationalMotion.setProfileVelocity(1.5f); + translationalMotion.setProfileAcceleration(1.5f); + translationalMotion.setProfileDeceleration(3.0f); + + rotationalMotion.setProfileVelocity(3.0f); + rotationalMotion.setProfileAcceleration(15.0f); + rotationalMotion.setProfileDeceleration(15.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 periodic task + + ticker.attach(callback(this, &Controller::run), PERIOD); +} + +/** + * Deletes the Controller object and releases all allocated resources. + */ +Controller::~Controller() { + + ticker.detach(); +} + +/** + * 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 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 method is called periodically by the ticker object and contains the + * algorithm of the speed controller. + */ +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 actual speed of 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 motor phase voltages + + float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+desiredSpeedLeft/KN; + float voltageRight = KP*(desiredSpeedRight-actualSpeedRight)+desiredSpeedRight/KN; + + // calculate, limit and set duty cycles + + 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.write(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.write(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 = actualTranslationalVelocity*PERIOD; + float deltaOrientation = actualRotationalVelocity*PERIOD; + + x += cos(alpha+deltaOrientation)*deltaTranslation; + y += sin(alpha+deltaOrientation)*deltaTranslation; + float alpha = this->alpha+deltaOrientation; + + while (alpha > PI) alpha -= 2.0f*PI; + while (alpha < -PI) alpha += 2.0f*PI; + + this->alpha = alpha; +}