begin van episch avontuur
Dependencies:
QEI
mbed
Diff: main.cpp
- Revision:
- 2:821ae727bf8c
- Parent:
- 1:b6a079ca16e0
- Child:
- 3:329acce2487c
--- a/main.cpp Fri Oct 27 12:08:44 2017 +0000
+++ b/main.cpp Tue Oct 31 12:16:16 2017 +0000
@@ -5,20 +5,23 @@
// Connecties
Serial pc(USBTX, USBRX); //Serial PC connectie
+DigitalOut led_g(LED_GREEN); //Groene led op k64f bord
DigitalOut motor1DirectionPin(D4); //Motorrichting op D4 (connected op het bord)
PwmOut motor1MagnitudePin(D5); //Motorkracht op D5 (connected op het bord)
-DigitalOut motor2DirectionPin(D6); //Motorrichting op D4 (connected op het bord)
-PwmOut motor2MagnitudePin(D7); //Motorkracht op D5 (connected op het bord)
+DigitalOut motor2DirectionPin(D7); //Motorrichting op D4 (connected op het bord)
+PwmOut motor2MagnitudePin(D6); //Motorkracht op D5 (connected op het bord)
QEI q1_enc(D13, D12, NC, 32); //encoder motor 1 instellen
QEI q2_enc(D11, D10, NC, 32); // encoder motor 2 instellen
const double pi = 3.1415926535897; // waarde voor pi aanmaken
// globale gegevens
Ticker tick_sample; // ticker voor aanroepen aansturing
+Ticker tick_wasd; //ticker voor toetsenbord aansturing
char key;
double ts=0.001;
int q1_puls;
int q2_puls;
+int n = 0;
// kinematica gegevens
// lengte armen
@@ -44,19 +47,24 @@
double ref2;
double PD1;
double PD2;
-double qset1=0;
-double qset1m=0;
-double qset2=0;
-double qset2m=0;
+double error1=0;
+double error1m=0;
+double error2=0;
+double error2m=0;
double Kp; // proportional coefficient (
double Kd; // differential coefficient
-void wasd(double& vx, double& vy)
+void wasd()
{
+ static char oldkey = 'p';
+ static double oldvx = 0;
+ static double oldvy = 0;
+
if(pc.readable()==true)
{ key = pc.getc();
+
if (key=='a')
{
vx = 0.04; //referentie snelheid m/s
@@ -77,53 +85,60 @@
vx = 0;
vy = -0.04;
}
- else
+ else
{
- vx=0;
- vy=0;
+ key = oldkey;
+ vx = oldvx;
+ vy = oldvy;
}
- }
- else
+
+ } //einde eerste if statemnet
+
+ else if (pc.readable()==false)
{
vx=0;
- vy=0;
+ vy=0;
+ key='p';
}
+oldkey = key;
+oldvx = vx;
+oldvy = vy;
+
}
-double kinematics(double& q1, double& q2, double q1_pos, double q2_pos, double vx, double vy )
+void kinematics()
{
- q1 = ((-(L3 + L2*cos(q1_pos))/(L1*L3*cos(q1_pos) + L1*L2*cos(q1_pos)*cos(q2_pos) + L1*L2*sin(q1_pos)*sin(q2_pos))) * vx + (-(L2*sin(q2_pos))/(L1*L3*cos(q1_pos) + L1*L2*cos(q1_pos)*cos(q2_pos) + L1*L2*sin(q1_pos)*sin(q2_pos))) * vy) * ts + q1_pos;
- q2 = (((L3 + L2*cos(q2_pos) - L1*sin(q1_pos))/(L1*L3*cos(q1_pos) + L1*L2*cos(q1_pos)*cos(q2_pos) + L1*L2*sin(q1_pos)*sin(q2_pos)) * vx) + ((L1*cos(q1_pos) + L2*sin(q2_pos))/(L1*L3*cos(q1_pos) + L1*L2*cos(q1_pos)*cos(q2_pos) + L1*L2*sin(q1_pos)*sin(q2_pos))) * vy) * ts + q2_pos;
- return 0;
+ q1 = ((-(L3 + L2*cos(q1 - q2))/(L1*L2*cos(2*q1 - q2) + L1*L3*cos(q1)))*vx + ((L2*sin(q1 - q2))/(L1*L2*cos(2*q1 - q2) + L1*L3*cos(q1)))*vy) * ts + q1_pos;
+ q2 = (((L3 - L1*sin(q1) + L2*cos(q1 - q2))/(L1*L2*cos(2*q1 - q2) + L1*L3*cos(q1))*vx) + ((L1*cos(q1) - L2*sin(q1 - q2))/(L1*L2*cos(2*q1 - q2) + L1*L3*cos(q1)))*vy) * ts + q2_pos;
}
-void controller(double qset1, double qset2)
+void controller(double q1ref, double q2ref)
{
// error = qset
// referentie bepalen
- ref1 = qset1 + q1_pos; // genereert het referentiesignaal
- ref2 = qset2 + q2_pos;
+ error1 = q1ref - q1_pos;
+ error2 = q2ref - q2_pos;
//PD
- PD1 = Kp*(qset1) + Kd*((qset1m-qset1)/ts) ; // PD sum; PD = proportianal + differential
- qset1m = qset1; // waarde voor qset wordt opgeslagen (m = memory)
- PD2 = Kp*(qset2) + Kd*((qset2m-qset2)/ts) ;
- qset2m = qset2;
+ PD1 = Kp*(error1) + Kd*((error1m-error1)/ts) ; // PD sum; PD = proportianal + differential
+ error1m = error1; // waarde voor qset wordt opgeslagen (m = memory)
+ PD2 = Kp*(error2) + Kd*((error2m-error2)/ts) ;
+ error2m = error2;
//Motor control
if (PD1<0 && PD2<0 )
{
motor1MagnitudePin = fabs(PD1);
- motor1DirectionPin = 1;
+ motor1DirectionPin = 0;
motor2MagnitudePin = fabs(PD2);
motor2DirectionPin = 1;
}
else if (PD1>0 && PD2<0)
{
motor1MagnitudePin = fabs(PD1);
- motor1DirectionPin = 0;
+ motor1DirectionPin = 1;
motor2MagnitudePin = fabs(PD2);
motor2DirectionPin = 1;
}
@@ -131,7 +146,7 @@
else if (PD1<0 && PD2>0)
{
motor1MagnitudePin = fabs(PD1);
- motor1DirectionPin = 1;
+ motor1DirectionPin = 0;
motor2MagnitudePin = fabs(PD2);
motor2DirectionPin = 0;
}
@@ -139,7 +154,7 @@
else if (PD1>0 && PD2>0)
{
motor1MagnitudePin = fabs(PD1);
- motor1DirectionPin = 0;
+ motor1DirectionPin = 1;
motor2MagnitudePin = fabs(PD2);
motor2DirectionPin = 0;
}
@@ -155,16 +170,20 @@
q1_pos = q1_puls*pulses2rad; // berekent positie q1 in radialen
q2_puls = q2_enc.getPulses();
q2_pos = q2_puls*pulses2rad; // berekent positie q2 in radialen
-
- // pijltjestoetsen
- wasd(vx, vy);
-
+
// kinematica bepaald gewenste q1 en q2 referenties afhankelijk van ingegeven vx en vy
- kinematics(q1, q2, q1_pos, q2_pos, vx, vy);
-
+ kinematics();
+
// PD controller gebruikt PD control en stuurt motor aan
controller(q1, q2);
-
+
+ if(n==500){
+ printf("motor1 = %f motor2 = %f\n\r", PD1, PD2);
+ n=0;
+ }
+ else{
+ n=n+1;
+ }
}
@@ -172,12 +191,15 @@
{
pc.baud(115200);
pulses2rad=(2*pi)/4200;
- Kp = 0.5/pi;
- Kd = 0.1*Kp;
- tick_sample.attach_us(&aansturing, 1000); //sample frequency 1 mHz;
-
+ Kp = 2.5;
+ Kd = 0.2*Kp;
+ tick_sample.attach_us(&aansturing, 1000); //sample frequency 1000 Hz;
+ tick_wasd.attach(&wasd, 0.1);
+ led_g = 0;
+
while(true)
{
+
}
Jorn-Jan van de Beld
