alles ingevoegd
Dependencies: FastPWM HIDScope MODSERIAL QEI mbed biquadFilter
Fork of deelPID by
main.cpp
- Committer:
- laurencebr
- Date:
- 2018-10-30
- Revision:
- 0:2aa29a0824df
- Child:
- 1:23834862b877
File content as of revision 0:2aa29a0824df:
#include "mbed.h" #include "FastPWM.h" #include "MODSERIAL.h" #include "QEI.h" #include "HIDScope.h" DigitalOut gpo(D0); DigitalOut led(LED_RED); AnalogIn pot1(A0); AnalogIn pot2(A1); AnalogIn pot3(A2); QEI encoder1(D10, D11, NC, 32); QEI encoder2(D12, D13, NC, 32); FastPWM Motor1PWM(D6); DigitalOut Motor1Direction(D7); FastPWM Motor2PWM(D5); //!!!!! Juiste poorten aangeven DigitalOut Motor2Direction(D4); //!!!!! Juiste poort aangeven MODSERIAL pc(USBTX, USBRX); //HIDSCOPE HIDScope scope(4); Ticker scopeTimer; ///Variables double q1Error; double q2Error; double PrevErrorq1[100]; double PrevErrorq2[100]; int n = 100; // Zelfde waarde als PrevErrorarray double q1Ref = 1.0; //VOOR TESTEN double q2Ref; //Double xPos; //Double yPos; double xRef; double yRef; double q1Pos; double q2Pos; double PIDerrorq1; double PIDerrorq2; double IntegralErrorq1 = 0.0; double IntegralErrorq2 = 0.0; double DerivativeErrorq1 = 0.0; double DerivativeErrorq2 = 0.0; double GainP1 = 2.0; double GainI1 = 1.2; double GainD1 = 0.0; double GainP2 = 2.0; double GainI2 = 2.0; double GainD2 = 2.0; double TickerPeriod = 1.0/400.0; Ticker Kikker; int count = 0; int countstep = 0; void UpdateRefPosition() { // Update, (based on EMG or pots) the reference position xRef, yRef //q1Ref = pot1.read()*2*3.1416; //q2Ref = pot2.read()*2*3.1416; } void InverseKinematics() { // Convert, using the inverse kinematics relations, xRef and yRef to q1Ref and q2Ref // (So, update the values of q1Ref and q2Ref) } void ReadCurrentPosition() //Update the coordinates of the end-effector q1Pos, q2Pos { q1Pos = encoder1.getPulses()/32/131.25*2*3.1416; //Position motor 1 in rad q2Pos = encoder2.getPulses()/32/131.25*2*3.1416; //Position motor 2 in rad } void UpdateError() //Update the q1Error and q2Error values based on q1Ref, q2Ref, q1Pos, q2Pos { q1Error = q1Ref - q1Pos; q2Error = q2Ref - q2Pos; //Update PrevErrorq1 and PrevErrorq2 for (int i = 0; i <= n-2 ; i++) { PrevErrorq1[i] = PrevErrorq1[i+1]; PrevErrorq2[i] = PrevErrorq2[i+1]; } PrevErrorq1[n-1] = q1Error; PrevErrorq2[n-1] = q2Error; } void PIDControl(){ // Update PIDerrorq1 and PIDerrorq2 based on the gains and the values q1Error and q2Error // PID control motor 1 //P-Control double P_error1 = GainP1 * q1Error; //I-Control IntegralErrorq1 = IntegralErrorq1 + q1Error * TickerPeriod; double I_error1 = GainI1 * IntegralErrorq1; //D-Control //First smoothen the error signal double MovAvq1_1 = 0.0; double MovAvq1_2 = 0.0; for (int i=0; i<=n-2; i++){ // Creates two mov. av. with one element shifted MovAvq1_1 = MovAvq1_1 + PrevErrorq1[i]/((double) (n-1)); MovAvq1_2 = MovAvq1_2 + PrevErrorq1[i+1]/((double)(n-1)); } DerivativeErrorq1 = (MovAvq1_2 - MovAvq1_1)/TickerPeriod; double D_error1 = GainD1 * DerivativeErrorq1; // Derivative error sum over all time? PIDerrorq1 = P_error1 + I_error1 + D_error1; // PID control motor 2 //P-Control double P_error2 = GainP2 * q2Error; //I-Control IntegralErrorq2 = IntegralErrorq2 + q2Error * TickerPeriod; double I_error2 = GainI2 * IntegralErrorq2; //D-Control //First smoothen the error signal double MovAvq2_1 = 0.0; double MovAvq2_2 = 0.0; for (int i=0; i<=n-2; i++){ // Creates two mov. av. with one element shifted MovAvq2_1 = MovAvq2_1 + PrevErrorq2[i]/((double)(n-1)); MovAvq2_2 = MovAvq2_2 + PrevErrorq2[i+1]/((double)(n-1)); } DerivativeErrorq2 = (MovAvq2_2 - MovAvq2_1)/TickerPeriod; double D_error2 = GainD2 * DerivativeErrorq2; // Derivative error sum over all time? PIDerrorq2 = P_error2 + I_error2 + D_error2; } void MotorControl() { //Motor 1 //Keep signal within bounds if (PIDerrorq1 > 1.0){ PIDerrorq1 = 1.0; } else if (PIDerrorq1 < -1.0){ PIDerrorq1 = -1.0; } //Direction if (PIDerrorq1 <= 0){ Motor1Direction = 0; Motor1PWM.write(-PIDerrorq1); //write Duty cycle } if (PIDerrorq1 >= 0){ Motor1Direction = 1; Motor1PWM.write(PIDerrorq1); //write Duty cycle } //Motor 2 //Keep signal within bounds if (PIDerrorq2 > 1.0){ PIDerrorq2 = 1.0; } else if (PIDerrorq2 < -1.0){ PIDerrorq2 = -1.0; } //Direction if (PIDerrorq2 <= 0){ Motor2Direction = 0; Motor2PWM.write(-PIDerrorq2); //write Duty cycle } if (PIDerrorq2 >= 0){ Motor2Direction = 1; Motor2PWM.write(PIDerrorq2); //write Duty cycle } } void NormalOperatingModus() { UpdateRefPosition(); InverseKinematics(); ReadCurrentPosition(); UpdateError(); PIDControl(); MotorControl(); scope.set(0, q1Pos); scope.set(1, q1Ref); GainP1 = pot3.read() * 10; GainI1 = pot2.read() * 10; GainD1 = pot1.read() ; countstep++; count++; if (count == 400) // print 1x per seconde waardes. { pc.printf("GainP1 = %1f, GainI1 = %1f, GainD1 = %1f, q1Pos = %lf, q1Ref = %1f \n\r", GainP1, GainI1, GainD1, q1Pos, q1Ref); count = 0; } if (countstep == 4000) { q1Ref = !q1Ref; countstep = 0; } } int main() { pc.baud(115200); //Initialize array errors to 0 for (int i = 0; i <= 9; i++){ PrevErrorq2[i] = 0; PrevErrorq2[i] = 0; } int frequency_pwm = 16700; //16.7 kHz PWM Motor1PWM.period(1.0/frequency_pwm); // T = 1/f Motor2PWM.period(1.0/frequency_pwm); // T = 1/f Kikker.attach(NormalOperatingModus, TickerPeriod); scopeTimer.attach_us(&scope, &HIDScope::send, 2e4); while(true); {} }