Biorobotica TIC
/
Practice_Run_1
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: QEI biquadFilter mbed
Fork of
Practice_Run
by 
Biorobotica TIC
Revision 0:2a4ed6c6cdc7, committed 2018-10-31
- Comitter:
- SilHeuvelink
- Date:
- Wed Oct 31 17:49:26 2018 +0000
- Child:
- 1:907507671a38
- Child:
- 2:a8ee608177ae
- Commit message:
- Lekker beunen werkt voor een halve meter
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/QEI.lib Wed Oct 31 17:49:26 2018 +0000 @@ -0,0 +1,1 @@ +http://os.mbed.com/users/aberk/code/QEI/#5c2ad81551aa
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/biquadFilter.lib Wed Oct 31 17:49:26 2018 +0000 @@ -0,0 +1,1 @@ +http://os.mbed.com/users/tomlankhorst/code/biquadFilter/#26861979d305
--- /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
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Wed Oct 31 17:49:26 2018 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/e95d10626187 \ No newline at end of file