Diff: main.cpp

Revision:

19:3af738c38db0

Parent:

18:32e9db0d47c9

Child:

20:04b5619a4fb7

--- a/main.cpp	Wed Nov 02 10:35:47 2016 +0000
+++ b/main.cpp	Wed Nov 02 11:01:23 2016 +0000
@@ -57,8 +57,8 @@
 volatile const double DEGREES_PER_PULSE = 8400 / 360;
 
 // Set initial Kp and Ki
-volatile double Kp = 0.02; // last known good Kp: 0.02
-volatile double Ki = 0.005;   // last known good Ki: 0.0025
+volatile double Kp = 0.05; // last known good Kp: 0.02
+volatile double Ki = 0.015;   // last known good Ki: 0.0025
 volatile double Kd = 0.0; // last known good Kd: 0.0
 
 // Set frequencies for sampling, calc and control.
@@ -103,7 +103,7 @@
 volatile double x = 0.0;                             // beginpositie x en y
 volatile double y = 305.5;
 volatile const double pi = 3.14159265359; 
-volatile double Speed = 30;                          // Speed with which x and y are changed, in mm/s
+volatile double Speed = 60;                          // Speed with which x and y are changed, in mm/s
 volatile double Theta1Gear = 60.0;                   // Beginpositie op 60 graden
 volatile double Theta2Gear = 60.0;
 volatile const int LargeGear = 42;
@@ -386,7 +386,7 @@
     if (x > -a) {
         Theta1Gear = pi - atan(y/(x+a)) - acos(Dia1/Bar);
     }
-    else if (x > -a) {
+    else if (x < -a) {
         Theta1Gear = pi - (pi + atan(y/(x+a))) - acos(Dia1/Bar);
     }
     else {  // als x=-a
@@ -608,8 +608,8 @@
 
 void PotControl()
 {
-    double Motor1_Value = (pot1.read() - 0.5f);
-    double Motor2_Value = (pot2.read() - 0.5f);
+    double Motor1_Value = (pot1.read() - 0.5f)*1.5f;
+    double Motor2_Value = (pot2.read() - 0.5f)*1.5f;
     //pc.printf("\r\n Motor value 1: %f \r\n",Motor1_Value);
     //pc.printf("\r\n Motor value 2: %f \r\n",Motor2_Value);
     double m1_Position = m1_GetPosition();
@@ -691,25 +691,26 @@
         
     // Actual code starts here
     // Start positioning the arms
-    pc.printf("\r\n ***************** \r\n");
-    pc.printf("\r\n Press Button 1 to start positioning the arms using PotControl\r\n");
-    pc.printf("\r\n Make sure both potentiometers are positioned halfway! \r\n");
+    pc.printf("\r\n ***************** \r");
+    pc.printf("\r\n Press Button 1 to start positioning the arms using PotControl\r");
+    pc.printf("\r\n Make sure both potentiometers are positioned halfway! \r");
     pc.printf("\r\n ***************** \r\n");
     //pc.printf("\r\n Button1_value = %d \r\n", button1_value);
     while (button1_value == false)
     {   
+        wait(2);
         //pc.printf("\r\n Button1_value = %d \r\n", button1_value);
         //wait(5);
     }
     PotControlTicker.attach(&PotControl,CONTROLLER_TS);
     
-    pc.printf("\r\n ***************** \r\n");
+    pc.printf("\r\n ***************** \r");
     pc.printf("\r\n When done positioning, press Button 1 to detach Potcontrol");
     pc.printf("\r\n ***************** \r\n");
     while (button1_value == true)
     {   
         //pc.printf("\r\n Button1_value = %d \r\n", button1_value);
-        //wait(5);
+        wait(2);
     }
     
     // Detach potcontrol
@@ -724,14 +725,14 @@
     ResetEncoders();
     
     // PID control starts
-    pc.printf("\r\n ***************** \r\n");
-    pc.printf("\r\n When done positioning, press button SW3 to start potmeter PID control");
-    pc.printf("\r\n Make sure both potentiometers are positioned halfway!!! \r\n");
+    pc.printf("\r\n ***************** \r");
+    pc.printf("\r\n When done positioning, press Button 1 to start potmeter PID control");
+    pc.printf("\r\n Make sure both potentiometers are positioned halfway!!!");
     pc.printf("\r\n ***************** \r\n");
     while (button1_value == false)
     {   
         //pc.printf("\r\n Button1_value = %d \r\n", button1_value);
-        //wait(5);
+        wait(2);
     }
     
     // Attach sample, calc and control tickers