Revision 32:e22f89879d99, committed 2019-10-30

Comitter:

PatrickZieverink

Date:

Wed Oct 30 09:14:54 2019 +0000

Parent:

31:9350f76903c3

Commit message:

Werkend geheel, alleen nog een waarde van P van motor 0 (1) moet nog worden aangepast

Changed in this revision

--- a/main.cpp	Tue Oct 29 15:50:26 2019 +0000
+++ b/main.cpp	Wed Oct 30 09:14:54 2019 +0000
@@ -197,7 +197,7 @@
         state_changed = false;
     }
     theta_desired[0] = theta_desired[0] + dt*(velocity_desired[1]*cos(theta[0])/theta[1] - velocity_desired[0]*sin(theta[0])/theta[1]);
-    theta_desired[1] = theta_desired[1] + dt*(velocity_desired[0]*cos(theta[0]) - velocity_desired[1]*sin(theta[0]));
+    theta_desired[1] = theta_desired[1] + dt*(velocity_desired[0]*cos(theta[0]) + velocity_desired[1]*sin(theta[0]));
     errors[0] = theta_desired[0] - theta[0];
     errors[1] = theta_desired[1] - theta[1];
     if (button2_pressed) {
@@ -220,7 +220,7 @@
         state_changed = false;
     }
     theta_desired[0] = theta_desired[0] + dt*(velocity_desired[1]*cos(theta[0])/theta[1] - velocity_desired[0]*sin(theta[0])/theta[1]);
-    theta_desired[1] = theta_desired[1] + dt*(velocity_desired[0]*cos(theta[0]) - velocity_desired[1]*sin(theta[0]));
+    theta_desired[1] = theta_desired[1] + dt*(velocity_desired[0]*cos(theta[0]) + velocity_desired[1]*sin(theta[0]));
     errors[0] = theta_desired[0] - theta[0];
     errors[1] = theta_desired[1] - theta[0];
 }
@@ -234,7 +234,7 @@
         state_changed = false;
     }
     theta_desired[0] = theta_desired[0] + dt*(velocity_desired[1]*cos(theta[0])/theta[1] - velocity_desired[0]*sin(theta[0])/theta[1]);
-    theta_desired[1] = theta_desired[1] + dt*(velocity_desired[0]*cos(theta[0]) - velocity_desired[1]*sin(theta[0]));
+    theta_desired[1] = theta_desired[1] + dt*(velocity_desired[0]*cos(theta[0]) + velocity_desired[1]*sin(theta[0]));
     errors[0] = theta_desired[0] - theta[0];
     errors[1] = theta_desired[1] - theta[0];
 }
@@ -268,7 +268,7 @@
         enc_value[c] -= enc_zero[c];
         theta[c] = (float)(enc_value[c])/(float)(8400)*2*PI;    //Revoluties        Theta 0 en 1 zijn de gemeten waardes hier!
     }
-    theta[1] = theta[1]*(5.05*0.008/2.0/PI)+0.63; 
+    theta[1] = theta[1]*(5.05*0.008/2.0/PI)*-1+0.63; 
     theta[0] = theta[0]*-1.0;                
     
     for(int c = 0; c<2; c++) {
@@ -334,7 +334,7 @@
         last_error[c] = errors[c];
     }
         
-        mg = (theta[1]-0.125)*9.81*0.240*cos(theta[0]*1.75);
+        mg = (theta[1]-0.125)*9.81*0.200*cos(theta[0]*1.75);
         duty_mg = mg/10.0;
         action[0] += duty_mg;
         
@@ -376,12 +376,12 @@
     scope.set(1, speed[0]);
     scope.set(2, actuator.duty_cycle[0]);
     */
-    scope.set(0, errors[0]);
-    scope.set(1, PID.I_counter[0]);
-    scope.set(2, errors[1]);
+    scope.set(0, actuator.duty_cycle[1]);
+    scope.set(1, theta[1]);
+    scope.set(2, theta_desired[1]);
     scope.set(3, actuator.duty_cycle[0]);
     scope.set(4, theta[0]);
-    scope.set(5, theta_desired[0]);
+    scope.set(5, errors[0]);
 }
 
 void state_machine()
@@ -472,13 +472,13 @@
     actuator.direction[1] = 0;
 
     actuator.default_direction[0] = true;
-    actuator.default_direction[1] = false;
+    actuator.default_direction[1] = false;          // dit is gedubbelcheckt!
     
-    PID.P[0] = -0.25;
-    PID.P[1] = 1.0;
+    PID.P[0] = 1.25;
+    PID.P[1] = 0.5;
     PID.I[0] = 0.0;
     PID.I[1] = 0.0;
-    PID.D[0] = -0.005;
+    PID.D[0] = 0.005;
     PID.D[1] = 0.0;
     PID.I_counter[0] = 0.0;
     PID.I_counter[1] = 0.0;