Diff: main.cpp

Revision:

14:0afc46ad1b99

Parent:

13:048458947701

Child:

15:fb0bbce41a0f

--- a/main.cpp	Mon Oct 21 08:57:45 2019 +0000
+++ b/main.cpp	Mon Oct 21 09:39:10 2019 +0000
@@ -17,10 +17,15 @@
 // Encoder
 DigitalIn encA1(D13);
 DigitalIn encB1(D12);
+DigitalIn encA2(D9);
+DigitalIn encB2(D8);
 QEI encoder1(D13,D12,NC,64,QEI::X4_ENCODING);
+QEI encoder2(D9,D8,NC,64,QEI::X4_ENCODING);
 float Ts = 0.01;
 float motor1angle;
+float motor2angle;
 float omega1;
+float omega2;
 
 
 // Motor
@@ -33,25 +38,35 @@
 
 //Motorcontrol
 bool motordir1;
+bool motordir2;
 float motor1ref= 0;
+float motor2ref=0;
 double controlsignal1;
-double potValue;
+double controlsignal2;
 double pi2= 6.283185;
 float motor1error; //e = error
+float motor2error;
 float Kp=0.27;
 float Ki=0.35;
 float u_p1;
+float u_p2;
 float u_i1;
+float u_i2;
 
 //Windup control
 float ux1;
+float ux2;
 float up1;
+float up2;
 float ek1;
+float ek2;
 float ei1= 0;
+float ei2=0;
 float Ka= 1;
 
 //Hidscope
 HIDScope scope(3); //Going to send 3 channels of data. To access data go to 'http:/localhost:18082/' after starting HIDScope application.
+
 // PC connection
 MODSERIAL pc(USBTX, USBRX);
 
@@ -65,8 +80,11 @@
 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
@@ -100,18 +118,49 @@
      return controlsignal1; 
 }
 
+float PID_controller2(float motor2error){
+    static float error_integral2=0;
+    
+    //Proportional part:
+    u_p2=Kp*motor2error;
+    
+    //Integral part
+    error_integral2=error_integral2+ei2*Ts;
+    u_i2=Ki*error_integral2;
+    
+    // Sum and limit
+    up2= u_p2+u_i2;
+    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();
+    counts1 = encoder1.getPulses();        
     deltaCounts1 = counts1 - countsPrev1;
-
     countsPrev1 = counts1;
+    
+    counts2 = encoder2.getPulses();
+    deltaCounts2 = counts2 - countsPrev2;
+    countsPrev2 = counts2;    
 }
 
 void togglehoek(){
     
     motor1ref= 0.5*pi2+motor1ref;
+    motor2ref= 0.5*pi2+motor2ref;
     // static float t = 0;
     // refangle= pi2/3.0f*sin(5.0f*t)*motor1Toggle;
     //t+=0.01;
@@ -119,22 +168,29 @@
     
 void motorControl()
 {
-    //togglehoek();
     button1.fall(&togglehoek);
+    
     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;
-    motor1error=motor1ref-motor1angle;
-    
-    controlsignal1=PID_controller1(motor1error);
-       
+    motor1error=motor1ref-motor1angle;    
+    controlsignal1=PID_controller1(motor1error);      
     if (controlsignal1<0){
         motordir1= 0;}
     else {
          motordir1= 1;}
     motor1Power.write(abs(controlsignal1));
     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));
+    motor2Direction= motordir2;    
 }
 
 void Plotje(){
@@ -156,6 +212,7 @@
     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);
@@ -163,12 +220,11 @@
     button2.fall(&toggleMotor);
     
     while (true) {
-
-        //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", motor1angle, omega1);
-        pc.printf("refangle: %f   Error:  %f     \r\n",motor1ref, motor1error);
-        pc.printf("Kp: %f   Ki:    %f \r\n", Kp, Ki);
+        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("counts2: %f delta2: %f \r\n", counts2, deltaCounts2);
         
         wait(0.5);
     }