Diff: main.cpp

Revision:

3:56f31420abac

Parent:

2:ca645c585a03

Child:

4:61d9325d7f2e

--- a/main.cpp	Wed Nov 02 16:58:55 2016 +0000
+++ b/main.cpp	Thu Nov 03 14:31:56 2016 +0000
@@ -5,7 +5,7 @@
 #include "HIDScope.h"
 #include "math.h"
 
-InterruptIn sw3(SW3);
+DigitalIn sw3(SW3);
 DigitalIn sw2(SW2);
 DigitalIn encoder1A(D13);
 DigitalIn encoder1B(D12);
@@ -19,6 +19,7 @@
 DigitalOut richting_motor2(D7);
 PwmOut pwm_motor2(D6);
 BiQuad PID_controller;
+BiQuad PID_controller2;     // maakt een biquad aan voor PID controler 2
 //HIDScope Scope(1);
 
 //constanten voor de encoder
@@ -42,69 +43,127 @@
 
 
 volatile float d_ref = 0;
+volatile float d_ref2 = 0;
 volatile float d_ref_new;
 const float w_ref = 3;
+const float w_ref2 = 4.5;
 const double Ts = 0.001;
 const double Kp = 0.3823;
 const double Ki = 0.127875;
 const double Kd = 0.2519; 
 const double N = 100;
+const double Ts2 = 0.001;
+const double Kp2 = 0.3823;
+const double Ki2 = 0.127875;
+const double Kd2 = 0.2519; 
+const double N2 = 100;
 Ticker tick;
-Ticker controllerticker;
+Ticker controllerticker; 
+Ticker tick2;                       // ticker voor reverence voor motor 2
+Ticker controllerticker2;            //ticker voor controler voor motor 2
 float rev_counts_motor1_rad;
+float rev_counts_motor2_rad;
 volatile float voltage_motor1=0.5;
+volatile float voltage_motor2=0.5;  // pwm motor 2 aansturing
 volatile double error1;
-volatile double ctrlOutput;
+volatile double error2;             // error voor tweede motor
+volatile double ctrlOutput1;
+volatile double ctrlOutput2;        // control output voor motor 2
 volatile double d_ref_const_cw = 0;
 volatile double d_ref_const_ccw = 0;
+volatile int n = 0;
 
