Gerjan Wolterink
/
Pregelaar
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: Encoder MODSERIAL mbed
Revision 5:8f3530006712, committed 2013-11-04
- Comitter:
- gerjan
- Date:
- Mon Nov 04 18:52:10 2013 +0000
- Parent:
- 4:863a52425322
- Child:
- 6:bea0424b407c
- Commit message:
- prepare i;
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Mon Nov 04 16:29:51 2013 +0000
+++ b/main.cpp Mon Nov 04 18:52:10 2013 +0000
@@ -26,7 +26,7 @@
// 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, inputsinus;
+ 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, dr_1, inputsinus;
double motor1_maxu, motor2_maxu;
double Ts;
@@ -58,6 +58,9 @@
motor2.setPosition(0);
x=0;
y=0;
+
+ kp_r =
+ ki_r =
@@ -73,7 +76,8 @@
y = 0;
inputsinus = inputsinus + Ts*pi;
- theta = 0.25*pi;
+
+ theta = atan(y/x)+0.25*pi;
r = (sqrt(x*x+y*y)) ;// * (2577/461.335);
@@ -83,9 +87,14 @@
dtheta = (theta - theta_pen);
dr = (r - r_pen);
+
+ //REGELAAR
+ up_r = kp_r * dr;
+ ui_r = (dr_1 + (dr * Ts)) * ki_r;
+ utot_r = up_r + ui_r;
- theta_pwm = (dtheta)*3;
- r_pwm = (dr)*0.0015;
+ theta_pwm = (dtheta)*3.0;
+ r_pwm = (utot_r/1.0);
//NAAR MOTOR