Diff: main.cpp

Revision:

22:6cc93216b323

Parent:

21:394a7a1deb73

Child:

23:78898ddfb103

--- a/main.cpp	Mon Oct 14 08:50:26 2019 +0000
+++ b/main.cpp	Mon Oct 14 13:26:46 2019 +0000
@@ -26,9 +26,9 @@
 bool    next    = false;
 HIDScope scope(3);
 QEI encoder(D12,D13,NC,8400,QEI::X4_ENCODING);
-volatile double theta;
-float   theta_error;
-volatile float theta_reference;
+volatile double theta_1;
+volatile float  theta_error_1;
+volatile float  theta_reference_1;
 float K     = 1;
 float ti    = 0.1;
 float td    = 10;
@@ -38,9 +38,9 @@
 float Ts    = 0.001;
 
 // functies
-float CalculateError()
+float CalculateError(float theta_reference,float theta)
 {
-    theta_error = theta_reference-theta;
+    float theta_error = theta_reference-theta;
     return theta_error;
 }
 
@@ -68,29 +68,30 @@
     return torque;
 }
 
-void CalculateDirectionMotor1() 
+void CalculateDirectionMotor() 
 {
-    direction_motor_1 = Controller(CalculateError()) <= 0.0f ;
+    direction_motor_1 = Controller(CalculateError(theta_reference_1,theta_1)) <= 0.0f;
+    //direction_motor_2 = Controller(CalculateError(theta_reference_2,theta_2)) <= 0.0f;
 }
 
 void WriteScope()
 {
-    scope.set(0,theta);
+    scope.set(0,theta_1);
     //scope.set(1,Controller(CalculateError()));
-    scope.set(1,CalculateError());
-    scope.set(2,theta_reference);
+    scope.set(1,CalculateError(theta_reference_1,theta_1));
+    scope.set(2,theta_reference_1);
     scope.send();
 }
 
 void ReadEncoder(){
-    theta = ((360.0f/64.0f)*(float)encoder.getPulses())/131.25f; // 360/64 voor de 64 CPR Encoder, 131.25 omdat de gear ratio 131.25:1 is. Hiermee is 1 omwenteling 360.
+    theta_1 = ((360.0f/64.0f)*(float)encoder.getPulses())/131.25f; // 360/64 voor de 64 CPR Encoder, 131.25 omdat de gear ratio 131.25:1 is. Hiermee is 1 omwenteling 360.
     }
 
 void Motor_1() {
     ReadEncoder();
-    theta_reference = 360*sin(0.1f*sinus_time.read()*2*3.14f);
-    CalculateDirectionMotor1();
-    PWM_motor_1.write(fabs(Controller(CalculateError())/360.0f));
+    theta_reference_1 = 360*sin(0.1f*sinus_time.read()*2*3.14f);
+    CalculateDirectionMotor();
+    PWM_motor_1.write(fabs(Controller(CalculateError(theta_reference_1,theta_1))/360.0f));
 }
 
 void go_next_1()