Mechatronics Robotics
/
BrobotV1
These are the core files for the Robot at Team conception.
SpeedControl.cpp
- Committer:
- obrie829
- Date:
- 2017-06-01
- Revision:
- 11:05d5539141c8
- Parent:
- 8:351b0b7b05b2
File content as of revision 11:05d5539141c8:
#include "mbed.h" #include "SpeedControl.h" #include "EncoderCounter.h" #include "LowpassFilter.h" long integralErrorRight=0; long integralErrorLeft=0; short integralErrorMax=1000; //Constructor //directly from mainSpeedControl int main() SpeedControl::SpeedControl(PwmOut* pwmLeft, PwmOut* pwmRight, EncoderCounter* counterLeft, EncoderCounter* counterRight) { this->pwmLeft = pwmLeft; this->pwmRight = pwmRight; // Initialisieren der PWM Ausgaenge pwmLeft.period(0.00005f); // PWM Periode von 50 us this->pwmLeft->period(0.00005f); // Setzt die Periode auf 50 μs this->pwmRight->period(0.00005f); this->pwmLeft->write( 0.5f); // Duty-Cycle von 50% pwmRight.period(0.00005f); // PWM Periode von 50 us this->pwmRight->write( 0.5f); // Duty-Cycle von 50% // Initialisieren von lokalen Variabeln 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; t2.attach( callback(this, &SpeedControl::speedCtrl), 0.1f); // SpeedControl:: allows you to access funtion within the class file } //Destructor SpeedControl::~SpeedControl() { } void SpeedControl::speedCtrl() //EncoderCounter* counterLeft, EncoderCounter* counterRight { //Calculate Effective speeds of the motor [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); //Error of each wheel from Pauls I code short eL = desiredSpeedLeft-actualSpeedLeft; short eR = desiredSpeedRight-actualSpeedRight; /* // Increment the error integral for each wheel integralErrorRight += eR; integralErrorLeft += eL; // Limit the integral term if (integralErrorRight > integralErrorMax) integralErrorRight = integralErrorMax; if (integralErrorRight < -integralErrorMax) integralErrorRight = -integralErrorMax; if (integralErrorLeft > integralErrorMax) integralErrorLeft = integralErrorMax; if (integralErrorLeft < -integralErrorMax) integralErrorLeft = -integralErrorMax; //end of Paul's I control */ // + KI*integralErrorLeft and KI*integralErrorRight + were removed from below // P + I Control! float voltageLeft = KP*eL + desiredSpeedLeft/KN; float voltageRight = KP*eR + desiredSpeedRight/KN; //printf("the voltage on the L and R sides are %f and %f \n\r", voltageLeft, voltageRight); // Berechnen, Limitieren und Setzen der Duty-Cycle float dutyCycleLeft = 0.5f+0.5f*voltageLeft/MAX_VOLTAGE; // Max_voltage is 12.0f if (dutyCycleLeft < MIN_DUTY_CYCLE) dutyCycleLeft = MIN_DUTY_CYCLE; else if (dutyCycleLeft > MAX_DUTY_CYCLE) dutyCycleLeft = MAX_DUTY_CYCLE; *pwmLeft = 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 = dutyCycleRight; // printf("the pwm L and R speeds should be %f and %f \n\r", dutyCycleLeft, dutyCycleRight); } //set desired wheel speed in RPM void SpeedControl::setDesiredSpeed( float L, float R) { desiredSpeedLeft = L; desiredSpeedRight = R; }