Controller.cpp

00001 /*
00002  * Controller.cpp
00003  * Copyright (c) 2018, ZHAW
00004  * All rights reserved.
00005  */
00006 
00007 #include <cmath>
00008 #include "Controller.h"
00009 
00010 using namespace std;
00011 
00012 const float Controller::PERIOD = 0.001f;                    // period of control task, given in [s]
00013 const float Controller::COUNTS_PER_TURN = 1200.0f;          // resolution of encoder counter
00014 const float Controller::LOWPASS_FILTER_FREQUENCY = 300.0f;  // frequency of lowpass filter for actual speed values, given in [rad/s]
00015 const float Controller::KN = 40.0f;                         // speed constant of motor, given in [rpm/V]
00016 const float Controller::KP = 0.2f;                          // speed controller gain, given in [V/rpm]
00017 const float Controller::MAX_VOLTAGE = 12.0f;                // supply voltage for power stage in [V]
00018 const float Controller::MIN_DUTY_CYCLE = 0.02f;             // minimum allowed value for duty cycle (2%)
00019 const float Controller::MAX_DUTY_CYCLE = 0.98f;             // maximum allowed value for duty cycle (98%)
00020 
00021 const float Controller::R = 0.17f/2.0f;                       // Abstand Antriebsraeder zu Mitte
00022 const float Controller::r = 0.0375f;                        // Radius der Antriebsraeder
00023 
00024 /**
00025  * Creates and initializes a Controller object.
00026  * @param pwmLeft a pwm output object to set the duty cycle for the left motor.
00027  * @param pwmRight a pwm output object to set the duty cycle for the right motor.
00028  * @param counterLeft an encoder counter object to read the position of the left motor.
00029  * @param counterRight an encoder counter object to read the position of the right motor.
00030  */
00031 Controller::Controller(PwmOut& pwmLeft, PwmOut& pwmRight, EncoderCounter& counterLeft, EncoderCounter& counterRight) : pwmLeft(pwmLeft), pwmRight(pwmRight), counterLeft(counterLeft), counterRight(counterRight) {
00032     
00033     // initialize periphery drivers
00034     
00035     pwmLeft.period(0.00005f);
00036     pwmLeft.write(0.5f);
00037     
00038     pwmRight.period(0.00005f);
00039     pwmRight.write(0.5f);
00040     
00041     // initialize local variables
00042     
00043     translationalMotion.setProfileVelocity(2.0f);
00044     translationalMotion.setProfileAcceleration(4.0f);
00045     translationalMotion.setProfileDeceleration(3.0f);
00046     
00047     rotationalMotion.setProfileVelocity(2.0f);
00048     rotationalMotion.setProfileAcceleration(8.0f);
00049     rotationalMotion.setProfileDeceleration(3.0f);
00050     
00051     translationalVelocity = 0.0f;
00052     rotationalVelocity = 0.0f;
00053     actualTranslationalVelocity = 0.0f;
00054     actualRotationalVelocity = 0.0f;
00055     
00056     previousValueCounterLeft = counterLeft.read();
00057     previousValueCounterRight = counterRight.read();
00058     
00059     speedLeftFilter.setPeriod(PERIOD);
00060     speedLeftFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
00061     
00062     speedRightFilter.setPeriod(PERIOD);
00063     speedRightFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
00064     
00065     desiredSpeedLeft = 0.0f;
00066     desiredSpeedRight = 0.0f;
00067     
00068     actualSpeedLeft = 0.0f;
00069     actualSpeedRight = 0.0f;
00070     
00071     // start periodic task
00072     
00073     ticker.attach(callback(this, &Controller::run), PERIOD);
00074 }
00075 
00076 /**
00077  * Deletes the Controller object and releases all allocated resources.
00078  */
00079 Controller::~Controller() {
00080     
00081     ticker.detach();
00082 }
00083 
00084 /**
00085  * Sets the desired translational velocity of the robot.
00086  * @param velocity the desired translational velocity, given in [m/s].
00087  */
00088 void Controller::setTranslationalVelocity(float velocity) {
00089     
00090     this->translationalVelocity = velocity;
00091 }
00092 
00093 /**
00094  * Sets the desired rotational velocity of the robot.
00095  * @param velocity the desired rotational velocity, given in [rad/s].
00096  */
00097 void Controller::setRotationalVelocity(float velocity) {
00098     
00099     this->rotationalVelocity = velocity;
00100 }
00101 
00102 /**
00103  * Gets the actual translational velocity of the robot.
00104  * @return the actual translational velocity, given in [m/s].
00105  */
00106 float Controller::getActualTranslationalVelocity() {
00107     
00108     return actualTranslationalVelocity;
00109 }
00110 
00111 /**
00112  * Gets the actual rotational velocity of the robot.
00113  * @return the actual rotational velocity, given in [rad/s].
00114  */
00115 float Controller::getActualRotationalVelocity() {
00116     
00117     return actualRotationalVelocity;
00118 }
00119 
00120 /**
00121  * This method is called periodically by the ticker object and contains the
00122  * algorithm of the speed controller.
00123  */
00124 void Controller::run() {
00125     
00126     // calculate the planned velocities using the motion planner
00127     
00128     translationalMotion.incrementToVelocity(translationalVelocity,PERIOD);
00129     rotationalMotion.incrementToVelocity(rotationalVelocity,PERIOD);
00130     
00131     
00132     // calculate the values 'desiredSpeedLeft' and 'desiredSpeedRight' using the kinematic model
00133     
00134     desiredSpeedLeft = 60.0f*(translationalMotion.getVelocity() - R*rotationalMotion.getVelocity())/(2.0f*3.14159f*r);
00135     // rad rechts muss invertiert werden für VORWAERTS
00136     desiredSpeedRight = 60.0f*(-1*(translationalMotion.getVelocity() + R*rotationalMotion.getVelocity()))/(2.0f*3.14159f*r);   
00137     
00138     // calculate actual speed of motors in [rpm]
00139     
00140     short valueCounterLeft = counterLeft.read();
00141     short valueCounterRight = counterRight.read();
00142     
00143     short countsInPastPeriodLeft = valueCounterLeft-previousValueCounterLeft;
00144     short countsInPastPeriodRight = valueCounterRight-previousValueCounterRight;
00145     
00146     previousValueCounterLeft = valueCounterLeft;
00147     previousValueCounterRight = valueCounterRight;
00148     
00149     actualSpeedLeft = speedLeftFilter.filter((float)countsInPastPeriodLeft/COUNTS_PER_TURN/PERIOD*60.0f);
00150     actualSpeedRight = speedRightFilter.filter((float)countsInPastPeriodRight/COUNTS_PER_TURN/PERIOD*60.0f);
00151     
00152     // calculate motor phase voltages
00153     
00154     float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+desiredSpeedLeft/KN;
00155     float voltageRight = KP*(desiredSpeedRight-actualSpeedRight)+desiredSpeedRight/KN;
00156     
00157     // calculate, limit and set duty cycles
00158     
00159     float dutyCycleLeft = 0.5f+0.5f*voltageLeft/MAX_VOLTAGE;
00160     if (dutyCycleLeft < MIN_DUTY_CYCLE) dutyCycleLeft = MIN_DUTY_CYCLE;
00161     else if (dutyCycleLeft > MAX_DUTY_CYCLE) dutyCycleLeft = MAX_DUTY_CYCLE;
00162     pwmLeft.write(dutyCycleLeft);
00163     
00164     float dutyCycleRight = 0.5f+0.5f*voltageRight/MAX_VOLTAGE;
00165     if (dutyCycleRight < MIN_DUTY_CYCLE) dutyCycleRight = MIN_DUTY_CYCLE;
00166     else if (dutyCycleRight > MAX_DUTY_CYCLE) dutyCycleRight = MAX_DUTY_CYCLE;
00167     pwmRight.write(dutyCycleRight);
00168     
00169     // calculate the values 'actualTranslationalVelocity' and 'actualRotationalVelocity' using the kinematic model
00170     
00171     //actualTranslationalVelocity = 0.5f * r *(actualSpeedLeft+actualSpeedRight);
00172     //actualRotationalVelocity = 0.5f /R*r*(actualSpeedLeft+actualSpeedRight);
00173     
00174 }
00175