Diff: main.cpp

Revision:

11:eda4fbf91948

Parent:

10:54b66bd1db20

Child:

12:0cf4e70f6b8e

--- a/main.cpp	Fri Oct 21 10:27:04 2016 +0000
+++ b/main.cpp	Mon Oct 24 09:51:54 2016 +0000
@@ -2,8 +2,7 @@
 #include "MODSERIAL.h"
 #include "QEI.h"
 #include "math.h"
-#include "HIDScope.h"
-
+//#include "HIDScope.h"
 
 //*****************Defining ports********************
 DigitalOut motor1DirectionPin (D4);
@@ -11,7 +10,7 @@
 DigitalIn button(D2);
 Serial pc(USBTX,USBRX);
 QEI encoder(D12,D13,NC,32);
-HIDScope scope(1);
+//HIDScope scope(2);
 DigitalOut led(D0);
 //*******************Setting tickers and printers*******************
 Ticker callMotor;
@@ -20,6 +19,7 @@
 Ticker t2;
 Ticker t3;
 Ticker myControllerTicker;
+Ticker scopeTimer;
 //**************Go flags********************************************
 volatile bool fn1_go = false;
 void fn1_activate(){ fn1_go = true; }; //Activates the go−flag
@@ -30,14 +30,14 @@
 
 //***************Global Variables***********************************
 const double pi = 3.14159265359;
-const double ts = 1.0/1000.0;
+double m1_ts = 0.01;
 const int velocityChannel = 0;
 const double transmissionShoulder =94.4/40.2;
 const double transmissionElbow = 1.0;
-const double MOTOR1_KP = 2.5;
-const double MOTOR1_KI = 1.0;
+const double motor1_Kp = 1;
+const double motor1_Ki = 0.1;
 const double CONTROLLER_TS = 0.01;
-double m1_err_int=0;
+double m1_error_int=0;
 volatile double radians;
 volatile double proportionalErrormotor1;
 volatile double motor1;
@@ -57,14 +57,20 @@
 double B6=-0.8994;
 
 //**********functions******************************
-double P(volatile double error, const double Kp){
-    return Kp * error;
+double PI(volatile double error, const double Kp, const double Ki, double Ts, double &error_int)
+{
+    error_int += Ts*error;
+    return Kp * error + Ki*error_int;
+    
     }
 
 void getAngPosition() 
 {   
     volatile int pulses = encoder.getPulses();
     radians = (pulses / (1 * 3591.84)) * 2*pi; //2 = encoding type, 3591.84 = counts per revoluton for the two big motors
+//     scope.set(velocityChannel,pulses);
+//     scope.send();
+  
 }
   
  // Next task, measure the error and apply the output to the plant
@@ -72,15 +78,16 @@
  {
  double reference = 2*pi;
  volatile double error1 = reference - radians;
-motor1 = P(error1,MOTOR1_KP) ;
-// scope.set(velocityChannel,proportionalErrormotor1);
+motor1 = PI(error1,motor1_Kp,motor1_Ki, m1_ts, m1_error_int);
+pc.printf("%d\n\r",motor1);
+// scope.set(velocityChannel,motor1);
 //scope.send();
  }
   
   
 void control(double motor1)
 {
-if(abs(motor1)>0.3)
+if(abs(motor1)>1.0)
         {
         motor1MagnitudePin=0.5;//MOET NOG TUSSENWAAREN KRIJGEN
         }
@@ -103,7 +110,7 @@
 t1.attach(&fn1_activate, 0.1f);
 t2.attach(&fn2_activate, 0.0001f);
 t3.attach(&fn3_activate, 0.0001f);
-
+pc.baud(115200);
 while(true)
 {
         if(fn1_go)
@@ -115,7 +122,6 @@
         {
         fn2_go = false;
         motor1_Controller(radians);
-
         }
         if(fn3_go)
         {