P1
Embed:
(wiki syntax)
Show/hide line numbers
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
Generated on Wed Jul 20 2022 10:34:32 by
1.7.2