Jorn-Jan van de Beld
/
PI_standaardsignalen
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.
Revision 5:949c2861a79b, committed 2017-11-02
- Comitter:
- JornJan
- Date:
- Thu Nov 02 18:40:41 2017 +0000
- Parent:
- 4:e98ad06f227c
- Commit message:
- pittige avondsessie;
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Thu Nov 02 16:06:05 2017 +0000
+++ b/main.cpp Thu Nov 02 18:40:41 2017 +0000
@@ -15,7 +15,6 @@
Timer t;
Ticker aansturing;
Ticker stepres;
-Ticker kinmat;
double ref1 = 0;
double refrad1;
@@ -33,6 +32,8 @@
double PI1;
int n;
+double stap = 0.01;
+
double error1_1;
double error2_1 = 0;
double error_I_1;
@@ -56,8 +57,8 @@
double q2_pos;
// EMG Input_k
-double vx;
-double vy;
+double vx = 0;
+double vy = 0;
void kinematica()
@@ -68,19 +69,27 @@
q2_puls = q2_enc.getPulses();
q2_pos = q2_puls*pulses2rad; // berekent positie q2 in radialen
- vx = oldvx + stap;
- vy = oldvy + stap
+
+ px = oldpx + stap;
+ py = oldpy + stap;
+
+ if (px >= pxmax || px <= pxmin)
+ {
+ vx = oldvx;
+ }
+ else if (py >= pymax || py <= pymin)
+ {
+ vy = oldvy;
+ }
q1 =
q2 =
+
+ ref1 = q1 * rad2pulses;
+ ref2 = q2 * rad2pulses;
- //q1 = ((-(L3 + L2*cos(q1_pos - q2_pos))/(L1*L2*cos(2*q1_pos - q2_pos) + L1*L3*cos(q1_pos)))*vx + ((L2*sin(q1_pos - q2_pos))/(L1*L2*cos(2*q1_pos - q2_pos) + L1*L3*cos(q1_pos)))*vy) * 5 + q1_pos;
- //q2 = (((L3 - L1*sin(q1_pos) + L2*cos(q1_pos - q2_pos))/(L1*L2*cos(2*q1_pos - q2_pos) + L1*L3*cos(q1_pos))*vx) + ((L1*cos(q1_pos) - L2*sin(q1_pos - q2_pos))/(L1*L2*cos(2*q1_pos - q2_pos) + L1*L3*cos(q1_pos)))*vy) * 5 + q2_pos;
-
- //ref1 = -q1*rad2pulses;
- ref2 = q2*rad2pulses;
- //ref2 =200;
-
+ px = oldpx;
+ py = oldpy;
}
@@ -109,12 +118,12 @@
if (PI1<=0)
{
motor1DirectionPin = 1;
- //motor1MagnitudePin = fabs(PI1);
+ motor1MagnitudePin = fabs(PI1);
}
else if (PI1>0)
{
motor1DirectionPin = 0;
- //motor1MagnitudePin = fabs(PI1);
+ motor1MagnitudePin = fabs(PI1);
}
if (PI2>=0)
@@ -144,7 +153,6 @@
pc.baud(115200);
t.start();
aansturing.attach_us(&controller, 1000);
- //kinmat.attach(&kinematica, 0.001);
while(true){
