P1

Dependencies:   mbed

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers Controller.cpp Source File

Controller.cpp

00001 /*
00002  * Controller.cpp
00003  * Copyright (c) 2020, ZHAW
00004  * All rights reserved.
00005  */
00006 
00007 #include "Controller.h"
00008 
00009 using namespace std;
00010 
00011 const float Controller::PERIOD = 0.001f;                    // period of 1 ms
00012 const float Controller::COUNTS_PER_TURN = 86016.0f;          // encoder resolution (pololu motors: 1200.0f, maxon motors: 86016.0f)
00013 const float Controller::LOWPASS_FILTER_FREQUENCY = 300.0f;  // given in [rad/s]
00014 const float Controller::KN = 45.0f;                         // speed constant in [rpm/V] (pololu motors: 40.0f, maxon motors: 45.0f)
00015 const float Controller::KP = 0.15f;                         // speed control parameter
00016 const float Controller::MAX_VOLTAGE = 12.0f;                // battery voltage in [V]
00017 const float Controller::MIN_DUTY_CYCLE = 0.01f;             // minimum duty-cycle
00018 const float Controller::MAX_DUTY_CYCLE = 0.98f;             // maximum duty-cycle
00019 
00020 /**
00021  * Creates and initialises the robot controller.
00022  * @param pwmLeft a reference to the pwm output for the left motor.
00023  * @param pwmRight a reference to the pwm output for the right motor.
00024  * @param counterLeft a reference to the encoder counter of the left motor.
00025  * @param counterRight a reference to the encoder counter of the right motor.
00026  */
00027 Controller::Controller(PwmOut& pwmLeft, PwmOut& pwmRight, EncoderCounter& counterLeft, EncoderCounter& counterRight) : pwmLeft(pwmLeft), pwmRight(pwmRight), counterLeft(counterLeft), counterRight(counterRight) {
00028     
00029     // initialise pwm outputs
00030 
00031     pwmLeft.period(0.00005f);  // pwm period of 50 us
00032     pwmLeft = 0.5f;            // duty-cycle of 50%
00033 
00034     pwmRight.period(0.00005f); // pwm period of 50 us
00035     pwmRight = 0.5f;           // duty-cycle of 50%
00036 
00037     // initialise local variables
00038 
00039     previousValueCounterLeft = counterLeft.read();
00040     previousValueCounterRight = counterRight.read();
00041 
00042     speedLeftFilter.setPeriod(PERIOD);
00043     speedLeftFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
00044 
00045     speedRightFilter.setPeriod(PERIOD);
00046     speedRightFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
00047 
00048     desiredSpeedLeft = 0.0f;
00049     desiredSpeedRight = 0.0f;
00050 
00051     actualSpeedLeft = 0.0f;
00052     actualSpeedRight = 0.0f;
00053 
00054     // start the periodic task
00055 
00056     ticker.attach(callback(this, &Controller::run), PERIOD);
00057 }
00058 
00059 /**
00060  * Deletes this Controller object.
00061  */
00062 Controller::~Controller() {
00063     
00064     ticker.detach(); // stop the periodic task
00065 }
00066 
00067 /**
00068  * Sets the desired speed of the left motor.
00069  * @param desiredSpeedLeft desired speed given in [rpm].
00070  */
00071 void Controller::setDesiredSpeedLeft(float desiredSpeedLeft) {
00072 
00073     this->desiredSpeedLeft = desiredSpeedLeft;
00074 }
00075 
00076 /**
00077  * Sets the desired speed of the right motor.
00078  * @param desiredSpeedRight desired speed given in [rpm].
00079  */
00080 void Controller::setDesiredSpeedRight(float desiredSpeedRight) {
00081 
00082     this->desiredSpeedRight = desiredSpeedRight;
00083 }
00084 
00085 /**
00086  * This is an internal method of the controller that is running periodically.
00087  */
00088 void Controller::run() {
00089 
00090     // calculate the actual speed of the motors in [rpm]
00091 
00092     short valueCounterLeft = counterLeft.read();
00093     short valueCounterRight = counterRight.read();
00094 
00095     short countsInPastPeriodLeft = valueCounterLeft-previousValueCounterLeft;
00096     short countsInPastPeriodRight = valueCounterRight-previousValueCounterRight;
00097 
00098     previousValueCounterLeft = valueCounterLeft;
00099     previousValueCounterRight = valueCounterRight;
00100 
00101     actualSpeedLeft = speedLeftFilter.filter((float)countsInPastPeriodLeft/COUNTS_PER_TURN/PERIOD*60.0f);
00102     actualSpeedRight = speedRightFilter.filter((float)countsInPastPeriodRight/COUNTS_PER_TURN/PERIOD*60.0f);
00103 
00104     // calculate desired motor voltages Uout
00105 
00106     // bitte implementieren!
00107     
00108     float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+desiredSpeedLeft/KN;
00109     //printf("voltageLeft:%f",voltageLeft);
00110     float voltageRight = (KP*(desiredSpeedRight-actualSpeedRight)+desiredSpeedRight/KN);
00111     //printf("voltageRigth:%f",voltageRight);
00112     // calculate, limit and set the duty-cycle
00113 
00114     float dutyCycleLeft = 0.5f+0.5f*voltageLeft/MAX_VOLTAGE;
00115     if (dutyCycleLeft < MIN_DUTY_CYCLE) dutyCycleLeft = MIN_DUTY_CYCLE;
00116     else if (dutyCycleLeft > MAX_DUTY_CYCLE) dutyCycleLeft = MAX_DUTY_CYCLE;
00117     pwmLeft = dutyCycleLeft;
00118 
00119     float dutyCycleRight = 0.5f+0.5f*voltageRight/MAX_VOLTAGE;
00120     if (dutyCycleRight < MIN_DUTY_CYCLE) dutyCycleRight = MIN_DUTY_CYCLE;
00121     else if (dutyCycleRight > MAX_DUTY_CYCLE) dutyCycleRight = MAX_DUTY_CYCLE;
00122     pwmRight = dutyCycleRight;
00123 }
00124