Diff: main.cpp

Revision:

41:dcfe6c86e3f5

Parent:

40:b26d19d52d40

Child:

42:ca4bbc3e0239

--- a/main.cpp	Tue Nov 05 14:27:57 2019 +0000
+++ b/main.cpp	Tue Nov 05 16:09:52 2019 +0000
@@ -3,12 +3,12 @@
 #include "QEI.h"
 #include "MODSERIAL.h"
 #include "BiQuad.h"
-//#include "FastPWM.h"
+#include "FastPWM.h"
 #define  M_PI 3.14159265358979323846  /* pi */
 #include <math.h>
-//#include "Servo.h"
+#include "Servo.h"
 #include <cmath>
-//#include <complex>
+#include <complex>
 
 Serial pc(USBTX, USBRX);
 
@@ -24,7 +24,7 @@
 DigitalIn Power_button_pressed(D1);                                                     // Definiëren van alle buttons, we gebruiken hiervoor geen InterruptIn, maar DigitalIn.
 DigitalIn Emergency_button_pressed(D2);
 DigitalIn Motor_calib_button_pressed(SW2);
-DigitalIn Homing_button_pressed(SW3);
+DigitalIn servo_button_pressed(SW3);
 
 AnalogIn EMG_biceps_right_raw (A0);                                                     // Definiëren van de ruwe EMG-signalen die binnenkomen via AnalogIn.
 AnalogIn EMG_biceps_left_raw (A1);                                                      // We gebruiken signalen van de kuit en de linker en rechter biceps.
@@ -40,6 +40,8 @@
 DigitalOut motor1_dir(D7);                                                              // Definities voor de richtingen van de motoren. Het getal 0 zorgt voor de ene richting, het getal 1 voor de andere.
 DigitalOut motor2_dir(D4);                                                              // In ons geval zijn beide motoren rechtsom draaiend vanaf de assen bekeken, wanneer de richting op 1 wordt gezet.
 
+Servo mijnServo(D7);                                                                    // Definitie voor de output naar de servo motor, benodigd voor het controleren van de spatelpositie.
+
 // VARIABELEN VOOR ENCODER, MOTORHOEK ETC.
 double counts1;                                                                         // Global variables definiëren voor het aantal counts dat uit de encoder komt en een begindefinitie voor
 double counts2;                                                                         // de offset opstellen.
@@ -161,6 +163,12 @@
 double abs_theta_t_1;                                                                   // Variabele opstellen voor de absolute waarde van theta_t.
 double abs_theta_t_2;
 
+// VARIABELEN VOOR OPERATION MODE (SERVO)
+double theta_f= 580;                                                                    // Dit zijn allemaal waarden tussen de 500 en 2400.
+double theta_sout;                                                                      // Dit zijn allemaal waarden tussen de 500 en 2400.
+double q1_ser;                                                                          // Dit zijn allemaal waarden tussen de 500 en 2400.
+double q2_ser;                                                                          // Dit zijn allemaal waarden tussen de 500 en 2400.
+
 // VOIDS
 
 void emergency()                                                                        // Noodfunctie waarbij de motoren uit worden gezet en de ProcessStateMachine wordt losgekoppeld
@@ -307,6 +315,40 @@
     Define_motor_dir();
 }
 
+void servo()
+{
+    q1_ser= q1*604.7887837;
+    q2_ser= q2*604.7887837;
+
+    if  (servo_button_pressed.read()== false) {
+        theta_sout=theta_f+q1_ser+q2_ser+400;
+        if (theta_sout>=2400) {
+            theta_sout=2400;
+        }
+        if (theta_sout<=500) {
+            theta_sout=500;
+        } else {
+            theta_sout;
+        }
+        mijnServo.SetPosition(theta_sout);
+        pc.printf("De servo staat op %f graden. in de klapstand\n\r", theta_sout);
+    }
+
+    else {
+        theta_sout=theta_f+q1_ser+q2_ser;
+        if (theta_sout>=2400) {
+            theta_sout=2400;
+        }
+        if (theta_sout<=500) {
+            theta_sout=500;
+        } else {
+            theta_sout;
+        }
+        mijnServo.SetPosition(theta_sout);
+        pc.printf("De servo staat op %f graden.\n\r", theta_sout);
+    }
+}
+
 // Aanmaken van een bool om te testen of de berekeningen in the ProcessStatemachine
 // meer tijd kosten dan wordt gegeven door de ticker. Dit zou mogelijk het encoder
 // probleem kunnen verklaren. Indien er te weinig tijd is, zou de loop zichzelf in moeten