Diff: main.cpp

Revision:

5:a54ea6514bc5

Parent:

4:85770b268599

Child:

6:6545e197858a

--- a/main.cpp	Wed Oct 31 16:00:45 2018 +0000
+++ b/main.cpp	Wed Oct 31 19:26:42 2018 +0000
@@ -22,11 +22,14 @@
 
 DigitalOut directionpin1(D7);   // motor 1
 DigitalOut directionpin2(D4);   // motor 2
+DigitalOut directionpin3(D13);  // motor 3
 PwmOut pwmpin1(D6);             // motor 1
 PwmOut pwmpin2(D5);             // motor 2
+PwmOut pwmpin3(D12);            // motor 3
 
 QEI encoder1 (D9, D8, NC, 8400, QEI::X4_ENCODING);
 QEI encoder2 (D11, D10, NC, 8400, QEI::X4_ENCODING); // motor 2
+QEI encoder3 (D3, D2, NC, 8400, QEI::X4_ENCODING);  // motor 3
 MODSERIAL pc(USBTX, USBRX);
 HIDScope scope(2);
 
@@ -219,6 +222,17 @@
     pwmpin1= fabs(u); //pwmduty cycle canonlybepositive, floatingpoint absolute value
 }
 
+void moter2_control(double u)
+{
+    directionpin2= u > 0.0f; //eithertrueor false
+    pwmpin2= fabs(u); //pwmduty cycle canonlybepositive, floatingpoint absolute value
+}
+
+void moter3_control(double u)
+{
+    directionpin3= u > 0.0f; //eithertrueor false
+    pwmpin3 = fabs(u); //pwmduty cycle canonlybepositive, floatingpoint absolute value
+}
 
 // CONTROLLING THE MOTOR
 void Motor_mover()
@@ -228,6 +242,20 @@
     double error = reference_rotation - motor_position*(2*PI)/8400;
     double u = PID_controller(error);
     moter_control(u);
+    
+    double motor_position2 = encoder2.getPulses(); //output in counts
+    double reference_rotation2 = hoek2(px, py);
+    double error_2 = reference_rotation2 - motor_position2*(2*PI)/8400;
+    double u_2 = PID_controller(error_2);
+    moter2_control(u_2);
+    
+    double motor_position3 = encoder3.getPulses(); //output in counts
+    double reference_rotation3 = hoek2(px, py);
+    double error_3 = reference_rotation3 - motor_position3*(2*PI)/8400;
+    double u_3 = PID_controller(error_3);
+    moter3_control(u_3);  
+    
+    
 }
 
 //PRINT TICKER
@@ -257,7 +285,7 @@
         
         // Tickers
         //show_counts.attach(PrintFlag, 0.2);
-        ref_rot.attach(Motor_mover, 0.1);
+        ref_rot.attach(Motor_mover, 0.01);
         //Scope_Data.attach(ScopeData, 0.01);