Gerjan Wolterink
codevoor esther
Dependencies: Encoder MODSERIAL mbed
Revision 0:155294201f36, committed 2013-11-01
- Comitter:
- gerjan
- Date:
- Fri Nov 01 15:34:03 2013 +0000
- Child:
- 1:0db79ea80741
- Commit message:
- draait heel hard;
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Encoder.lib Fri Nov 01 15:34:03 2013 +0000 @@ -0,0 +1,1 @@ +https://mbed.org/users/vsluiter/code/Encoder/#2dd7853c911a
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MODSERIAL.lib Fri Nov 01 15:34:03 2013 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/Sissors/code/MODSERIAL/#b04ce87dc424
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Fri Nov 01 15:34:03 2013 +0000
@@ -0,0 +1,120 @@
+#include "mbed.h"
+#include "encoder.h"
+#include "MODSERIAL.h"
+
+// Maken van een looptimer.
+volatile bool looptimerflag;
+void setlooptimerflag(void)
+{
+ looptimerflag = true;
+}
+
+int main()
+{
+
+// Communicatie voor pc
+ MODSERIAL pc(USBTX,USBRX);
+ pc.baud(115200);
+
+// Voor aansturen motoren
+ double pi;
+ pi=3.14159265359;
+
+ double x,y;
+
+
+
+// Variabelen benoemen voor regelaar motor.
+ double theta, theta_pen, up_theta, kp_theta, kd_theta, dtheta, ei_theta, ui_theta, ki_theta, ed_theta, u_theta, ud_theta, theta_pwm;
+ double r, r_pen, up_r, kp_r, kd_r, dr, ei_r, ui_r, ki_r, ed_r, u_r, ud_r, r_pwm;
+ double motor1_maxu, motor2_maxu;
+
+ double Ts;
+
+// Sample tijd
+ Ts = 0.001;
+
+// Pinnen voor potmeter
+ AnalogIn potmeter1(PTB2);
+ AnalogIn potmeter2(PTB3);
+
+// Pinnen voor encoder
+ /* First pin should be PTDx or PTAx because those pins can be used as interruptIn */
+ Encoder motor1(PTD0,PTC9);
+ Encoder motor2(PTD2,PTC8);
+
+ /* PWM naar motor */
+ PwmOut pwm_motor1(PTA12);
+ PwmOut pwm_motor2(PTA5);
+
+ /* Pin voor richting */
+ DigitalOut motor1dir(PTD3);
+ DigitalOut motor2dir(PTD1);
+
+ //Tijd looptimer instellen.
+ Ticker looptimer;
+ looptimer.attach(setlooptimerflag,Ts);
+
+
+// Oneidige loop...
+ while(true) {
+ while(looptimerflag != true);
+ looptimerflag = false;
+
+ x = (potmeter1.read()*297.0+69.8);
+ y = (potmeter2.read()*210.0+69.8);
+
+
+ theta = atan(y/x) ;// * (400.0/(.5*pi));
+ r = (sqrt(x*x+y*y)) ;// * (2577/461.335);
+
+
+ theta_pen = motor1.getPosition() * ((.5*pi)/400);
+ r_pen = motor2.getPosition() * (363/2577);
+
+
+ dtheta = (theta - theta_pen);
+ dr = (r - r_pen);
+
+ dtheta =
+
+ theta_pwm = (dtheta)*0.001;
+ r_pwm = (dr)*0.001;
+
+ //NAAR MOTOR
+
+ //Zorgen dat pwm tussen -1 en 1 blijft.
+ if(theta_pwm > 1) {
+ theta_pwm=1;
+ }
+ if(theta_pwm < -1) {
+ theta_pwm=-1;
+ }
+ if(r_pwm > 1) {
+ r_pwm=1;
+ }
+ if(r_pwm < -1) {
+ r_pwm=-1;
+ }
+
+ // Bepaal richting waarin motoren moeten draaien
+ if(theta_pwm > 0)
+ motor1dir.write(1);
+ else
+ motor1dir.write(0);
+ if(r_pwm > 0)
+ motor2dir.write(1);
+ else
+ motor2dir.write(0);
+
+ // print naar pc
+ pc.printf("t=%.3f dt=%.3f tpwm=%.3f\n", theta, dtheta, theta_pwm);
+
+ //schrijf PWM naar motor
+ pwm_motor1.write(abs(theta_pwm));
+ pwm_motor2.write(abs(r_pwm));
+
+
+
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Fri Nov 01 15:34:03 2013 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/a9913a65894f \ No newline at end of file