Mechatronics Robotics
/
BrobotV1
These are the core files for the Robot at Team conception.
SpeedControl.cpp@11:05d5539141c8, 2017-06-01 (annotated)
- Committer:
- obrie829
- Date:
- Thu Jun 01 08:34:27 2017 +0000
- Revision:
- 11:05d5539141c8
- Parent:
- 8:351b0b7b05b2
v10
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
obrie829 | 8:351b0b7b05b2 | 1 | |
obrie829 | 8:351b0b7b05b2 | 2 | #include "mbed.h" |
obrie829 | 8:351b0b7b05b2 | 3 | #include "SpeedControl.h" |
obrie829 | 8:351b0b7b05b2 | 4 | #include "EncoderCounter.h" |
obrie829 | 8:351b0b7b05b2 | 5 | #include "LowpassFilter.h" |
obrie829 | 8:351b0b7b05b2 | 6 | |
obrie829 | 11:05d5539141c8 | 7 | long integralErrorRight=0; |
obrie829 | 11:05d5539141c8 | 8 | long integralErrorLeft=0; |
obrie829 | 11:05d5539141c8 | 9 | short integralErrorMax=1000; |
obrie829 | 11:05d5539141c8 | 10 | |
obrie829 | 11:05d5539141c8 | 11 | |
obrie829 | 11:05d5539141c8 | 12 | //Constructor |
obrie829 | 8:351b0b7b05b2 | 13 | //directly from mainSpeedControl int main() |
obrie829 | 8:351b0b7b05b2 | 14 | SpeedControl::SpeedControl(PwmOut* pwmLeft, PwmOut* pwmRight, EncoderCounter* counterLeft, EncoderCounter* counterRight) |
obrie829 | 8:351b0b7b05b2 | 15 | { |
obrie829 | 8:351b0b7b05b2 | 16 | this->pwmLeft = pwmLeft; |
obrie829 | 8:351b0b7b05b2 | 17 | this->pwmRight = pwmRight; |
obrie829 | 8:351b0b7b05b2 | 18 | |
obrie829 | 11:05d5539141c8 | 19 | // Initialisieren der PWM Ausgaenge pwmLeft.period(0.00005f); // PWM Periode von 50 us |
obrie829 | 8:351b0b7b05b2 | 20 | this->pwmLeft->period(0.00005f); // Setzt die Periode auf 50 μs |
obrie829 | 8:351b0b7b05b2 | 21 | this->pwmRight->period(0.00005f); |
obrie829 | 8:351b0b7b05b2 | 22 | this->pwmLeft->write( 0.5f); // Duty-Cycle von 50% pwmRight.period(0.00005f); // PWM Periode von 50 us |
obrie829 | 8:351b0b7b05b2 | 23 | this->pwmRight->write( 0.5f); // Duty-Cycle von 50% |
obrie829 | 8:351b0b7b05b2 | 24 | |
obrie829 | 8:351b0b7b05b2 | 25 | // Initialisieren von lokalen Variabeln |
obrie829 | 8:351b0b7b05b2 | 26 | previousValueCounterLeft = counterLeft->read(); |
obrie829 | 8:351b0b7b05b2 | 27 | previousValueCounterRight = counterRight->read(); |
obrie829 | 8:351b0b7b05b2 | 28 | speedLeftFilter.setPeriod(PERIOD); |
obrie829 | 8:351b0b7b05b2 | 29 | speedLeftFilter.setFrequency(LOWPASS_FILTER_FREQUENCY); |
obrie829 | 8:351b0b7b05b2 | 30 | speedRightFilter.setPeriod(PERIOD); |
obrie829 | 8:351b0b7b05b2 | 31 | speedRightFilter.setFrequency(LOWPASS_FILTER_FREQUENCY); |
obrie829 | 8:351b0b7b05b2 | 32 | |
obrie829 | 8:351b0b7b05b2 | 33 | desiredSpeedLeft = 0.0f; |
obrie829 | 8:351b0b7b05b2 | 34 | desiredSpeedRight = 0.0f; |
obrie829 | 8:351b0b7b05b2 | 35 | actualSpeedLeft = 0.0f; |
obrie829 | 8:351b0b7b05b2 | 36 | actualSpeedRight = 0.0f; |
obrie829 | 8:351b0b7b05b2 | 37 | |
obrie829 | 11:05d5539141c8 | 38 | t2.attach( callback(this, &SpeedControl::speedCtrl), 0.1f); // SpeedControl:: allows you to access funtion within the class file |
obrie829 | 8:351b0b7b05b2 | 39 | |
obrie829 | 8:351b0b7b05b2 | 40 | } |
obrie829 | 8:351b0b7b05b2 | 41 | |
obrie829 | 8:351b0b7b05b2 | 42 | //Destructor |
obrie829 | 8:351b0b7b05b2 | 43 | SpeedControl::~SpeedControl() |
obrie829 | 8:351b0b7b05b2 | 44 | { |
obrie829 | 8:351b0b7b05b2 | 45 | } |
obrie829 | 8:351b0b7b05b2 | 46 | |
obrie829 | 8:351b0b7b05b2 | 47 | void SpeedControl::speedCtrl() //EncoderCounter* counterLeft, EncoderCounter* counterRight |
obrie829 | 8:351b0b7b05b2 | 48 | { |
obrie829 | 11:05d5539141c8 | 49 | //Calculate Effective speeds of the motor [rpm] |
obrie829 | 8:351b0b7b05b2 | 50 | short valueCounterLeft = counterLeft->read(); |
obrie829 | 8:351b0b7b05b2 | 51 | short valueCounterRight = counterRight->read(); |
obrie829 | 8:351b0b7b05b2 | 52 | short countsInPastPeriodLeft = valueCounterLeft-previousValueCounterLeft; |
obrie829 | 8:351b0b7b05b2 | 53 | short countsInPastPeriodRight = valueCounterRight-previousValueCounterRight; |
obrie829 | 8:351b0b7b05b2 | 54 | |
obrie829 | 8:351b0b7b05b2 | 55 | previousValueCounterLeft = valueCounterLeft; |
obrie829 | 8:351b0b7b05b2 | 56 | previousValueCounterRight = valueCounterRight; |
obrie829 | 8:351b0b7b05b2 | 57 | actualSpeedLeft = speedLeftFilter.filter((float)countsInPastPeriodLeft /COUNTS_PER_TURN/PERIOD*60.0f); |
obrie829 | 8:351b0b7b05b2 | 58 | actualSpeedRight = speedRightFilter.filter((float)countsInPastPeriodRight /COUNTS_PER_TURN/PERIOD*60.0f); |
obrie829 | 8:351b0b7b05b2 | 59 | |
obrie829 | 11:05d5539141c8 | 60 | |
obrie829 | 8:351b0b7b05b2 | 61 | //Error of each wheel from Pauls I code |
obrie829 | 11:05d5539141c8 | 62 | |
obrie829 | 8:351b0b7b05b2 | 63 | short eL = desiredSpeedLeft-actualSpeedLeft; |
obrie829 | 8:351b0b7b05b2 | 64 | short eR = desiredSpeedRight-actualSpeedRight; |
obrie829 | 8:351b0b7b05b2 | 65 | |
obrie829 | 11:05d5539141c8 | 66 | /* |
obrie829 | 8:351b0b7b05b2 | 67 | // Increment the error integral for each wheel |
obrie829 | 8:351b0b7b05b2 | 68 | integralErrorRight += eR; |
obrie829 | 8:351b0b7b05b2 | 69 | integralErrorLeft += eL; |
obrie829 | 8:351b0b7b05b2 | 70 | // Limit the integral term |
obrie829 | 8:351b0b7b05b2 | 71 | if (integralErrorRight > integralErrorMax) |
obrie829 | 8:351b0b7b05b2 | 72 | integralErrorRight = integralErrorMax; |
obrie829 | 8:351b0b7b05b2 | 73 | if (integralErrorRight < -integralErrorMax) |
obrie829 | 8:351b0b7b05b2 | 74 | integralErrorRight = -integralErrorMax; |
obrie829 | 8:351b0b7b05b2 | 75 | if (integralErrorLeft > integralErrorMax) |
obrie829 | 8:351b0b7b05b2 | 76 | integralErrorLeft = integralErrorMax; |
obrie829 | 8:351b0b7b05b2 | 77 | if (integralErrorLeft < -integralErrorMax) |
obrie829 | 8:351b0b7b05b2 | 78 | integralErrorLeft = -integralErrorMax; |
obrie829 | 8:351b0b7b05b2 | 79 | //end of Paul's I control |
obrie829 | 11:05d5539141c8 | 80 | */ |
obrie829 | 11:05d5539141c8 | 81 | // + KI*integralErrorLeft and KI*integralErrorRight + were removed from below |
obrie829 | 8:351b0b7b05b2 | 82 | // P + I Control! |
obrie829 | 11:05d5539141c8 | 83 | float voltageLeft = KP*eL + desiredSpeedLeft/KN; |
obrie829 | 11:05d5539141c8 | 84 | float voltageRight = KP*eR + desiredSpeedRight/KN; |
obrie829 | 11:05d5539141c8 | 85 | |
obrie829 | 11:05d5539141c8 | 86 | //printf("the voltage on the L and R sides are %f and %f \n\r", voltageLeft, voltageRight); |
obrie829 | 11:05d5539141c8 | 87 | |
obrie829 | 8:351b0b7b05b2 | 88 | // Berechnen, Limitieren und Setzen der Duty-Cycle |
obrie829 | 11:05d5539141c8 | 89 | float dutyCycleLeft = 0.5f+0.5f*voltageLeft/MAX_VOLTAGE; // Max_voltage is 12.0f |
obrie829 | 8:351b0b7b05b2 | 90 | if (dutyCycleLeft < MIN_DUTY_CYCLE) dutyCycleLeft = MIN_DUTY_CYCLE; |
obrie829 | 8:351b0b7b05b2 | 91 | else if (dutyCycleLeft > MAX_DUTY_CYCLE) dutyCycleLeft = MAX_DUTY_CYCLE; |
obrie829 | 8:351b0b7b05b2 | 92 | |
obrie829 | 8:351b0b7b05b2 | 93 | *pwmLeft = dutyCycleLeft; |
obrie829 | 8:351b0b7b05b2 | 94 | |
obrie829 | 8:351b0b7b05b2 | 95 | float dutyCycleRight = 0.5f+0.5f*voltageRight/MAX_VOLTAGE; |
obrie829 | 8:351b0b7b05b2 | 96 | if (dutyCycleRight < MIN_DUTY_CYCLE) dutyCycleRight = MIN_DUTY_CYCLE; |
obrie829 | 8:351b0b7b05b2 | 97 | else if (dutyCycleRight > MAX_DUTY_CYCLE) dutyCycleRight = MAX_DUTY_CYCLE; |
obrie829 | 8:351b0b7b05b2 | 98 | |
obrie829 | 8:351b0b7b05b2 | 99 | *pwmRight = dutyCycleRight; |
obrie829 | 11:05d5539141c8 | 100 | |
obrie829 | 11:05d5539141c8 | 101 | // printf("the pwm L and R speeds should be %f and %f \n\r", dutyCycleLeft, dutyCycleRight); |
obrie829 | 11:05d5539141c8 | 102 | |
obrie829 | 8:351b0b7b05b2 | 103 | } |
obrie829 | 8:351b0b7b05b2 | 104 | |
obrie829 | 11:05d5539141c8 | 105 | //set desired wheel speed in RPM |
obrie829 | 8:351b0b7b05b2 | 106 | void SpeedControl::setDesiredSpeed( float L, float R) |
obrie829 | 8:351b0b7b05b2 | 107 | { |
obrie829 | 8:351b0b7b05b2 | 108 | desiredSpeedLeft = L; |
obrie829 | 8:351b0b7b05b2 | 109 | desiredSpeedRight = R; |
obrie829 | 8:351b0b7b05b2 | 110 | } |
obrie829 | 8:351b0b7b05b2 | 111 |