Diff: main.cpp

Revision:

3:4b6b3b5e3a1b

Parent:

2:44758d95cb0b

--- a/main.cpp	Wed Oct 31 15:08:58 2018 +0000
+++ b/main.cpp	Wed Oct 31 15:35:56 2018 +0000
@@ -19,10 +19,14 @@
 // PID  CONTROLLER     --        PIN DEFENITIONS 
 AnalogIn button2(A4);
 AnalogIn button1(A3);
-DigitalOut directionpin1(D7);   // motor 1
+
+DigitalOut directionpin1(D4);   // motor 1
+DigitalOut directionpin2(D7);   // motor 2
 PwmOut pwmpin1(D6);             // motor 1
+PwmOut pwmpin2(D5);             // motor 2
 
 QEI encoder1 (D9, D8, NC, 8400, QEI::X4_ENCODING);
+QEI encoder2 (D11, D10, NC, 8400, QEI::X4_ENCODING); // motor 2
 MODSERIAL pc(USBTX, USBRX);
 HIDScope scope(2);
 
@@ -40,6 +44,7 @@
 double Kd = 3; //200, 10
 double Ts = 0.1; // Sample time in seconds
 double reference_rotation; //define as radians
+double countsToRadians = (2*PI)/8400;
 double motor_position;
 bool AlwaysTrue;
 
@@ -209,21 +214,30 @@
 
 
 // DIRECTON AND SPEED CONTROL
-void moter_control(double u)
+void motor_control(double u_1, double u_2)
 {
-    directionpin1= u > 0.0f; //eithertrueor false
-    pwmpin1= fabs(u); //pwmduty cycle canonlybepositive, floatingpoint absolute value
+    directionpin1= u_1 > 0.0f; //eithertrueor false
+    pwmpin1= fabs(u_1); //pwmduty cycle canonlybepositive, floatingpoint absolute value
+    
+    directionpin2= u_2 > 0.0f; //eithertrueor false
+    pwmpin2= fabs(u_2); //pwmduty cycle canonlybepositive, floatingpoint absolute value
 }
 
 
 // CONTROLLING THE MOTOR
 void Motor_mover()
 {
-    double motor_position = encoder1.getPulses(); //output in counts
-    double reference_rotation = hoek2(px, py);
-    double error = reference_rotation - motor_position*(2*PI)/8400;
-    double u = PID_controller(error);
-    moter_control(u);
+    double motor_position1 = encoder1.getPulses(); //output in counts
+    double reference_rotation1 = hoek2(px, py);
+    double error_1 = reference_rotation1 - motor_position1*countsToRadians;
+    double u_1 = PID_controller(error_1);
+    
+    double motor_position2 = encoder2.getPulses(); //output in counts
+    double reference_rotation2 = hoek2(px, py);
+    double error_2 = reference_rotation2 - motor_position2*countsToRadians;
+    double u_2 = PID_controller(error_2);
+    
+    motor_control(u_1, u_2);
 }
 
 //PRINT TICKER