Diff: main.cpp

Revision:

17:457dd9a70c7c

Parent:

15:d38d5d4ae86a

Child:

18:e1354199fd48

--- a/main.cpp	Tue Oct 25 13:15:14 2016 +0000
+++ b/main.cpp	Wed Oct 26 13:08:37 2016 +0000
@@ -6,15 +6,17 @@
 //#include "HIDScope.h"
 
 //*****************Defining ports********************
-DigitalOut motor1DirectionPin (D4);
+
 PwmOut motor1MagnitudePin(D5);
-DigitalOut motor2DirectionPin (D6);
-PwmOut motor2MagnitudePin(D7);
+PwmOut motor2MagnitudePin(D6);
+DigitalOut motor1DirectionPin (D4);
+DigitalOut motor2DirectionPin (D7);
+
 QEI encoder_m1(D12,D13,NC,32);
 QEI encoder_m2(D10,D11,NC,32);
-//HIDScope scope(2);
+//HIDScope scope(1);
 DigitalIn button(D2);
-Serial pc(USBTX,USBRX);
+MODSERIAL pc(USBTX,USBRX);
 //*******************Setting tickers and printers*******************
 Ticker angPos1;
 Ticker t1;
@@ -23,8 +25,6 @@
 Ticker t4;
 Ticker t5;
 Ticker t6;
-
-
 //**************Go flags********************************************
 volatile bool fn1_go = false;
 void fn1_activate(){ fn1_go = true; }; //Activates the go−flag
@@ -43,8 +43,8 @@
 //const double transmissionShoulder =94.4/40.2;
 //const double transmissionElbow = 1.0;
 // controller constants 
-const double m1_Kp = 0.000048765659063912, m1_Ki = 0.0000228295351674407, m1_Kd = 0.0000255784613247063, m1_N = 54.5397025421619;
-const double m2_Kp = 0.000048765659063912, m2_Ki = 0.0000228295351674407, m2_Kd = 0.0000255784613247063, m2_N = 54.5397025421619;
+const double m1_Kp = 120.0, m1_Ki = 1.44876354368902, m1_Kd = -1.55261758822823, m1_N = 1.70578345077793;
+const double m2_Kp = 120.0, m2_Ki = 1.44876354368902, m2_Kd = -1.55261758822823, m2_N = 1.70578345077793;
 const double m1_Ts = 0.001; // Controller sample time motor 1 
 const double m2_Ts = 0.001; // Controller sample time motor 2
 double m1_v1 = 0;
@@ -76,7 +76,7 @@
 double B6=-0.8994;
 
 //**********functions******************************
- double PID( double err, const double Kp, const double Ki, const double Kd,
+ double PID1( double err, const double Kp, const double Ki, const double Kd,
  const double Ts, const double N, double &v1, double &v2 ) {
  // These variables are only calculated once!
  const double a1 = (-4.0/(N*Ts+2));
@@ -91,41 +91,56 @@
  return u;
  }
 
+ double PID2( double err, const double Kp, const double Ki, const double Kd,
+ const double Ts, const double N, double &v1_2, double &v2_2 ) {
+ // These variables are only calculated once!
+ const double a1 = (-4.0/(N*Ts+2));
+ const double a2 = -(N*Ts-2)/(N*Ts+2);
+ const double b0 = (4.0*Kp + 4*Kd*N + 2*Ki*Ts + 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4);
+ const double b1 = (Ki*N*pow(Ts,2) - 4.0*Kp - 4.0*Kd*N)/(N*Ts + 2.0);
+ const double 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_2 - a2*v2_2;
+ double u = b0*v + b1*v1_2 + b2*v2_2; 
+ v2_2 = v1_2; v1_2 = v;
+ return u;
+ }
 
 void getAngPosition_m1()  //Get angular position motor 1
 {   
     volatile int pulses_m1 = encoder_m1.getPulses();
-    radians_m1 = (pulses_m1 / (1 * 3591.84)) * 2*pi; //2 = encoding type, 3591.84 = counts per revoluton for the two big motors
+    radians_m1 = (pulses_m1*0.002991*0.5); //2 = encoding type, 3591.84 = counts per revoluton for the two big motors
 }
 
 void getAngPosition_m2() //Get angular position motor 2
 {   
     volatile int pulses_m2 = encoder_m2.getPulses();
-    radians_m2 = (pulses_m2 / (1 * 3591.84)) * 2*pi; //2 = encoding type, 3591.84 = counts per revoluton for the two big motors  
+    radians_m2 = (pulses_m2*0.002991*0.5); //2 = encoding type, 3591.84 = counts per revoluton for the two big motors  
 }
   
  // Next task, measure the error and apply the output to the plant
  void motor1_Controller(double radians_m1)
  {
- double reference_m1 = 1.0*pi;
+ double reference_m1 = -1.0*pi;
  volatile double error_m1 = reference_m1 - radians_m1;
- motor1 = PID( error_m1,m1_Kp,m1_Ki,m1_Kd,m1_Ts, m1_N, m1_v1, m1_v2 );
+ motor1 = PID1( error_m1,m1_Kp,m1_Ki,m1_Kd,m1_Ts, m1_N, m1_v1, m1_v2 );
  }
  
   // Next task, measure the error and apply the output to the plant
  void motor2_Controller(double radians_m2)
  {
- double reference_m2 = -1/2*pi;
+ double reference_m2 = -0.5*pi;
  volatile double error_m2 = reference_m2 - radians_m2;
- motor2 = PID( error_m2,m2_Kp,m2_Ki,m2_Kd,m2_Ts, m2_N, m2_v1, m2_v2 );
- 
+ motor2 = PID1( error_m2,m2_Kp,m2_Ki,m2_Kd,m2_Ts, m2_N, m2_v1, m2_v2 );
+
+pc.printf("motor2 = %d",reference_m2);
  }
   
   
   
 void control_m1(double motor1)
 {
-if(abs(motor1)>0.000005)
+if(abs(motor1)>1.0)
         {
         motor1MagnitudePin=0.5;//MOET NOG TUSSENWAAREN KRIJGEN
         }
@@ -144,15 +159,15 @@
 
 void control_m2(double motor2)
 {
-if(abs(motor2)>0.000005)
+if(abs(motor2)>1)
         {
-        motor2MagnitudePin=0.5;//MOET NOG TUSSENWAAREN KRIJGEN
+        motor2MagnitudePin=0.2;///MOET NOG TUSSENWAAREN KRIJGEN
         }
 else
         {
         motor2MagnitudePin=0.0;    
         }
-if(motor2<=0)      
+if(motor2<=0)    
         {
         motor2DirectionPin=1.0;
         }
@@ -161,6 +176,8 @@
         }
 }
 
+
+
 //****************MAIN FUNCTION*********************************
 int main()
 {
@@ -178,7 +195,7 @@
         if(fn1_go)
         {
         fn1_go = false;
-        control_m1(motor1);
+        getAngPosition_m1();
         }
         if(fn2_go)
         {
@@ -188,12 +205,12 @@
         if(fn3_go)
         {
         fn3_go = false;
-        getAngPosition_m1();
+        control_m1(motor1);
         }
         if(fn4_go)
         {
         fn4_go = false;
-        control_m2(motor2);
+       getAngPosition_m2();
         }
         if(fn5_go)
         {
@@ -203,7 +220,7 @@
         if(fn6_go)
         {
         fn6_go = false;
-        getAngPosition_m2();
+        control_m2(motor2);
         }
 
 }