Diff: main.cpp

Revision:

15:a90c450b1e0e

Parent:

14:92614abdb7e3

Child:

16:acf850a87e01

--- a/main.cpp	Tue Oct 06 12:56:33 2015 +0000
+++ b/main.cpp	Tue Oct 06 18:12:47 2015 +0000
@@ -4,24 +4,40 @@
 #include "HIDScope.h"
 
 // Serial pc(USBTX,USBRX);
-HIDScope scope(2);          // definieerd het aantal kanalen van de scope
+HIDScope scope(4);          // definieerd het aantal kanalen van de scope
+
+Ticker          controller1, controller2;        // definieer de ticker die controler1 doet
 
-Ticker          mod;        // definieer de ticker die alles bijhoud.
 
-//motor 1 gegevens
+//MOTOR INPUTPINS
+// motor 1
 PwmOut motor1_aan(D6);      // PWM signaal motor 2 (uit sheets)
 DigitalOut motor1_rich(D7); // digitaal signaal voor richting
-// einde motor 1
+
+// motor 2
+PwmOut motor2_aan(D5);
+DigitalOut motor2_rich(D4);
+// EINDE MOTOR
+
 
 // ENCODER
 Encoder motor1_enc(D12,D11);
+Encoder motor2_enc(D10,D9);
 
 //POTMETERS
 AnalogIn potright(A0);
+AnalogIn potleft(A1);
 
-double setpoint;
-double rads;
-const double K = 1 ;
+// RESETBUTTON
+DigitalIn   button(PTA4);
+int button_pressed = 0;
+
+
+// controller stuff
+double controlfreq = 10 ;    // controlloops frequentie (Hz)
+double controlstep = 1/controlfreq; // timestep derived from controlfreq
+const double K1 = 1 ;       // P constant motorcontrolle 1
+const double K2 = 1;        // p constant motorcontroller 2
 
 
 // counts 2 radians
@@ -46,14 +62,8 @@
 }
 
 // this function is a simple K control called by the motor function
-double K_control()
+double K_control(double error,double K)
 {
-    double setpoint = setpoint_f(potright.read());      // determine the setpoint that has been set by the inputsignal
-    scope.set(0,setpoint);
-    double rads = get_radians(motor1_enc.getPosition());    // determine the position of the motor
-    scope.set(1,rads);
-    scope.send();
-    double error = (setpoint - rads);                       // determine the error (reference - position)
     double output = K*error;                            // controller output K*e
     
     // Limit the output to to a number between -1 and 1 because 
@@ -79,23 +89,57 @@
 }
 
 // this function controls the input for one of the electric motors and is called by a ticker
+// MOTOR 1
+// // // // // // // // // //
 void motor1_control()
 {
-    double output = K_control();        // bereken de controller output (zie hierboven)
-    if(output > 0) {                    // 
+    double setpoint1 = setpoint_f(potright.read());      // determine the setpoint that has been set by the inputsignal
+    double rads1 = get_radians(motor1_enc.getPosition());    // determine the position of the motor
+    double error1 = (setpoint1 - rads1);                       // determine the error (reference - position)
+    scope.set(0,setpoint1);
+    scope.set(1,rads1);
+    scope.send();
+    double output1 = K_control(error1, K1);        // bereken de controller output voor motor 1(zie hierboven)
+    if(output1 > 0) {                    // 
         motor1_rich.write(0);
-        motor1_aan.write(output);
-    } else if(output < 0) {
+        motor1_aan.write(output1);
+    } else if(output1 < 0) {
         motor1_rich.write(1);
-        motor1_aan.write(abs(output));
+        motor1_aan.write(abs(output1));
+    }
+}
+
+// MOTOR 2
+// // // // // // //
+void motor2_control()
+{
+    double setpoint2 = setpoint_f(potleft.read());      // determine the setpoint that has been set by the inputsignal
+    double rads2 = get_radians(motor2_enc.getPosition());    // determine the position of the motor
+    double error2 = (setpoint2 - rads2);                       // determine the error (reference - position)
+    scope.set(3,setpoint2);
+    scope.set(4,rads2);
+    scope.send();
+    double output2 = K_control(error2, K2);        // bereken de controller output voor motor 1(zie hierboven)
+    if(output2 > 0) {                    // 
+        motor2_rich.write(0);
+        motor2_aan.write(output2);
+    } else if(output2 < 0) {
+        motor2_rich.write(1);
+        motor2_aan.write(abs(output2));
     }
 }
 
 
 int main()
 {
-    mod.attach(&motor1_control, 0.1);
+    controller1.attach(&motor1_control, controlstep);
+    controller2.attach(&motor2_control, controlstep);
     while(true) 
     {
+        if(button.read() == button_pressed)
+        {
+            motor1_enc.setPosition(0);
+            motor2_enc.setPosition(0);
+        }
     }
 }