Diff: main.cpp

Revision:

0:335646ab45c0

Child:

1:a76fd17e18b3

diff -r 000000000000 -r 335646ab45c0 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Oct 28 15:50:03 2019 +0000
@@ -0,0 +1,298 @@
+#include "mbed.h"
+#include "HIDScope.h"
+#include "QEI.h"
+#include "MODSERIAL.h"
+#include "BiQuad.h"
+#include "FastPWM.h"
+
+// Button and potmeter1 control
+InterruptIn button1(D11);
+InterruptIn button2(D10);
+InterruptIn buttonsw2(SW2);
+InterruptIn buttonsw3(SW3);
+AnalogIn potmeter1(A0);
+AnalogIn potmeter2(A1);
+AnalogIn potmeter3(A2);
+AnalogIn potmeter4(A3);
+// Encoder
+DigitalIn encA1(D9);
+DigitalIn encB1(D8);
+DigitalIn encA2(D13);
+DigitalIn encB2(D13);
+QEI encoder1(D9,D8,NC,64,QEI::X4_ENCODING);     //Encoding motor 1
+QEI encoder2(D13,D12,NC,64,QEI::X4_ENCODING);   //Encoding motor 2
+float Ts = 0.01;                                //Sample time
+float motor1angle;                              //Measured angle motor 1
+float motor2angle;                              //Measured angle motor 2
+float omega1;                                   //velocity rad/s motor 1
+float omega2;                                   //Velocity rad/s motor2
+float deg2rad=0.0174532;                        //Conversion factor degree to rad
+float rad2deg=57.29578;                         //Conversion factor rad to degree
+
+
+// Motor
+DigitalOut motor2Direction(D4);
+FastPWM motor2Power(D5);
+DigitalOut motor1Direction(D7);
+FastPWM motor1Power(D6);
+
+volatile int motortoggle = 1;                   //Toggle to stop motors
+
+//Motorcontrol
+bool motordir1;
+bool motordir2;
+float motor1ref=0.1745;
+float motor2ref=0.0873;
+double controlsignal1;
+double controlsignal2;
+double pi2= 6.283185;
+float motor1error;                              //motor 1 error
+float motor2error;
+float Kp=0.27;
+float Ki=0.35;
+float Kd=0.1;
+float u_p1;
+float u_p2;
+float u_i1;
+float u_i2;
+
+//Windup control
+float ux1;
+float ux2;
+float up1;                                      //Proportional contribution motor 1
+float up2;                                      //Proportional contribution motor 2
+float ek1;                                          
+float ek2;
+float ei1= 0.0;                                 //Error integral motor 1
+float ei2=0.0;                                  //Error integral motor 2
+float Ka= 1.0;                                  //Integral windup gain
+
+//RKI 
+float Vx=0.0;                                   //Desired linear velocity x direction
+float Vy=0.0;                                   //Desired linear velocity y direction
+float q1=0.0f*deg2rad;                          //Angle of first joint [rad]
+float q2=-135.0f*deg2rad;                       //Angle of second joint [rad]
+float q1dot;                                    //Velocity of first joint [rad/s]
+float q2dot;                                    //Velocity of second joint [rad/s]
+float l1=26.0;                                  //Distance base-link [cm]
+float l2=62.0;                                  //Distance link-endpoint [cm]
+float xe;                                       //Endpoint x position [cm]
+float ye;                                       //Endpoint y position [cm]
+
+//Hidscope
+HIDScope scope(6);                              //Going to send x channels of data. To access data go to 'http:/localhost:18082/' after starting HIDScope application.
+
+// PC connection
+MODSERIAL pc(USBTX, USBRX);
+
+// Intializing tickers
+Ticker motorTicker;
+Ticker controlTicker;
+Ticker directionTicker;
+Ticker encoderTicker;
+Ticker scopeTicker;
+
+const float PWM_period = 1e-6;
+
+volatile int counts1; // Encoder counts
+volatile int counts2;
+volatile int countsPrev1 = 0;
+volatile int countsPrev2 = 0;
+volatile int deltaCounts1;
+volatile int deltaCounts2;
+
+float factorin = 6.23185/64; // Convert encoder counts to angle in rad
+float gearratio = 131.25; // Gear ratio of gearbox
+
+
+float PID_controller1(float motor1error){
+    static float error_integral1=0;
+    static float e_prev1=motor1error;
+    
+    //Proportional part:
+    u_p1=Kp*motor1error;
+    
+    //Integral part
+    error_integral1=error_integral1+ei1*Ts;
+    u_i1=Ki*error_integral1;
+    
+    //Derivative part
+    float error_derivative1=(motor1error-e_prev1)/Ts;
+    float u_d1=Kd*error_derivative1;
+    e_prev1=motor1error;
+    
+    // Sum and limit
+    up1= u_p1+u_i1+u_d1;
+    if (up1>1){
+        controlsignal1=1;}
+    else if (up1<-1){
+        controlsignal1=-1;}
+    else {
+        controlsignal1=up1;}
+    
+    // To prevent windup
+    ux1= up1-controlsignal1;
+    ek1= Ka*ux1;
+    ei1= motor1error-ek1;
+     //Return
+     return controlsignal1; 
+}
+
+float PID_controller2(float motor2error){
+    static float error_integral2=0;
+    static float e_prev2=motor2error;
+    
+    //Proportional part:
+    u_p2=Kp*motor2error;
+    
+    //Integral part
+    error_integral2=error_integral2+ei2*Ts;
+    u_i2=Ki*error_integral2;
+    
+    //Derivative part
+    float error_derivative2=(motor2error-e_prev2)/Ts;
+    float u_d2=Kd*error_derivative2;
+    e_prev2=motor2error;
+    
+    // Sum and limit
+    up2= u_p2+u_i2+u_d2;
+    if (up2>1){
+        controlsignal2=1;}
+    else if (up2<-1){
+        controlsignal2=-1;}
+    else {
+        controlsignal2=up2;}
+    
+    // To prevent windup
+    ux2= up2-controlsignal2;
+    ek2= Ka*ux2;
+    ei2= motor2error-ek2;
+     //Return
+     return controlsignal2; 
+}
+
+
+void readEncoder()
+{
+    counts1 = encoder1.getPulses();        
+    deltaCounts1 = counts1 - countsPrev1;
+    countsPrev1 = counts1;
+    
+    counts2 = encoder2.getPulses();
+    deltaCounts2 = counts2 - countsPrev2;
+    countsPrev2 = counts2;    
+}
+
+void togglehoek(){
+  // static float t=0;
+  // Vy=6.0f*sin(50.0f*t);
+  // t+=0.01f;
+   }
+   
+void RKI(){
+  
+    //Vy=potmeter1.read()*10.0*motortoggle;
+    //Vy=potmeter2.read()*10*motortoggle;
+    static float t=0;
+    Vx=6.0f*sin(1.0f*t);
+    t+=0.01f;
+    //Vx=-1.0*;
+    Vy=0.0f;
+    q1dot=(l2*cos(q1+q2)*Vx+l2*sin(q1+q2)*Vy)/((-l1*sin(q1)-l2*sin(q1+q2))*l2*cos(q1+q2)+l2*sin(q1+q2)*(l1*cos(q1)+l2*cos(q1+q2)));
+    q2dot=((-l1*cos(q1)-l2*cos(q1+q2))*Vx+(-l1*sin(q1)-l2*sin(q1+q2))*Vy)/((-l1*sin(q1)-l2*sin(q1+q2))*l2*cos(q1+q2)+l2*sin(q1+q2)*(l1*cos(q1)+l2*cos(q1+q2)));
+    q1=q1+q1dot*Ts;
+    q2=q2+q2dot*Ts;  
+    
+    xe=l1*cos(q1)+l2*cos(q1+q2);
+    ye=l1*sin(q1)+l2*sin(q1+q2);
+    
+    
+    
+    if (q1<0.0f){
+        q1=0.0;}
+    else if (q1>90.0f*deg2rad){
+        q1=90.0f*deg2rad;}    
+    else{
+        q1=q1;}
+    
+    if (q2>-45.0*deg2rad){
+        q2=-45.0*deg2rad;}
+    else if (q2<-135.0*deg2rad){
+        q2=-135.0*deg2rad;}
+    else{
+        q2=q2;}
+    
+    motor1ref=5.5f*q1+5.5f*q2;
+    motor2ref=2.75f*q1;
+    }    
+    
+void motorControl()
+{
+    button1.fall(&togglehoek);
+    RKI();
+    motor1angle = (counts1 * factorin / gearratio)-(135.0*5.5*deg2rad); // Angle of motor shaft in rad
+    omega1 = deltaCounts1 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
+    motor1error=motor1ref-motor1angle;    
+    controlsignal1=PID_controller1(motor1error);      
+    if (controlsignal1<0){
+        motordir1= 0;}
+    else {
+         motordir1= 1;}
+    motor1Power.write(abs(controlsignal1*motortoggle));
+    motor1Direction= motordir1;
+    
+    motor2angle = (counts2 * factorin / gearratio); // Angle of motor shaft in rad
+    omega2 = deltaCounts2 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
+    motor2error=motor2ref-motor2angle;    
+    controlsignal2=PID_controller2(motor2error);      
+    if (controlsignal2<0){
+        motordir2= 0;}
+    else {
+         motordir2= 1;}
+    motor2Power.write(abs(controlsignal2*motortoggle));
+    motor2Direction= motordir2;    
+}
+
+void Plotje(){
+    scope.set(0,q1*rad2deg); //gewenste hoek
+    scope.set(1,motor1angle/5.5f*rad2deg); //Gemeten hoek
+    scope.set(2,motor1error/5.5f*rad2deg); //verschil in gewenste en gemeten hoek
+    scope.set(3,q2*rad2deg); //gewenste hoek
+    scope.set(4,motor2angle/5.5f*rad2deg); //Gemeten hoek
+    scope.set(5,motor2error/5.5f*rad2deg); //verschil in gewenste en gemeten hoek
+    
+    scope.send(); //send what's in scope memory to PC
+}
+
+void toggleMotor()
+{
+    motortoggle = !motortoggle;
+}
+
+int main()
+{
+    pc.baud(115200);
+    pc.printf("\r\nStarting...\r\n\r\n");
+    
+    motor1Power.period(PWM_period);
+    motor2Power.period(PWM_period);
+    motorTicker.attach(motorControl, 0.01);
+    scopeTicker.attach(Plotje, 0.01);
+    encoderTicker.attach(readEncoder, Ts);
+    
+    button2.fall(&toggleMotor);
+    
+    while (true) {
+//        pc.printf("Angle1:  %f   Omega1:       %f\r\n", motor1angle, omega1);
+//        pc.printf("refangle1: %f   Error1:  %f     \r\n",motor1ref, motor1error);
+//        pc.printf("Angle2:  %f   Omega2:       %f\r\n", motor2angle, omega2);
+//        pc.printf("refangle2: %f   Error2:  %f     \r\n",motor2ref, motor2error);
+//        pc.printf("controlsignal2: %d \r\n", controlsignal2);
+//        pc.printf("Vx: %f Vy: %f \r\n",Vx,Vy);
+//        pc.printf("q1dot: %f q2dot: %f \r\n",q1dot,q2dot);
+            pc.printf("q1: %f q1dot: %f motor1ref: %f motor1angle: %f\r\n", q1*rad2deg, q1dot*rad2deg, motor1ref*rad2deg, motor1angle*rad2deg);
+            pc.printf("q2: %f q2dot: %f motor2ref: %f motor2angle: %f\r\n", q2*rad2deg, q2dot*rad2deg, motor2ref*rad2deg, motor2angle*rad2deg);
+            pc.printf("X: %f Y: %f\r\n",xe,ye);
+        wait(0.5);
+    }
+}