Diff: main.cpp

Revision:

14:92614abdb7e3

Parent:

13:f6ecdd3f6db1

Child:

15:a90c450b1e0e

--- a/main.cpp	Mon Oct 05 11:56:30 2015 +0000
+++ b/main.cpp	Tue Oct 06 12:56:33 2015 +0000
@@ -21,7 +21,7 @@
 
 double setpoint;
 double rads;
-const double K = 2 ;
+const double K = 1 ;
 
 
 // counts 2 radians
@@ -55,34 +55,45 @@
     scope.send();
     double error = (setpoint - rads);                       // determine the error (reference - position)
     double output = K*error;                            // controller output K*e
+    
+    // Limit the output to to a number between -1 and 1 because 
+    // the motorcode will not handle anything smaller than -1 or larger than 1
+    // should be put in own function to improve readability
+    if(output>1)                                
+    {
+        output = 1;
+    }
+    else if(output < 1 && output > 0)
+    {
+        output = output;
+    }
+    else if(output > -1 && output < 0)
+    {
+        output = output;
+    }
+    else if(output < -1)
+    {
+        (output = -1);
+    }
     return output;
 }
 
-// this function controls the input for one of the electric motore and is called by a ticker
+// this function controls the input for one of the electric motors and is called by a ticker
 void motor1_control()
 {
     double output = K_control();        // bereken de controller output (zie hierboven)
     if(output > 0) {                    // 
         motor1_rich.write(0);
-        motor1_aan.write(1);
+        motor1_aan.write(output);
     } else if(output < 0) {
         motor1_rich.write(1);
-        motor1_aan.write(1);
+        motor1_aan.write(abs(output));
     }
 }
 
-// send specified data to hidscope
-// word nu in control loop gedaan
-void send()
-{
-    scope.set(0,setpoint);
-    scope.set(1,rads);
-    scope.send();
-}
 
 int main()
 {
-    // mod.attach(&send,0.01);         // send a signal to hidscope
     mod.attach(&motor1_control, 0.1);
     while(true) 
     {