Diff: main.cpp

Revision:

23:78898ddfb103

Parent:

22:6cc93216b323

Child:

24:a9ec9b836fd9

diff -r 6cc93216b323 -r 78898ddfb103 main.cpp
--- a/main.cpp	Mon Oct 14 13:26:46 2019 +0000
+++ b/main.cpp	Mon Oct 21 10:14:44 2019 +0000
@@ -9,14 +9,16 @@
 MODSERIAL pc(USBTX, USBRX);
 InterruptIn stepresponse(D0);
 FastPWM     PWM_motor_1(D6);
+FastPWM     PWM_motor_2(D5);
 DigitalOut  direction_motor_1(D7);
+DigitalOut  direction_motor_2(D4);
 InterruptIn button_1(SW2);
 InterruptIn button_2(SW3);
 
 // variables
 Ticker          TickerStateMachine;
-Ticker          ControlMotor1;
-Ticker          RW_scope;
+Ticker          motor_control;
+Ticker          write_scope;
 Timer           sinus_time;
 enum    states  {start, motor_calibration, demo_mode, emg_calibration, vertical_movement, horizontal_movement};
 states  CurrentState = start;
@@ -25,17 +27,18 @@
 bool    emg     = false;
 bool    next    = false;
 HIDScope scope(3);
-QEI encoder(D12,D13,NC,8400,QEI::X4_ENCODING);
+QEI encoder_1(D10,D11,NC,8400,QEI::X4_ENCODING);
+QEI encoder_2(D12,D13,NC,8400,QEI::X4_ENCODING);
 volatile double theta_1;
-volatile float  theta_error_1;
+//volatile float  theta_error_1;
 volatile float  theta_reference_1;
-float K     = 1;
-float ti    = 0.1;
-float td    = 10;
-float Kp    = K*(1+td/ti);
-float Ki    = K/ti;
-float Kd    = K*td;
+volatile double theta_2;
+//volatile float  theta_error_2;
+volatile float  theta_reference_2;
 float Ts    = 0.001;
+float Kp;
+float Ki;
+float Kd;
 
 // functies
 float CalculateError(float theta_reference,float theta)
@@ -44,8 +47,26 @@
     return theta_error;
 }
 
-float Controller(float theta_error)
+float Controller(float theta_error, bool motor)
 {
+    if (motor == false)
+    {
+        float K     = 1;
+        float ti    = 0.1;
+        float td    = 10;
+        Kp    = K*(1+td/ti);
+        Ki    = K/ti;
+        Kd    = K*td;
+    }
+    else
+    {
+        float K     = 1;
+        float ti    = 0.1;
+        float td    = 10;
+        Kp    = K*(1+td/ti);
+        Ki    = K/ti;
+        Kd    = K*td;
+    }
     static float error_integral = 0;
     static float error_prev = 0;
     static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
@@ -70,28 +91,31 @@
 
 void CalculateDirectionMotor() 
 {
-    direction_motor_1 = Controller(CalculateError(theta_reference_1,theta_1)) <= 0.0f;
-    //direction_motor_2 = Controller(CalculateError(theta_reference_2,theta_2)) <= 0.0f;
+    direction_motor_1 = Controller(CalculateError(theta_reference_1,theta_1),0) <= 0.0f;
+    direction_motor_2 = Controller(CalculateError(theta_reference_2,theta_2),1) <= 0.0f;
 }
 
 void WriteScope()
 {
     scope.set(0,theta_1);
-    //scope.set(1,Controller(CalculateError()));
     scope.set(1,CalculateError(theta_reference_1,theta_1));
     scope.set(2,theta_reference_1);
     scope.send();
 }
 
-void ReadEncoder(){
-    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 ReadEncoder()
+{
+    theta_1 = ((360.0f/64.0f)*(float)encoder_1.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_2 = ((360.0f/64.0f)*(float)encoder_2.getPulses())/131.25f;
+}
 
-void Motor_1() {
+void MotorControl() 
+{
     ReadEncoder();
-    theta_reference_1 = 360*sin(0.1f*sinus_time.read()*2*3.14f);
+    theta_reference_1 = 360*sin(0.1f*sinus_time.read()*2*3.14f); // voor test, moet weg in eindscript
     CalculateDirectionMotor();
-    PWM_motor_1.write(fabs(Controller(CalculateError(theta_reference_1,theta_1))/360.0f));
+    PWM_motor_1.write(fabs(Controller(CalculateError(theta_reference_1,theta_1),0)/360.0f));
+    PWM_motor_2.write(fabs(Controller(CalculateError(theta_reference_2,theta_2),1)/360.0f));
 }
 
 void go_next_1()
@@ -244,8 +268,8 @@
     button_2.fall(go_next_2);
     sinus_time.start();
     PWM_motor_1.period_ms(10);
-    ControlMotor1.attach(&Motor_1, Ts);
-    RW_scope.attach(&WriteScope, 0.1);
+    motor_control.attach(&MotorControl, Ts);
+    write_scope.attach(&WriteScope, 0.1);
     //TickerStateMachine.attach(StateMachine,1.00f);
     while(true) {
         StateMachine();