Diff: main.cpp

Revision:

16:2083f634c91c

Parent:

15:d38d5d4ae86a

--- a/main.cpp	Tue Oct 25 13:15:14 2016 +0000
+++ b/main.cpp	Tue Oct 25 14:18:30 2016 +0000
@@ -3,7 +3,7 @@
 #include "QEI.h"
 #include "math.h"
 #include "BiQuad.h"
-//#include "HIDScope.h"
+#include "HIDScope.h"
 
 //*****************Defining ports********************
 DigitalOut motor1DirectionPin (D4);
@@ -12,7 +12,7 @@
 PwmOut motor2MagnitudePin(D7);
 QEI encoder_m1(D12,D13,NC,32);
 QEI encoder_m2(D10,D11,NC,32);
-//HIDScope scope(2);
+HIDScope scope(2);
 DigitalIn button(D2);
 Serial pc(USBTX,USBRX);
 //*******************Setting tickers and printers*******************
@@ -23,6 +23,7 @@
 Ticker t4;
 Ticker t5;
 Ticker t6;
+Ticker HIDS;
 
 
 //**************Go flags********************************************
@@ -43,7 +44,7 @@
 //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 m1_Kp =  33760.9402682728, m1_Ki = 17183475.7181081, m1_Kd = 11.5703086491897, m1_N = 14221.4403045604;
 const double m2_Kp = 0.000048765659063912, m2_Ki = 0.0000228295351674407, m2_Kd = 0.0000255784613247063, m2_N = 54.5397025421619;
 const double m1_Ts = 0.001; // Controller sample time motor 1 
 const double m2_Ts = 0.001; // Controller sample time motor 2
@@ -54,6 +55,9 @@
 // position variable
 volatile double radians_m1;
 volatile double radians_m2;
+volatile double error_m1;
+volatile double error_m2;
+
 //plant variable
 volatile double motor1;
 volatile double motor2;
@@ -76,7 +80,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,6 +95,20 @@
  return u;
  }
 
+ double PID2( 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_2 = (-4.0/(N*Ts+2));
+ const double a2_2 = -(N*Ts-2)/(N*Ts+2);
+ const double b0_2 = (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_2 = (Ki*N*pow(Ts,2) - 4.0*Kp - 4.0*Kd*N)/(N*Ts + 2.0);
+ const double b2_2 = (4*Kp + 4*Kd*N - 2*Ki*Ts - 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4);
+
+ double v_2 = err - a1_2*v1_2 - a2_2*v2_2;
+ double u_2 = b0_2*v_2 + b1_2*v1_2 + b2_2*v2_2; 
+ v2_2 = v1_2; v1_2 = v_2;
+ return u_2;
+ }
 
 void getAngPosition_m1()  //Get angular position motor 1
 {   
@@ -107,17 +125,17 @@
  // 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 = 2*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 = 2*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 = PID2( error_m2,m2_Kp,m2_Ki,m2_Kd,m2_Ts, m2_N, m2_v1, m2_v2 );
  
  }
   
@@ -144,7 +162,7 @@
 
 void control_m2(double motor2)
 {
-if(abs(motor2)>0.000005)
+if(abs(motor2)>0.005)
         {
         motor2MagnitudePin=0.5;//MOET NOG TUSSENWAAREN KRIJGEN
         }
@@ -154,16 +172,24 @@
         }
 if(motor2<=0)      
         {
-        motor2DirectionPin=1.0;
+        motor2DirectionPin=0.0;
         }
 else    {
-        motor2DirectionPin=0.0;
+        motor2DirectionPin=1.0;
         }
 }
 
+void print()
+{
+scope.set(0,radians_m1);
+scope.send();  
+scope.set(1,radians_m2);
+scope.send();    
+}
+
 //****************MAIN FUNCTION*********************************
 int main()
-{
+{HIDS.attach(print,0.001f);
 motor1MagnitudePin.period(1.0/1000.0);
 motor2MagnitudePin.period(1.0/1000.0);
 t1.attach(&fn1_activate, 0.0001f);