Diff: main.cpp

Revision:

0:8f027052f23a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Oct 30 11:39:57 2018 +0000
@@ -0,0 +1,194 @@
+#include "mbed.h"
+
+// Load all the necessary pins, buttons, etc.
+
+// Load all the EMGs
+AnalogIn EMG1; // Move X
+AnalogIn EMG2; // Move Y
+AnalogIn EMG3; // Change direction
+
+// Initiate motor 1
+PwmOut pwmpin1(D6);
+DigitalOut directionpin1(D7);
+InterruptIn motor1A(D12);   // Encoder 1a
+InterruptIn motor1B(D13);   // Encoder 1b
+QEI         Encoder1(D12,D13,NC,64);    // Note that we use X2 encoding, so there are 4200 counts/rotation, as opposed to 8400 from the documentation.
+
+// Initiate motor 2
+DigitalOut directionpin2(D4);
+PwmOut pwmpin2(D5);
+InterruptIn motor2A(D10);   // Encoder 1a
+InterruptIn motor2B(D11);   // Encoder 1b
+QEI         Encoder2(D10,D11,NC,64);    // Note that we use X2 encoding, so there are 4200 counts/rotation, as opposed to 8400 from the documentation.
+
+// -------------------------------------------------------------------------- \\
+
+double FilterEMG1(EMG1){
+    filter emg1
+    return filteredemg1
+}
+
+double FilterEMG2(EMG2){
+    filter emg2
+    return filteredemg2
+}
+
+double FilterEMG3(EMG3){
+    filter emg3
+    return filteredemg3
+}
+
+// ----------------------Read & relate the emgs --------------------------- \\
+
+// We want to know whether we should move X or Y
+bool DirBool = 1; // initialise with moving X
+void ChangeDir(emg3 lezen){
+    // Read EMG 3
+    if(voldoet emg3 aan de voorwaarde? verander dan de x/y bool.){
+        DirBool = !DirBool;
+        return DirBool;
+}
+
+// Start moving in the x-direction
+void MoveX(bool DirBool, EMG1){
+    // first, we check whether or not we should even move in the x-direction
+    if(DirBool == 1){ // We can move in the x-direction if this is true
+ // we make a list of 2 long, checking emg 1 and 2
+ *list*
+ check whether emg 1 is active or not
+ if(emg1 voldoet aan statement){
+     request a change in motor angle // could be done by saying motorangle = motorangle - something we want, check convention!!!
+     }
+     if(emg2 voldoet aan statement){
+         request a change in motor angle // could be done by saying motorangle = motorangle - something we want, check convention!!!
+         }
+         return de angle die we willen veranderen voor de motoren
+ }
+
+// When we should move, we should relate this change in x-direction to a change 
+// in the motor angles, this comes from the inverse kinematics.
+ 
+ void MoveY(bool DirBool, EMG1){
+    // first, we check whether or not we should even move in the y-direction
+    if(DirBool == 0){ // We can move in the y-direction if this is true
+ // we make a list of 2 long, checking emg 1 and 2
+ *list*
+ check whether emg 1 is active or not
+ if(emg1 voldoet aan statement){
+     request a change in motor angle // could be done by saying motorangle = motorangle - something we want, check convention!!!
+     }
+     if(emg2 voldoet aan statement){
+         request a change in motor angle // could be done by saying motorangle = motorangle - something we want, check convention!!!
+         }
+         return de angle die we willen veranderen voor de motoren
+ }
+ 
+// When we should move, we should relate this change in y-direction to a change 
+// in the motor angles, this comes from the inverse kinematics.
+
+// ----------Set the desired motor positions & get the actual ones---------- \\
+
+// Get position motor 1 should be in
+double GetReferencePosition1(){
+    return ReferencePosition1;
+}
+
+// Get position motor 1 actually is in
+double GetActualPosition1(){
+    double ActualPosition = Encoder1.getPulses(); // Gets the actual amount of counts from motor 1. Note the minus sign for convention (CCW = positive counts.)
+    return ActualPosition1;
+}
+
+// Get position motor 2 should be in
+double GetReferencePosition2(){
+    return ReferencePosition2;
+}
+
+// Get position motor 2 actually is in
+double GetActualPosition2(){
+    double ActualPosition2 = Encoder2.getPulses(); // Gets the actual amount of counts from motor 1. Note the minus sign for convention (CCW = positive counts.)
+    return ActualPosition2;
+}
+
+// PID controller for motor 1
+double PID_controller1(double error){
+    static double error_integral = 0;
+    static double error_prev = error; // initialization with this value only done once!
+    static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
+    double Ts = 0.01;
+    
+    // Proportional part
+        
+        double Kp = 0.1;
+        double u_k = Kp*error;
+
+    // Integral part
+        double Ki = 0.1;
+        error_integral = error_integral + error * Ts;
+        double u_i = Ki * error_integral;
+    
+    // Derivative part
+        double Kd = 10.1;
+        double error_derivative = (error - error_prev)/Ts;
+        double filtered_error_derivative = LowPassFilter.step(error_derivative);
+        double u_d = Kd * filtered_error_derivative;
+        error_prev = error;
+    // Sum all parts and return it
+    return u_k + u_i + u_d;
+}
+
+// PID controller for motor 2
+double PID_controller2(double error){
+    static double error_integral = 0;
+    static double error_prev = error; // initialization with this value only done once!
+    static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
+    double Ts = 0.01;
+    
+    // Proportional part
+        
+        double Kp = 0.1;
+        double u_k = Kp*error;
+
+    // Integral part
+        double Ki = 0.1;
+        error_integral = error_integral + error * Ts;
+        double u_i = Ki * error_integral;
+    
+    // Derivative part
+        double Kd = 10.1;
+        double error_derivative = (error - error_prev)/Ts;
+        double filtered_error_derivative = LowPassFilter.step(error_derivative);
+        double u_d = Kd * filtered_error_derivative;
+        error_prev = error;
+    // Sum all parts and return it
+    return u_k + u_i + u_d;
+}
+
+void SetMotor1(double motorValue){
+    // Given -1<=motorValue<=1, this sets the PWM and direction
+    // bits for motor 1. Positive value makes motor rotating
+    // ccw. motorValues outside range are truncated to
+    // within range
+    if (motorValue >=0) directionpin1=1;
+        else directionpin1=0;
+    if (fabs(motorValue)>1) pwmpin1 = 1;
+        else pwmpin1 = fabs(motorValue);
+}
+
+void SetMotor2(double motorValue){
+    // Given -1<=motorValue<=1, this sets the PWM and direction
+    // bits for motor 1. Positive value makes motor rotating
+    // ccw. motorValues outside range are truncated to
+    // within range
+    if (motorValue >=0) directionpin2=1;
+        else directionpin2=0;
+    if (fabs(motorValue)>1) pwmpin2 = 1;
+        else pwmpin2 = fabs(motorValue);
+}
+
+int main()
+{
+    while (true) {
+
+    }
+}
\ No newline at end of file