Allan Brignoli
gugus
Controller.cpp
- Committer:
- Brignall
- Date:
- 2018-05-18
- Revision:
- 0:1a0321f1ffbc
File content as of revision 0:1a0321f1ffbc:
/*
* 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%)
const float Controller::SIGMA_TRANSLATION = 0.0001; // standard deviation of estimated translation per period, given in [m]
const float Controller::SIGMA_ORIENTATION = 0.0002; // standard deviation of estimated orientation per period, given in [rad]
const float Controller::SIGMA_DISTANCE = 0.01; // standard deviation of distance measurement, given in [m]
const float Controller::SIGMA_GAMMA = 0.03; // standard deviation of angle measurement, given in [rad]
/**
* 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;
p[0][0] = 0.001f;
p[0][1] = 0.0f;
p[0][2] = 0.0f;
p[1][0] = 0.0f;
p[1][1] = 0.001f;
p[1][2] = 0.0f;
p[2][0] = 0.0f;
p[2][1] = 0.0f;
p[2][2] = 0.001f;
// 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;
}
/**
* Correct the pose with given actual and measured coordinates of a beacon.
* @param xActual the actual x coordinate of the beacon, given in [m].
* @param yActual the actual y coordinate of the beacon, given in [m].
* @param xMeasured the x coordinate of the beacon measured with a sensor(i.e. a laser scanner), given in [m].
* @param yMeasured the y coordinate of the beacon measured with a sensor(i.e. a laser scanner), given in [m].
*/
void Controller::correctPoseWithBeacon(float xActual, float yActual, float xMeasured, float yMeasured) {
// create copies of current state and covariance matrix for Kalman filter P
float x = this->x;
float y = this->y;
float alpha = this->alpha;
float p[3][3];
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
p[i][j] = this->p[i][j];
}
}
// calculate covariance matrix of innovation S
float s[2][2];
float r = sqrt((xActual-x)*(xActual-x)+(yActual-y)*(yActual-y));
s[0][0] = 1.0f/r/r*(p[1][0]*xActual*yActual+p[1][1]*yActual*yActual+r*r*SIGMA_DISTANCE*SIGMA_DISTANCE+p[0][0]*(xActual-x)*(xActual-x)-p[1][0]*yActual*x+p[0][1]*(xActual-x)*(yActual-y)-p[1][0]*xActual*y-2.0f*p[1][1]*yActual*y+p[1][0]*x*y+p[1][1]*y*y);
s[0][1] = -(1.0f/r/r/r*(-p[1][1]*xActual*yActual+p[1][0]*yActual*yActual-p[0][2]*xActual*r*r-p[1][2]*yActual*r*r-p[0][1]*(xActual-x)*(xActual-x)+p[1][1]*yActual*x+p[0][2]*r*r*x+p[0][0]*(xActual-x)*(yActual-y)+p[1][1]*xActual*y-2.0f*p[1][0]*yActual*y+p[1][2]*r*r*y-p[1][1]*x*y+p[1][0]*y*y));
s[1][0] = ((xActual-x)*(p[2][0]*r*r+p[1][0]*(xActual-x)+p[0][0]*(-yActual+y))+(yActual-y)*(p[2][1]*r*r+p[1][1]*(xActual-x)+p[0][1]*(-yActual+y)))/r/r/r;
s[1][1] = p[2][2]+SIGMA_GAMMA*SIGMA_GAMMA+p[1][2]*(xActual-x)/r/r+p[0][2]*(-yActual+y)/r/r-(yActual-y)*(p[2][0]*r*r+p[1][0]*(xActual-x)+p[0][0]*(-yActual+y))/r/r/r/r+(xActual-x)*(p[2][1]*r*r+p[1][1]*(xActual-x)+p[0][1]*(-yActual+y))/r/r/r/r;
// calculate Kalman matrix K
float k[3][2];
k[0][0] = -((s[1][0]*(-p[0][2]+(p[0][1]*(-xActual+x))/r/r+(p[0][0]*(yActual-y))/r/r))/(-(s[0][1]*s[1][0])+s[0][0]*s[1][1]))+(s[1][1]*((p[0][0]*(-xActual+x))/r+(p[0][1]*(-yActual+y))/r))/(-(s[0][1]*s[1][0])+s[0][0]*s[1][1]);
k[0][1] = (s[0][0]*(-p[0][2]+(p[0][1]*(-xActual+x))/r/r+(p[0][0]*(yActual-y))/r/r))/(-(s[0][1]*s[1][0])+s[0][0]*s[1][1])-(s[0][1]*((p[0][0]*(-xActual+x))/r+(p[0][1]*(-yActual+y))/r))/(-(s[0][1]*s[1][0])+s[0][0]*s[1][1]);
k[1][0] = -((s[1][0]*(-p[1][2]+(p[1][1]*(-xActual+x))/r/r+(p[1][0]*(yActual-y))/r/r))/(-(s[0][1]*s[1][0])+s[0][0]*s[1][1]))+(s[1][1]*((p[1][0]*(-xActual+x))/r+(p[1][1]*(-yActual+y))/r))/(-(s[0][1]*s[1][0])+s[0][0]*s[1][1]);
k[1][1] = (s[0][0]*(-p[1][2]+(p[1][1]*(-xActual+x))/r/r+(p[1][0]*(yActual-y))/r/r))/(-(s[0][1]*s[1][0])+s[0][0]*s[1][1])-(s[0][1]*((p[1][0]*(-xActual+x))/r+(p[1][1]*(-yActual+y))/r))/(-(s[0][1]*s[1][0])+s[0][0]*s[1][1]);
k[2][0] = -((s[1][0]*(-p[2][2]+(p[2][1]*(-xActual+x))/r/r+(p[2][0]*(yActual-y))/r/r))/(-(s[0][1]*s[1][0])+s[0][0]*s[1][1]))+(s[1][1]*((p[2][0]*(-xActual+x))/r+(p[2][1]*(-yActual+y))/r))/(-(s[0][1]*s[1][0])+s[0][0]*s[1][1]);
k[2][1] = (s[0][0]*(-p[2][2]+(p[2][1]*(-xActual+x))/r/r+(p[2][0]*(yActual-y))/r/r))/(-(s[0][1]*s[1][0])+s[0][0]*s[1][1])-(s[0][1]*((p[2][0]*(-xActual+x))/r+(p[2][1]*(-yActual+y))/r))/(-(s[0][1]*s[1][0])+s[0][0]*s[1][1]);
// calculate pose correction
float distanceMeasured = sqrt((xMeasured-x)*(xMeasured-x)+(yMeasured-y)*(yMeasured-y));
float gammaMeasured = atan2(yMeasured-y, xMeasured-x)-alpha;
if (gammaMeasured > PI) gammaMeasured -= 2.0f*PI;
else if (gammaMeasured < -PI) gammaMeasured += 2.0f*PI;
float distanceEstimated = sqrt((xActual-x)*(xActual-x)+(yActual-y)*(yActual-y));
float gammaEstimated = atan2(yActual-y, xActual-x)-alpha;
if (gammaEstimated > PI) gammaEstimated -= 2.0f*PI;
else if (gammaEstimated < -PI) gammaEstimated += 2.0f*PI;
x += k[0][0]*(distanceMeasured-distanceEstimated)+k[0][1]*(gammaMeasured-gammaEstimated);
y += k[1][0]*(distanceMeasured-distanceEstimated)+k[1][1]*(gammaMeasured-gammaEstimated);
alpha += k[2][0]*(distanceMeasured-distanceEstimated)+k[2][1]*(gammaMeasured-gammaEstimated);
this->x = x;
this->y = y;
this->alpha = alpha;
// calculate correction of covariance matrix for Kalman filter P
p[0][0] = k[0][1]*p[2][0]+p[0][0]*(1-(k[0][0]*(-xActual+x))/r-(k[0][1]*(yActual-y))/r/r)+p[1][0]*(-((k[0][1]*(-xActual+x))/r/r)-(k[0][0]*(-yActual+y))/r);
p[0][1] = k[0][1]*p[2][1]+p[0][1]*(1-(k[0][0]*(-xActual+x))/r-(k[0][1]*(yActual-y))/r/r)+p[1][1]*(-((k[0][1]*(-xActual+x))/r/r)-(k[0][0]*(-yActual+y))/r);
p[0][2] = k[0][1]*p[2][2]+p[0][2]*(1-(k[0][0]*(-xActual+x))/r-(k[0][1]*(yActual-y))/r/r)+p[1][2]*(-((k[0][1]*(-xActual+x))/r/r)-(k[0][0]*(-yActual+y))/r);
p[1][0] = k[1][1]*p[2][0]+p[0][0]*(-((k[1][0]*(-xActual+x))/r)-(k[1][1]*(yActual-y))/r/r)+p[1][0]*(1-(k[1][1]*(-xActual+x))/r/r-(k[1][0]*(-yActual+y))/r);
p[1][1] = k[1][1]*p[2][1]+p[0][1]*(-((k[1][0]*(-xActual+x))/r)-(k[1][1]*(yActual-y))/r/r)+p[1][1]*(1-(k[1][1]*(-xActual+x))/r/r-(k[1][0]*(-yActual+y))/r);
p[1][2] = k[1][1]*p[2][2]+p[0][2]*(-((k[1][0]*(-xActual+x))/r)-(k[1][1]*(yActual-y))/r/r)+p[1][2]*(1-(k[1][1]*(-xActual+x))/r/r-(k[1][0]*(-yActual+y))/r);
p[2][0] = (1+k[2][1])*p[2][0]+p[0][0]*(-((k[2][0]*(-xActual+x))/r)-(k[2][1]*(yActual-y))/r/r)+p[1][0]*(-((k[2][1]*(-xActual+x))/r/r)-(k[2][0]*(-yActual+y))/r);
p[2][1] = (1+k[2][1])*p[2][1]+p[0][1]*(-((k[2][0]*(-xActual+x))/r)-(k[2][1]*(yActual-y))/r/r)+p[1][1]*(-((k[2][1]*(-xActual+x))/r/r)-(k[2][0]*(-yActual+y))/r);
p[2][2] = (1+k[2][1])*p[2][2]+p[0][2]*(-((k[2][0]*(-xActual+x))/r)-(k[2][1]*(yActual-y))/r/r)+p[1][2]*(-((k[2][1]*(-xActual+x))/r/r)-(k[2][0]*(-yActual+y))/r);
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
this->p[i][j] = p[i][j];
}
}
}
/**
* 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;
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;
// calculate covariance matrix for Kalman filter P
p[0][0] = p[0][0]+SIGMA_TRANSLATION*SIGMA_TRANSLATION*cosAlpha*cosAlpha+deltaTranslation*deltaTranslation*(SIGMA_ORIENTATION*SIGMA_ORIENTATION+p[2][2])*sinAlpha*sinAlpha-deltaTranslation*(p[0][2]+p[2][0])*sinAlpha;
p[0][1] = p[0][1]-deltaTranslation*p[2][1]*sinAlpha+cosAlpha*(deltaTranslation*p[0][2]+(SIGMA_TRANSLATION*SIGMA_TRANSLATION-deltaTranslation*deltaTranslation*(SIGMA_ORIENTATION*SIGMA_ORIENTATION+p[2][2]))*sinAlpha);
p[0][2] = p[0][2]-deltaTranslation*(SIGMA_ORIENTATION*SIGMA_ORIENTATION+p[2][2])*sinAlpha;
p[1][0] = p[1][0]-deltaTranslation*p[1][2]*sinAlpha+cosAlpha*(deltaTranslation*p[2][0]+(SIGMA_TRANSLATION*SIGMA_TRANSLATION-deltaTranslation*deltaTranslation*(SIGMA_ORIENTATION*SIGMA_ORIENTATION+p[2][2]))*sinAlpha);
p[1][1] = p[1][1]+deltaTranslation*deltaTranslation*(SIGMA_ORIENTATION*SIGMA_ORIENTATION+p[2][2])*cosAlpha*cosAlpha+deltaTranslation*(p[1][2]+p[2][1])*cosAlpha+SIGMA_TRANSLATION*SIGMA_TRANSLATION*sinAlpha*sinAlpha;
p[1][2] = p[1][2]+deltaTranslation*(SIGMA_ORIENTATION*SIGMA_ORIENTATION+p[2][2])*cosAlpha;
p[2][0] = p[2][0]-deltaTranslation*(SIGMA_ORIENTATION*SIGMA_ORIENTATION+p[2][2])*sinAlpha;
p[2][1] = p[2][1]+deltaTranslation*(SIGMA_ORIENTATION*SIGMA_ORIENTATION+p[2][2])*cosAlpha;
p[2][2] = p[2][2]+SIGMA_ORIENTATION*SIGMA_ORIENTATION;
}