Diff: main.cpp

Revision:

13:048458947701

Parent:

12:23b94b5dcc60

Child:

14:0afc46ad1b99

--- a/main.cpp	Fri Oct 18 10:36:42 2019 +0000
+++ b/main.cpp	Mon Oct 21 08:57:45 2019 +0000
@@ -15,12 +15,12 @@
 AnalogIn potmeter3(A2);
 AnalogIn potmeter4(A3);
 // Encoder
-DigitalIn encA(D13);
-DigitalIn encB(D12);
-QEI encoder(D13,D12,NC,64,QEI::X4_ENCODING);
+DigitalIn encA1(D13);
+DigitalIn encB1(D12);
+QEI encoder1(D13,D12,NC,64,QEI::X4_ENCODING);
 float Ts = 0.01;
-float angle;
-float omega;
+float motor1angle;
+float omega1;
 
 
 // Motor
@@ -32,24 +32,22 @@
 volatile int motor1Toggle = 1;
 
 //Motorcontrol
-bool motordir;
-double motorpwm;
-double premotorpwm;
-float refangle= 0;
-double u2;
+bool motordir1;
+float motor1ref= 0;
+double controlsignal1;
 double potValue;
 double pi2= 6.283185;
-float e; //e = error
-float Kp=0.49;
-float Ki;
-float u_k;
-float u_i;
+float motor1error; //e = error
+float Kp=0.27;
+float Ki=0.35;
+float u_p1;
+float u_i1;
 
 //Windup control
-float ux;
-float up;
-float ek;
-float ei= 0;
+float ux1;
+float up1;
+float ek1;
+float ei1= 0;
 float Ka= 1;
 
 //Hidscope
@@ -66,56 +64,54 @@
 
 const float PWM_period = 1e-6;
 
-volatile int counts; // Encoder counts
-volatile int countsPrev = 0;
-volatile int deltaCounts;
+volatile int counts1; // Encoder counts
+volatile int countsPrev1 = 0;
+volatile int deltaCounts1;
 
 float factorin = 6.23185/64; // Convert encoder counts to angle in rad
 float gearratio = 131.25; // Gear ratio of gearbox
 
 
-float PID_controller(float e){
-    static float error_integral=0;
+float PID_controller1(float motor1error){
+    static float error_integral1=0;
     //static float e_prev=e;
     
     //Proportional part:
-    Kp=potmeter1.read();
-    u_k=Kp*e;
+    u_p1=Kp*motor1error;
     
     //Integral part
-    Ki=potmeter2.read();
-    error_integral=error_integral+ei*Ts;
-    u_i=Ki*error_integral;
+    error_integral1=error_integral1+ei1*Ts;
+    u_i1=Ki*error_integral1;
     
     // Sum and limit
-    u2= u_k+u_i;
-    if (u2>1){
-        up=1;}
-    else if (u2<-1){
-        up=-1;}
+    up1= u_p1+u_i1;
+    if (up1>1){
+        controlsignal1=1;}
+    else if (up1<-1){
+        controlsignal1=-1;}
     else {
-        up=u2;}
+        controlsignal1=up1;}
     
     // To prevent windup
-    ux= u2-up;
-    ek= Ka*ux;
-    ei= e-ek;
+    ux1= up1-controlsignal1;
+    ek1= Ka*ux1;
+    ei1= motor1error-ek1;
      //Return
-     return up; 
+     return controlsignal1; 
 }
 
 
 void readEncoder()
 {
-    counts = encoder.getPulses();
-    deltaCounts = counts - countsPrev;
+    counts1 = encoder1.getPulses();
+    deltaCounts1 = counts1 - countsPrev1;
 
-    countsPrev = counts;
+    countsPrev1 = counts1;
 }
 
 void togglehoek(){
     
-    refangle= 15*pi2+refangle;
+    motor1ref= 0.5*pi2+motor1ref;
     // static float t = 0;
     // refangle= pi2/3.0f*sin(5.0f*t)*motor1Toggle;
     //t+=0.01;
@@ -125,31 +121,26 @@
 {
     //togglehoek();
     button1.fall(&togglehoek);
-    angle = counts * factorin / gearratio; // Angle of motor shaft in rad
-    omega = deltaCounts / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
+    motor1angle = counts1 * factorin / gearratio; // Angle of motor shaft in rad
+    omega1 = deltaCounts1 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
     potValue= potmeter1.read();
     //refangle= (potValue*2*pi2)-pi2;
-    e=refangle-angle;
-    
-    u2=PID_controller(e);
+    motor1error=motor1ref-motor1angle;
     
-    premotorpwm= fabs(u2);
-    if (premotorpwm>1.0){
-        motorpwm=1;}
+    controlsignal1=PID_controller1(motor1error);
+       
+    if (controlsignal1<0){
+        motordir1= 0;}
     else {
-        motorpwm=premotorpwm;}    
-    if (u2<0){
-        motordir= 0;}
-    else {
-         motordir= 1;}
-    motor1Power.write(motorpwm);
-    motor1Direction= motordir;
+         motordir1= 1;}
+    motor1Power.write(abs(controlsignal1));
+    motor1Direction= motordir1;
 }
 
 void Plotje(){
-    scope.set(0,refangle); //gewenste hoek
-    scope.set(1,angle); //Gemeten hoek
-    scope.set(2,e); //verschil in gewenste en gemeten hoek
+    scope.set(0,motor1ref); //gewenste hoek
+    scope.set(1,motor1angle); //Gemeten hoek
+    scope.set(2,motor1error); //verschil in gewenste en gemeten hoek
 
     scope.send(); //send what's in scope memory to PC
 }
@@ -175,8 +166,8 @@
 
         //pc.printf("Potmeter: %d \r\n", potValue,);
         //pc.printf("Counts: %i   DeltaCounts: %i\r\n", counts, deltaCounts);
-        pc.printf("Angle:  %f   Omega:       %f\r\n", angle, omega);
-        pc.printf("refangle: %f   Error:  %f     \r\n",refangle, e);
+        pc.printf("Angle:  %f   Omega:       %f\r\n", motor1angle, omega1);
+        pc.printf("refangle: %f   Error:  %f     \r\n",motor1ref, motor1error);
         pc.printf("Kp: %f   Ki:    %f \r\n", Kp, Ki);
         
         wait(0.5);