Diff: main.cpp

Revision:

0:d02f4c7e8906

Child:

1:ffa6f4d78c8e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Oct 26 08:45:24 2016 +0000
@@ -0,0 +1,140 @@
+#include "mbed.h"
+#include "FastPWM.h"
+#include "MODSERIAL.h"
+#include "QEI.h"
+#include "HIDScope.h"  //make sure hidscope cable is also attached
+#include "BiQuad.h"
+
+// in gebruik: D(0,1,4,5,6,7,10,11,12,13) 
+
+DigitalOut motor1dir(D7); //direction of motor 1, attach at m1, set to 0: cw
+FastPWM motor1(D6);     // speed of motor 1
+FastPWM motor2(D5);     //speed of motor 2
+DigitalOut motor2dir(D4);   //direction of motor 2, attach at m2, set to 0: ccw 
+
+Ticker control; //Ticker for processing encoder input
+volatile bool control_go=false;
+
+HIDScope scope(3); // aantal scopes in hidscope opzetten
+
+  
+double m1_pwm=0;    //variable for PWM control motor 1
+double m2_pwm=0;    //variable for PWM control motor 2
+
+const double m1_Kp = 2.5, m1_Ki = 1.0, m1_Kd = 1.0, m1_N = 100; // controller constants motor 1
+double m1_v1 = 0, m1_v2 = 0; // Memory variables
+const double m1_Ts = 0.01; // Controller sample time
+
+const double m2_Kp = 2.5, m2_Ki = 1.0, m2_Kd = 1.0, m2_N = 100; // controller constants motor 2
+double m2_v1 = 0, m2_v2 = 0; // Memory variables
+const double m2_Ts = 0.01; // Controller sample time
+
+const double pi=3.14159265359;
+const double res = 64/(1/131.25*2*pi);     // resolution on gearbox shaft per pulse
+const double V_max=9.0;                    // maximum voltage supplied by trafo
+const double minRadius=0.3;                // minimum radius of arm
+const double maxRadius=0.6;                // maximum radius of arm
+const double pulleyDiameter=0.0398;        // pulley diameter
+const double minAngle=-1.25;               // minimum angle for limiting controller
+
+QEI Encoder1(D13,D12,NC,64,QEI::X4_ENCODING); //defining encoder
+QEI Encoder2(D11,D10,NC,64,QEI::X4_ENCODING); //defining encoder
+
+
+
+MODSERIAL pc(USBTX,USBRX);
+
+void activate_controller(){controller_go=true}; //activate go flag
+
+double PID( double err, const double Kp, const double Ki, const double Kd,
+const double Ts, const double N, double &v1, double &v2 ) {
+    const double a1 =-4/(N*Ts+2),
+        a2=-(N*Ts-2)/(N*Ts+2),
+        b0=(4*Kp + 4*Kd*N + 2*Ki*Ts+2*Kp*N*Ts+Ki*N*pow(Ts,2))/(2*N*Ts+4),
+        b1=(Ki*N*pow(Ts,2)-4*Kp-4*Kd*N)/(N*Ts+2),
+        b2=(4*Kp+4*Kd*N-2*Ki*Ts-2*Kp*N*Ts+Ki*N*pow(Ts,2))/(2*N*Ts+4);
+
+    double v=err-a1*v1-a2*v2;
+    double u=b0*v+b1*v1+b2*v2
+    v2=v1; v1=v;
+    return u;
+}
+
+ 
+ void controller(){ //function for executing controller action
+        // convert ref to gearbox angle
+        double theta=atan(refy/refx);
+        double radius=sqrt(pow(refx,2)+pow(refy,2));
+        
+        //limit theta
+        if (theta =< minAngle){
+            theta=minAngle
+        }
+        else if (theta => 0){
+            theta=0;
+        }
+         
+          //limit radius
+        if (radius =< minRadius){
+            radius=minRadius
+        }
+        else if (radius > maxRadius){
+            radius=maxRadius;
+        } 
+        
+        //converting radius and theta to gearbox angle    
+        ref_angle1=16*theta;    
+        ref_angle2=(-radius+minRadius)/pi/pulleyDiameter;
+        
+                
+        angle1 = Encoder1.getPulses()/res;   //get number of pulses (counterclockwise is positive)
+        angle2 = Encoder2.getPulses()/res;   //get number of pulses
+        m1_pwm = (PID(ref_angle1-angle1,m1_Kp,m1_Ki,m1_Kd,m1_Ts,m1_N,m1_v1,m1_v2))/V_max; 
+        //divide by voltage to get pwm duty cycle percentage)
+        m2_pwm = (PID(ref_angle2-angle2,m2_Kp,m2_Ki,m2_Kd,m2_Ts,m2_N,m2_v1,m2_v2))/V_max;    
+        
+        if (m1_pwm >=0.0f && m1_pwm <=1.0f){
+            motor1dir=0;
+            motor1.write(m1_pwm);
+        }
+        else if (m1_pwm < 0.0f && m1_pwm >= -1.0f){ 
+            motor1dir=1;
+            motor1.write(-m1_pwm);
+        }
+        
+        if (m2_pwm >=0.0f && m2_pwm <=1.0f){
+            motor1dir=0;
+            motor1.write(m2_pwm);
+        }
+        else if (m2_pwm < 0.0f && m2_pwm >= -1.0f){ 
+            motor1dir=1;
+            motor1.write(-m2_pwm);
+        }
+              
+        //hidsopce to check what the code does exactly
+        scope.set(0,angle1);
+        scope.set(1,refangle1-angle1);
+        scope.set(2,m1_pwm
+        scope.send();
+        
+        
+     }     
+
+
+        }
+
+int main()
+{
+    pc.baud(115200);
+    motor1.period(0.02f); //period of pwm signal for motor 1
+    motor2.period(0.02f); // period of pwm signal for motor 2
+    motor1dir=0; // setting direction to ccw 
+    motor2dir=0; // setting direction to ccw
+    control.attach(&controller,m1_Ts); //Ticker for processing encoder input
+    pc.printf("RESET\n\r");
+    while (true) {
+        if(controller_go){
+            controller_go=false;
+            controller();
+    }
+}
\ No newline at end of file