Allan Brignoli
gugus
Diff: Controller.cpp
- Revision:
- 0:1a0321f1ffbc
diff -r 000000000000 -r 1a0321f1ffbc Controller.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Controller.cpp Fri May 18 12:18:21 2018 +0000
@@ -0,0 +1,362 @@
+/*
+ * 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;
+}
+