Diff: Controller.cpp

Revision:

0:bb408887ab78

Child:

1:f38485404dbe

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Controller.cpp	Fri Mar 09 15:29:36 2018 +0000
@@ -0,0 +1,175 @@
+/*
+ * 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::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::R = 0.17f/2.0f;                       // Abstand Antriebsraeder zu Mitte
+const float Controller::r = 0.0375f;                        // Radius der Antriebsraeder
+
+/**
+ * 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(3.0f);
+    translationalMotion.setProfileAcceleration(1.0f);
+    translationalMotion.setProfileDeceleration(1.0f);
+    
+    rotationalMotion.setProfileVelocity(2.0f);
+    rotationalMotion.setProfileAcceleration(2.0f);
+    rotationalMotion.setProfileDeceleration(2.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;
+    
+    // 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;
+}
+
+/**
+ * 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 = 60.0f*(translationalMotion.getVelocity() - R*rotationalMotion.getVelocity())/(2.0f*3.14159f*r);
+    // rad rechts muss invertiert werden für VORWAERTS
+    desiredSpeedRight = 60.0f*(-1*(translationalMotion.getVelocity() + R*rotationalMotion.getVelocity()))/(2.0f*3.14159f*r);   
+    
+    // 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 = 0.5f * r *(actualSpeedLeft+actualSpeedRight);
+    //actualRotationalVelocity = 0.5f /R*r*(actualSpeedLeft+actualSpeedRight);
+    
+}
+