Diff: main.cpp

Revision:

9:6a065971d0ae

Parent:

8:7cc4d6d9c2b5

Child:

10:54b66bd1db20

--- a/main.cpp	Thu Oct 20 13:33:47 2016 +0000
+++ b/main.cpp	Fri Oct 21 09:40:00 2016 +0000
@@ -35,9 +35,12 @@
 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 CONTROLLER_TS = 0.01;
+double m1_err_int=0;
 volatile double radians;
 volatile double proportionalErrormotor1;
+volatile double motor1;
 //*****************Angles Arms***********************
 
 double O1=1.7633;
@@ -54,10 +57,14 @@
 double B6=-0.8994;
 
 //**********functions******************************
+double P(volatile double error, const double Kp){
+    return Kp * error;
+    }
+
 void getAngPosition() 
 {   
     volatile int pulses = encoder.getPulses();
-    radians = (pulses / (2 * 3591.84)) * 2*pi; //2 = encoding type, 3591.84 = counts per revoluton for the two big motors
+    radians = (pulses / (1 * 3591.84)) * 2*pi; //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
@@ -65,25 +72,29 @@
  {
  double reference = -2*pi;
  volatile double error1 = reference - radians;
- proportionalErrormotor1 = error1 * MOTOR1_KP;
-  // scope.set(0, proportionalErrormotor1);
-   //scope.send();
-   
-   // scope.set(velocityChannel,proportionalErrormotor1);
-    //scope.send();
+motor1 = P(error1,MOTOR1_KP) ;
+// scope.set(velocityChannel,proportionalErrormotor1);
+//scope.send();
  }
   
   
 void control(double proportionalErrormotor1)
 {
-if(proportionalErrormotor1<0)
+if(abs(motor1)>0.3)
         {
-        motor1MagnitudePin=1.0;//MOET NOG TUSSENWAAREN KRIJGEN
+        motor1MagnitudePin=0.5;//MOET NOG TUSSENWAAREN KRIJGEN
         }
-    else
+else
         {
         motor1MagnitudePin=0.0;    
         }
+if(motor1<=0)      
+        {
+        motor1DirectionPin=0.0;
+        }
+else    {
+        motor1DirectionPin=1.0;
+        }
 }
 
 
@@ -94,6 +105,8 @@
 t1.attach(&fn1_activate, 0.1f);
 t2.attach(&fn2_activate, 0.0001f);
 t3.attach(&fn3_activate, 0.0001f);
+scope.set(velocityChannel, proportionalErrormotor1);
+scope.send();
 while(true){
         if(fn1_go)
         {
@@ -111,5 +124,4 @@
         getAngPosition();
         }
 }
-
 }
\ No newline at end of file