P-controller geordend
Dependencies: Encoder HIDScope MODSERIAL mbed
main.cpp
- Committer:
- Gerber
- Date:
- 2017-11-02
- Revision:
- 11:126ae33919b3
- Parent:
- 10:d5369a546201
File content as of revision 11:126ae33919b3:
//libaries #include "mbed.h" #include "HIDScope.h" #include "encoder.h" #include "MODSERIAL.h" // globale variables Ticker AInTicker; //We make a ticker named AIn (use for HIDScope) Ticker Treecko; //We make a awesome ticker for our control system AnalogIn potMeter2(A1); //Analoge input of potmeter 2 (will be use for te reference position) PwmOut M1E(D6); //Biorobotics Motor 1 PWM control of the speed DigitalOut M1D(D7); //Biorobotics Motor 1 diraction control Encoder motor1(D13,D12,true); MODSERIAL pc(USBTX,USBRX); float PwmPeriod = 1.0/5000.0; //set up of PWM periode (5000 Hz, want 5000 periodes in 1 seconde) const float Ts = 0.1; // tickettijd/ sample time float e_prev = 0; float e_int = 0; //tweede motor AnalogIn potMeter1(A2); PwmOut M2E(D5); DigitalOut M2D(D4); Encoder motor2(D9,D8,true); Ticker DubbelTreecko; float PwmPeriod2 = 1.0/5000.0; //set up of PWM periode (5000 Hz, want 5000 periodes in 1 seconde) float e_prev2 = 0; float e_int2 = 0; double Potmeterwaarde1; double Potmeterwaarde2; double Potmeterwaarde; double potmeterwaarde2; int maxwaarde, maxwaarde2; float refP, refP2; float kp, Proportional, kd, VelocityError, Derivative, ki, Integrator, motorValue; float kp2, Proportional2, kd2, VelocityError2, Derivative2, ki2, Integrator2, motorValue2; float Huidigepositie; float Aerror; float Huidigepositie2; float Aerror2; float motorVal, motorVal2; double pi = 3.14159265359; double SetPx = 38; //Setpoint position x-coordinate from changePosition (EMG dependent) double SetPy = 30; //Setpoint position y-coordinate from changePosition (EMG dependent) volatile double q1 = 0; //Reference position q1 from calibration (only the first time) volatile double q2 = (pi/2); //Reference position q2 from calibration (only the first time) const double L1 = 30; //Length arm 1 const double L2 = 38; //Length arm 2 double K = 1; //Spring constant for movement end-joint to setpoint double B1 = 1; //Friction coefficient for motor 1 double B2 = 1; //Friction coefficient for motot 2 double T = 0.02; //Desired time step double Motor1Set; //Motor1 angle double Motor2Set; //Motor2 angle double p; double pp; double bb; double cc; double a; double aa; void RKI() { p=sin(q1)*L1; pp=sin(q2)*L2; a=cos(q1)*L1; aa=cos(q2)*L2; bb=SetPy; cc=SetPx; q1 = q1 + ((p + pp)*bb - (a + aa)*cc)*(K*T)/B1; //Calculate desired joint 1 position q2 = q2 + ((bb - a)*pp + (p - cc)*aa)*(K*T)/B2; //Calculate desired joint 2 position int maxwaarde = 4096; // = 64x64 Motor1Set = (q1/(2*pi))*maxwaarde; //Calculate the desired motor1 angle from the desired joint positions Motor2Set = ((pi-q2-q1)/(2*pi))*maxwaarde; //Calculate the desired motor2 angle from the desired joint positions //pc.printf("waarde p = %f, waarde pp = %f, a= %f, aa = %f, bb = %f, cc = %f \r\n",p,pp,a,aa,bb,cc); //pc.printf("q1 = %f, q2 = %f, Motor1Set = %f, Motor2Set = %f \r\n", q1, q2, Motor1Set, Motor2Set); //pc.printf("Setpointx = %f, Setpointy = %f \r\n", SetPx, SetPy); } void SetpointRobot() { Potmeterwaarde2 = potMeter2.read(); Potmeterwaarde1 = potMeter1.read(); if (Potmeterwaarde2>0.6) { SetPx++; // hoe veel verder gaat hij? 1 cm? 10 cm? } if (Potmeterwaarde2<0.4) { SetPx--; } if (Potmeterwaarde1>0.6) { SetPy++; } if (Potmeterwaarde1<0.4) { SetPy--; } //pc.printf("Setpointx = %f, Setpointy = %f \r\n", SetPx, SetPy); } float GetReferencePosition() { Potmeterwaarde = potMeter2.read(); maxwaarde = 4096; // = 64x64 refP = Potmeterwaarde*maxwaarde; return refP; // value between 0 and 4096 } float GetReferencePosition2() { potmeterwaarde2 = potMeter1.read(); maxwaarde2 = 4096; // = 64x64 refP2 = potmeterwaarde2*maxwaarde2; return refP2; // value between 0 and 4096 } float FeedBackControl(float error, float &e_prev, float &e_int) // schaalt de snelheid naar de snelheid zodat onze chip het begrijpt (is nog niet in werking) { kp = 0.001; // kind of scaled. Proportional= kp*error; kd = 0.0004; // kind of scaled. VelocityError = (error - e_prev)/Ts; Derivative = kd*VelocityError; e_prev = error; ki = 0.0005; // kind of scaled. e_int = e_int+Ts*error; Integrator = ki*e_int; motorVal = Proportional + Integrator + Derivative; return motorVal; } float FeedBackControl2(float error2, float &e_prev2, float &e_int2) // schaalt de snelheid naar de snelheid zodat onze chip het begrijpt (is nog niet in werking) { kp2 = 0.001; // kind of scaled. Proportional2= kp2*error2; kd2 = 0.0004; // kind of scaled. VelocityError2 = (error2 - e_prev2)/Ts; Derivative2 = kd2*VelocityError2; e_prev2 = error2; ki2 = 0.0005; // kind of scaled. e_int2 = e_int2+Ts*error2; Integrator2 = ki2*e_int2; motorVal2 = Proportional2 + Integrator2 + Derivative2; return motorVal2; } void SetMotor1(float motorValue) { if (motorValue >= 0) { M1D = 0; } else { M1D = 1; } if (fabs(motorValue) > 1) { M1E = 1; //de snelheid wordt teruggeschaald naar 8.4 rad/s (maximale snelheid, dus waarde 1) } else { M1E = fabs(motorValue); //de absolute snelheid wordt bepaald, de motor staat uit bij een waarde 0 } } void SetMotor2(float motorValue2) { if (motorValue2 >= 0) { M2D = 1; } else { M2D = 0; } if (fabs(motorValue2) > 1) { M2E = 1; //de snelheid wordt teruggeschaald naar 8.4 rad/s (maximale snelheid, dus waarde 1) } else { M2E = fabs(motorValue2); //de absolute snelheid wordt bepaald, de motor staat uit bij een waarde 0 } } float Encoder () { Huidigepositie = motor1.getPosition (); return Huidigepositie; // huidige positie = current position } float Encoder2 () { Huidigepositie2 = motor2.getPosition (); return Huidigepositie2; // huidige positie = current position } void MeasureAndControl(void) { // RKI aanroepen SetpointRobot(); RKI(); // hier the control of the control system //refP = GetReferencePosition(); Huidigepositie = Encoder(); Aerror = (Motor1Set - Huidigepositie);// make an error motorValue = FeedBackControl(Aerror, e_prev, e_int); SetMotor1(motorValue); // hier the control of the control system //refP2 = GetReferencePosition2(); Huidigepositie2 = Encoder2(); Aerror2 = (Motor2Set - Huidigepositie2);// make an error motorValue2 = FeedBackControl2(Aerror2, e_prev2, e_int2); SetMotor2(motorValue2); pc.baud(115200); pc.printf("SetPx = %f, SetPy = %f\r\n potmeter = %f, refP = %f, huidigepositie = %f, error = %f, motorvalue = %f \r\npotmeter2 = %f, refP2 = %f, huidigepositie2 = %f, error2 = %f, motorvalue2 = %f \r\n", SetPx, SetPy, Potmeterwaarde1, Motor1Set, Huidigepositie, Aerror, motorValue, Potmeterwaarde2, Motor2Set, Huidigepositie2, Aerror2, motorValue2); } int main() { M1E.period(PwmPeriod); M2E.period(PwmPeriod2); Treecko.attach(MeasureAndControl, Ts); //Elke 1 seconde zorgt de ticker voor het runnen en uitlezen van de verschillende //functies en analoge signalen. Veranderingen worden elke 1 seconde doorgevoerd. //DubbelTreecko.attach(MeasureAndControl2, Ts); while(1) { wait(0.2); //pc.printf("Potmeter2 = %f, refP = %f, motorValue = %f, \r\nPotmeter1 = %f, refP2 = %f, motorValue2 = %f \r\n", Potmeterwaarde, refP, motorValue, potmeterwaarde2, refP2, motorValue2); //pc.baud(115200); float B = motor1.getPosition(); float Potmeterwaarde = potMeter2.read(); //float positie = B%4096; //pc.printf("pos: %d, speed %f, potmeter = %f V, \r\n",motor1.getPosition(), motor1.getSpeed(),(potMeter2.read()*3.3)); //potmeter uitlezen. tussen 0-1. voltage, dus *3.3V } }