Diff: main.cpp

Revision:

0:2a4ed6c6cdc7

Child:

1:907507671a38

Child:

2:a8ee608177ae

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Oct 31 17:49:26 2018 +0000
@@ -0,0 +1,86 @@
+ #include "mbed.h"
+#include "math.h"
+#include "BiQuad.h"
+#include <string>
+#include "QEI.h"
+
+//----------------- INITIAL -------------------------
+QEI Encoder1(D12,D13,NC,64,QEI::X2_ENCODING);
+QEI Encoder2(D2,D3,NC,64,QEI::X2_ENCODING);
+Ticker EncoderTicker;
+
+DigitalOut motor1direction(D7);
+PwmOut motor1control(D6);
+ 
+DigitalOut motor2direction(D4);
+PwmOut motor2control(D5);
+
+InterruptIn button1(D10);
+
+Serial pc(USBTX, USBRX);
+
+// Definitie constanten
+double L0 = 0.09;
+double K_p1 = 0.02;
+double K_p2 = 0.20;
+double p_desired_x = 0.2;
+double p_desired_y = 0.2;
+double r_pulley = 0.015915;
+double pi = 3.141592653589793;
+double gearratio = 3.857142857;
+
+// Definitie variabelen
+double angle_trans;
+double translatie;
+double angle;
+double length;
+double angle_desired;
+double length_desired;
+double motor1_pwm;
+double length_dot;
+double motor2_pwm;
+
+double p_current_x;
+double p_current_y;
+
+void EncoderFunc() 
+{
+    angle_trans = Encoder1.getPulses() * 0.0857142857*0.0174532925;                  // Translation [rad]
+    translatie = angle_trans * r_pulley;           // Translatie arm [m]
+    angle = Encoder2.getPulses() * 0.0857142857*0.0174532925/gearratio;                        // Angle arm [rad]
+    length = translatie+L0;
+
+    p_current_x = (length)*cos(angle);
+    p_current_y = (length)*sin(angle);
+    
+    //p_dot_x = v_constant*(p_desired_x - p_current_x);
+    //p_dot_y = v_constant*(p_desired_y - p_current_y);
+    
+    angle_desired = atan2(p_desired_y,p_desired_x);
+    length_desired = sqrt(pow(p_desired_x,2)+pow(p_desired_y,2));
+
+    motor1_pwm = K_p1*(length_desired-length)/r_pulley;
+    motor2_pwm = K_p2*(angle_desired-angle);
+        
+    motor1control.write(fabs(motor1_pwm));
+    motor2control.write(fabs(motor2_pwm));
+    //motor1direction = motor1_pwm < 0;
+    //motor2direction = motor2_pwm < 0;
+ }
+
+
+int main()
+{
+EncoderTicker.attach(&EncoderFunc, 0.02);
+pc.baud(115200);
+motor1direction = false;
+motor2direction = false;
+
+while(true)
+    {
+    wait(0.1);
+    pc.printf("angle = %f, length = %f \r\n", angle, length);
+    pc.printf("x = %f, y = %f \r\n", p_current_x, p_current_y);
+    pc.printf("motor1_pwm = %f, motor2_pwm = %f \r\n", motor1_pwm, motor2_pwm);
+    }
+}
\ No newline at end of file