working PID + Kinematics + Motorcontrol
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Diff: main.cpp
- Revision:
- 0:a6f2b6cc83ca
- Child:
- 1:4f1cd3b918f5
- Child:
- 2:44758d95cb0b
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Tue Oct 30 16:04:17 2018 +0000 @@ -0,0 +1,274 @@ +// KINEMATICS + PID + MOTOR CONTROL + +//----------------~INITIATING------------------------- +#include "mbed.h" + +// KINEMATICS -- DEPENDENCIES +#include "stdio.h" +#define _USE_MATH_DEFINES +#include <math.h> +#define M_PI 3.14159265358979323846 /* pi */ + +// PID CONTROLLER -- DEPENDENCIES +#include "BiQuad.h" +#include "QEI.h" +#include "MODSERIAL.h" +#include "HIDScope.h" +//#include "Math.h" + +// PID CONTROLLER -- PIN DEFENITIONS +AnalogIn button2(A4); +DigitalOut directionpin1(D7); // motor 1 +PwmOut pwmpin1(D6); // motor 1 + +QEI encoder1 (D9, D8, NC, 8400, QEI::X4_ENCODING); +MODSERIAL pc(USBTX, USBRX); +HIDScope scope(2); + +// TICKERS +Ticker ref_rot; +Ticker show_counts; +Ticker Scope_Data; + +//----------------GLOBALS-------------------------- + +// CONSTANTS PID CONTROLLER +double PI = M_PI;// CHANGE THIS INTO M_PI +double Kp = 16; //200 , 50 +double Ki = 0; //1, 0.5 +double Kd = 4.5; //200, 10 +double Ts = 0.1; // Sample time in seconds +double reference_rotation; //define as radians +double motor_position; +bool AlwaysTrue; + +//CONSTANTS KINEMATICS +// constants +const float la = 0.256; // lengte actieve arm +const float lp = 0.21; // lengte passieve arm +const float rp = 0.052; // straal van midden end effector tot hoekpunt +const float rm = 0.23; // straal van global midden tot motor +const float a = 0.09; // zijde van de driehoek +const float xas = 0.40; // afstand van motor 1 tot motor 3 +const float yas = 0.346; // afstand van xas tot motor 2 +const float thetap = 0; // rotatiehoek van de end effector + +// motor locatie +const int a1x = 0; //x locatie motor 1 +const int a1y = 0; //y locatie motor 1 +const float a2x = (0.5)*xas; // x locatie motor 2 +const float a2y = yas; // y locatie motor 2 +const float a3x = xas; // x locatie motor 3 +const int a3y = 0; // y locatie motor 3 + +// script voor het bepalen van de desired position aan de hand van emg (1/0) + +// EMG OUTPUT +int EMGxplus; +int EMGxmin ; +int EMGyplus; +int EMGymin ; + +// Dit moet experimenteel geperfectioneerd worden +float tijdstap = 0.05; //nu wss heel langzaam, kan miss omhoog +float v = 0.1; // snelheid kan wss ook hoger + +float px = 0.2; //starting x +float py = 0.155; // starting y + +// limits (since no forward kinematics) +float upperxlim = 0.36; //niet helemaal naar requierments ff kijken of ie groter kan +float lowerxlim = 0.04; +float upperylim = 0.30; +float lowerylim = 0.04; + + +//----------------FUNCTIONS-------------------------- + +// ~~~~~~~~~~~~~~ROBOT KINEMATICS ~~~~~~~~~~~~~~~~~~ + +// functie x positie +float positionx(int EMGxplus,int EMGxmin) +{ + float EMGx = EMGxplus - EMGxmin; + + float verplaatsingx = EMGx * tijdstap * v; + float pxnieuw = px + verplaatsingx; + // x limit + if (pxnieuw <= upperxlim && pxnieuw >= lowerxlim) + { + px = pxnieuw; + } + else + { + if (pxnieuw >= lowerxlim) + { + px = upperxlim; + } + else + { + px = lowerxlim; + } + } +//printf("X eindpunt (%f) en verplaatsing: (%f)\n\r",px,verplaatsingx); +return px; +} + + +// functie y positie +float positiony(int EMGyplus,int EMGymin) +{ + float EMGy = EMGyplus - EMGymin; + + float verplaatsingy = EMGy * tijdstap * v; + float pynieuw = py + verplaatsingy; + + // y limit + if (pynieuw <= upperylim && pynieuw >= lowerylim) + { + py = pynieuw; + } + else + { + if (pynieuw >= lowerylim) + { + py = upperylim; + } + else + { + py = lowerylim; + } + } +//printf("Y eindpunt (%f) en verplaatsing: (%f) \n\r",py,verplaatsingy); +return (py); +} + + +//~~~~~~~~~~~~CALCULATIING MOTOR ANGLES ~~~~~~~~ +// arm 1 --> reference angle motor 1 +float hoek1(float px, float py) // input: ref x, ref y +{ + float c1x = px - rp * cos(thetap +(M_PI/6)); // x locatie hoekpunt end-effector + float c1y = py - rp*sin(thetap+(M_PI/6)); // y locatie hoekpunt end-effector + float alpha1 = atan2((c1y-a1y),(c1x-a1x)); // hoek tussen horizontaal en lijn van motor naar bijbehorende end-effector punt + float psi1 = acos(( pow(la,2)-pow(lp,2)+pow((c1x-a1x),2)+pow((c1y-a1y),2))/(2*la*sqrt(pow ((c1x-a1x),2)+pow((c1y-a1y),2) ))); //Hoek tussen lijn van motor naar bijbehorende end=effector punt en actieve arm + float a1 = alpha1 + psi1; //hoek tussen horizontaal en actieve arm + //printf("arm 1 = %f \n\r",a1); + return a1; +} + +// arm 2 --> reference angle motor 2 +float hoek2(float px, float py) +{ + float c2x = px - rp * cos(thetap -(M_PI/2)); + float c2y = py - rp*sin(thetap-(M_PI/2)); + float alpha2 = atan2((c2y-a2y),(c2x-a2x)); + float psi2 = acos(( pow(la,2)-pow(lp,2)+pow((c2x-a2x),2)+pow((c2y-a2y),2))/(2*la*sqrt(pow ((c2x-a2x),2)+pow((c2y-a2y),2) ))); + float a2 = alpha2 + psi2; + //printf("arm 2 = %f \n\r",a2); + return a2; +} + +//arm 3 --> reference angle motor 3 +float hoek3(float px, float py) +{ + float c3x = px - rp * cos(thetap +(5*M_PI/6)); + float c3y = py - rp*sin(thetap+(5*M_PI/6)); + float alpha3 = atan2((c3y-a3y),(c3x-a3x)); + float psi3 = acos(( pow(la,2)-pow(lp,2)+pow((c3x-a3x),2)+pow((c3y-a3y),2))/(2*la*sqrt(pow ((c3x-a3x),2)+pow((c3y-a3y),2) ))); + float a3 = alpha3 + psi3; + //printf("arm 3 = %f \n\r",a3); + return a3; +} + +// ~~~~~~~~~~~~~~PID CONTROLLER~~~~~~~~~~~~~~~~~~ + +double PID_controller(double error) +{ + static double error_integral = 0; + static double error_prev = error; // initialization with this value only done once! + static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); + + // Proportional part: + double u_k = Kp * error; + + // Integral part + error_integral = error_integral + error * Ts; + double u_i = Ki * error_integral; + + // Derivative part + double error_derivative = (error - error_prev)/Ts; + double filtered_error_derivative = LowPassFilter.step(error_derivative); + double u_d = Kd * filtered_error_derivative; + error_prev = error; + + // Sum all parts and return it + return u_k + u_i + u_d; +} + + +// DIRECTON AND SPEED CONTROL +void moter_control(double u) +{ + directionpin1= u > 0.0f; //eithertrueor false + pwmpin1= fabs(u); //pwmduty cycle canonlybepositive, floatingpoint absolute value +} + + +// CONTROLLING THE MOTOR +void Motor_mover() +{ + double motor_position = encoder1.getPulses(); //output in counts + double reference_rotation = hoek3(px, py); + double error = reference_rotation - motor_position*(2*PI)/8400; + double u = PID_controller(error); + moter_control(u); +} + +//PRINT TICKER +void PrintFlag() +{ + AlwaysTrue = true; +} + +// HIDSCOPE +void ScopeData() +{ + double y = encoder1.getPulses(); + scope.set(0, y); + scope.send(); +} + + +//----------------------MAIN--------------------------------- +int main() +{ + // ~~~~~~~~~~~~~~~~ INITIATING ~~~~~~~~~~~~ + pwmpin1.period_us(60); // setup motor + + // setup printing service + pc.baud(9600); + pc.printf("test"); + + // Tickers + //show_counts.attach(PrintFlag, 0.2); + ref_rot.attach(Motor_mover, 0.01); + Scope_Data.attach(ScopeData, 0.01); + + +// berekenen positie + float px = positionx(1,0); // EMG: +x, -x + float py = positiony(1,0); // EMG: +y, -y + //printf("positie (%f,%f)\n\r",px,py); + +// berekenen hoeken +/* +float a1 = hoek1(px, py); +float a2 = hoek2(px, py); +float a3 = hoek3(px, py); + +printf("hoek(%f,%f,%f)\n\r",a1,a2,a3); + + return 0; +*/ +}