Diff: main.cpp

Revision:

18:e1354199fd48

Parent:

17:457dd9a70c7c

diff -r 457dd9a70c7c -r e1354199fd48 main.cpp
--- a/main.cpp	Wed Oct 26 13:08:37 2016 +0000
+++ b/main.cpp	Fri Oct 28 08:35:16 2016 +0000
@@ -57,7 +57,11 @@
 //plant variable
 volatile double motor1;
 volatile double motor2;
-
+//max/min angles
+const double max_rad_m1 = pi;
+const double min_rad_m1 = -0.5*pi;
+const double max_rad_m2 = pi;
+const double min_rad_m2 = -0.5*pi;
 
 
 //*****************Angles Arms***********************
@@ -121,7 +125,7 @@
  // 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 = 0.5*pi;
  volatile double error_m1 = reference_m1 - radians_m1;
  motor1 = PID1( error_m1,m1_Kp,m1_Ki,m1_Kd,m1_Ts, m1_N, m1_v1, m1_v2 );
  }
@@ -129,7 +133,7 @@
   // Next task, measure the error and apply the output to the plant
  void motor2_Controller(double radians_m2)
  {
- double reference_m2 = -0.5*pi;
+ double reference_m2 = 2*pi;
  volatile double error_m2 = reference_m2 - radians_m2;
  motor2 = PID1( error_m2,m2_Kp,m2_Ki,m2_Kd,m2_Ts, m2_N, m2_v1, m2_v2 );
 
@@ -138,11 +142,11 @@
   
   
   
-void control_m1(double motor1)
+void control_m1(double motor1,double radians_m1)
 {
 if(abs(motor1)>1.0)
         {
-        motor1MagnitudePin=0.5;//MOET NOG TUSSENWAAREN KRIJGEN
+        motor1MagnitudePin=0.2;//MOET NOG TUSSENWAAREN KRIJGEN
         }
 else
         {
@@ -155,9 +159,17 @@
 else    {
         motor1DirectionPin=1.0;
         }
+if (radians_m1>max_rad_m1)
+        {
+        motor1MagnitudePin = 0;
+        }   
+else if (radians_m1<min_rad_m1)
+        {
+        motor1MagnitudePin = 0;
+        }   
 }
 
-void control_m2(double motor2)
+void control_m2(double motor2,double radians_m2)
 {
 if(abs(motor2)>1)
         {
@@ -174,6 +186,14 @@
 else    {
         motor2DirectionPin=0.0;
         }
+if (radians_m2>max_rad_m2)
+        {
+        motor2MagnitudePin = 0;
+        }   
+else if (radians_m2<min_rad_m2)
+        {
+        motor2MagnitudePin = 0;
+        }   
 }
 
 
@@ -205,7 +225,7 @@
         if(fn3_go)
         {
         fn3_go = false;
-        control_m1(motor1);
+        control_m1(motor1,radians_m1);
         }
         if(fn4_go)
         {
@@ -220,7 +240,7 @@
         if(fn6_go)
         {
         fn6_go = false;
-        control_m2(motor2);
+        control_m2(motor2,radians_m2);
         }
 
 }