+void SwitchN() {                        // maakt simpele functie die 1 bij n optelt voor de switch naar motor 2
+    n++;
+    }
+ 
 
 void reference(){
-    if (button_cw == 0 ){
-        d_ref = d_ref + w_ref * Ts;
+   if(n%2== 0){ 
+        if (button_cw == 0){
+            d_ref = d_ref + w_ref * Ts;
+        }
+             if (d_ref > 21.36){
+                d_ref = 21.36;
+                //d_ref_const_cw = 1;
+            }
+        else{ 
+            d_ref = d_ref;
+        }
+        
+        if (button_ccw == 0){
+            d_ref = d_ref - w_ref * Ts;
+        }
+            if (d_ref < -21.36){
+            d_ref = -21.36;
+            
+        }
+        else{
+            d_ref = d_ref;
+        }
+  }     // haakje sluit voor if loop voor n%==0 
+}
+
+void reference2(){                    // maakt reference aan voor motor2
+if(n%2 != 0){
+    if (button_cw == 0){
+        d_ref2 = d_ref2 + w_ref2 * Ts;
     }
-         if (d_ref > 21.36 ){
-            d_ref = 21.36;
+         if (d_ref2 > 21.36){
+            d_ref2 = 21.36;
             //d_ref_const_cw = 1;
         }
     else{ 
-        d_ref = d_ref;
+        d_ref2 = d_ref2;
     }
     
     if (button_ccw == 0){
-        d_ref = d_ref - w_ref * Ts;
+        d_ref2 = d_ref2 - w_ref2 * Ts;
     }
-        if (d_ref < -21.36){
-        d_ref = -21.36;
+        if (d_ref2 < -21.36){
+        d_ref2 = -21.36;
         
     }
     else{
-        d_ref = d_ref;
+        d_ref2 = d_ref2;
     }
-    
+  } // haakje sluit voor if loop met n%!=0)  
 }
     
 void m1_controller(){
     error1 = d_ref-rev_counts_motor1_rad;
-    ctrlOutput = PID_controller.step(error1);
+    ctrlOutput1 = PID_controller.step(error1);
 }
+void m2_controller(){                       // void voor tweede controller
+    error2 = d_ref2-rev_counts_motor2_rad;
+    ctrlOutput2 = PID_controller.step(error2);
+    }
     
-void tickerfunctie()                // maakt een ticker functie aan
-{
-tickerflag = 1;                     // het enige wat deze functie doet is zorgen dat tickerflag 1 is
-}
+//void tickerfunctie()                // maakt een ticker functie aan
+//{
+//tickerflag = 1;                     // het enige wat deze functie doet is zorgen dat tickerflag 1 is
+//}
 
 
 int main()
 {
      pc.baud(115200);  
-QEI Encoder1(D10,D11, NC, rev_rond,QEI::X4_ENCODING);
+QEI Encoder2(D10,D11, NC, rev_rond,QEI::X4_ENCODING);
+QEI Encoder1(D13,D12, NC, rev_rond,QEI::X4_ENCODING);       //encoder motor 2
 int counts_encoder1;                  //variabele counts aanmaken
+int counts_encoder2;                  // variabele counts voor motor2
 float rev_counts_motor1; 
-t.attach(&tickerfunctie,ticker_TS);
+float rev_counts_motor2;                // variabele coutns voor motor2
+//t.attach(&tickerfunctie,ticker_TS);
+sw3.attach(&SwitchN,Ts);
 tick.attach(&reference,Ts); 
+tick2.attach(&reference2,Ts);           // ticker voor reference2 attach
 controllerticker.attach(&m1_controller,Ts);
+controllerticker2.attach(&m2_controller,Ts); // ticker voor controller2 attach
+
+
 PID_controller.PIDF(Kp,Ki,Kd,N,Ts);
+PID_controller2.PIDF(Kp2,Ki2,Kd2,N2,Ts2);
 
     while (true) {
        // if (button_cw == 0){
@@ -115,48 +174,31 @@
                 //pc.printf("pwm_motor is %f\r\n", pwm_motor1);
                 
                 counts_encoder1 = Encoder1.getPulses();                   // zorgt er voor dat het aantal rondjes geteld worden
+                counts_encoder2 = Encoder2.getPulses();
                 rev_counts_motor1=(float)counts_encoder1/(gearboxratio*rev_rond); 
                 rev_counts_motor1_rad=rev_counts_motor1*6.28318530718; 
-                voltage_motor1=ctrlOutput;
+                rev_counts_motor2=(float)counts_encoder2/(gearboxratio*rev_rond); 
+                rev_counts_motor2_rad=rev_counts_motor2*6.28318530718; 
+                voltage_motor1=ctrlOutput1;
+                voltage_motor2=ctrlOutput2;
             if (voltage_motor1<0){
                 richting_motor1 = 0;
                 }
             else   {
                 richting_motor1 = 1;
                 }
+            if (voltage_motor2<0){
+                richting_motor2 = 0;
+                }
+            else   {
+                richting_motor2 = 1;
+                }
                 pwm_motor1 = fabs(voltage_motor1);
-                
-                pc.printf("%f", pwm_motor1.read());
-                pc.printf("   %f", d_ref);
-                pc.printf("   %f \r\n", rev_counts_motor1_rad);
-                //Scope.set(0, rev_counts_motor1_rad);
-                //Scope.send();
-                //tickerflag = 0;
-              //  d_ref_const_ccw = 0;
-            //}
-      //  }
-       // else if (button_ccw == 0){
-    
-               /* richting_motor1 = 0;
-                pwm_motor1 = voltage_motor1;
-                //pc.printf("pwm_motor is %f\r\n", pwm_motor1);
+                pwm_motor2 = fabs(voltage_motor2);
                 
-                counts_encoder1 = Encoder1.getPulses();                   // zorgt er voor dat het aantal rondjes geteld worden
-                rev_counts_motor1=(float)counts_encoder1/(gearboxratio*rev_rond); 
-                rev_counts_motor1_rad=rev_counts_motor1*6.28318530718; 
-                voltage_motor1=ctrlOutput;
-                pc.printf("%f", pwm_motor1);
-                pc.printf("   %f", d_ref);
-                pc.printf("   %f \r\n", rev_counts_motor1_rad);
-                //Scope.set(0, rev_counts_motor1_rad);
-                //Scope.send();
-                //tickerflag = 0;
-                d_ref_const_cw = 0; */
-  //  }
-
-     //   else{
-      //      pwm_motor1=0;
-    
-        //}
+                pc.printf("%f", pwm_motor2.read());
+                pc.printf("   %f", d_ref2);
+                pc.printf("   %f \r\n", rev_counts_motor2_rad);
+                
     }
 }
\ No newline at end of file