Arnoud Domhof
Laatste versie van Arnoud, Data wordt geplot
Dependencies: FastPWM MODSERIAL QEI mbed
Diff: main_demo.cpp
- Revision:
- 8:088904ebb85b
- Parent:
- 7:2e0cdf836495
--- a/main_demo.cpp Thu Nov 01 15:54:44 2018 +0000
+++ b/main_demo.cpp Thu Nov 01 17:02:30 2018 +0000
@@ -33,6 +33,10 @@
const double pi = 3.14159265359;
volatile double error1;
volatile double error2;
+volatile double error1_final = 10.0;
+volatile double error2_final = 10.0;
+volatile double q1_diff_final;
+volatile double q2_diff_final;
double point1x = 200.0;
double point1y = 200.0;
double point2x = 350.0;
@@ -48,11 +52,13 @@
volatile double setpointy = y0;
volatile double U1;
volatile double U2;
+volatile double xfinal;
+volatile double yfinal;
// Determine demo setpoints
const double stepsize1 = 0.15;
const double stepsize2 = 0.25;
-const double setpoint_error = 0.3;
+const double setpoint_error = 0.03;
// Inverse Kinematics
volatile double q1_diff;
@@ -67,10 +73,6 @@
double q2_0_enc = q2_0 - q1_0;
// Controller
-double potwaarde1;
-double pot1;
-double potwaarde2;
-double pot2;
volatile double Kp1 = 20.0; // Kp (proportionele controller, nu nog een random waarde)
volatile double Kp2 = 20.0; // Kp (proportionele controller, nu nog een random waarde)
volatile double Ki1 = 6.0; // Ki (I-actie van de controller)
@@ -99,124 +101,90 @@
// -------------------------------------------------------------------------
// -------------- Determine Setpoints --------------------------------------
// -------------------------------------------------------------------------
-double determinedemosetx(double setpointx, double setpointy)
+void determinedemoset()
{
-
- if (setpointx < point1x && track == 1){
- setpointx = setpointx + stepsize1;
+ if (track == 1){
+ xfinal = point1x;
+ yfinal = point1y;
}
+
+ if ((setpointx < point1x) && (track == 1)){
+ setpointx = setpointx + stepsize2;
+ }
+ if ((setpointy < point1y) && (track == 1)){
+ setpointy = setpointy + stepsize1;
+ }
// Van punt 1 naar punt 2.
- if (fabs(setpointx - point1x) <= setpoint_error && fabs(setpointy - point1y) <= setpoint_error && (track == 1 || track == 41)) {
+ if ((error1_final <= setpoint_error) && (error2_final <= setpoint_error) && (track == 1)){
+ //setpointx = point2x;
+ //setpointy = setpointy + (-0.2); // Van punt 1 naar punt 2 op dezelfde y blijven.
+ ledr = !ledr; // Aangeven met een ROOD lampje dat hij op de plaats van bestemming is
track = 12;
}
- if (setpointx < point2x && track == 12){
- setpointx = setpointx + stepsize2;
- }
-
- // Van punt 2 naar punt 3.
- if (fabs(setpointx - point2x) <= setpoint_error && fabs(setpointy - point2y) <= setpoint_error && track == 12)
- {
- setpointx = point3x;
- track = 23;
+ if (track == 12){
+ xfinal = point2x;
+ yfinal = point2y;
}
- if (setpointy > point3y && track == 23)
- {
- setpointx = point3x; // Van punt 1 naar punt 2 op dezelfde x blijven.
- }
-
-
- // Van punt 3 naar punt 4.
- if ((fabs(setpointx - point3x) <= setpoint_error) && (fabs(setpointy - point3y) <= setpoint_error) && (track == 23))
- {
- setpointy = point4y;
- track = 34;
- }
-
- if (setpointx > point4x && track == 34)
- {
- setpointx = setpointx - stepsize2;
- }
-
- // Van punt 4 naar punt 1.
- if ((fabs(setpointx - point4x) <= setpoint_error) && (fabs(setpointy - point4y) <= setpoint_error) && (track == 34))
- {
- setpointx = point4x;
- track = 41;
+ if ((setpointy > point2y) && (track == 12)) {
+ setpointx = point2x;
+ //setpointx = setpointx + 0.2;
+ setpointy = setpointy + (-stepsize2);
}
- if (setpointy < point1y && track == 41)
- {
- setpointx = point4x; // Van punt 4 naar punt 2 op dezelfde x blijven.
+ if ((error1_final <= setpoint_error) && (error2_final <= setpoint_error) && (track == 12)){
+ ledr = !ledr;
+ ledg = !ledg;
+ track = 23;
+ }
+
+
+ /**
+ if (setpointy > point2y && track == 12){
+ setpointx = point2x;
+ setpointy = setpointy + (-0.2);
}
-
- return setpointx;
-}
-
-double determinedemosety(double setpointx, double setpointy)
-{
- // Van reference positie naar punt 1.
- if(setpointy < point1y && track == 1){
- setpointy = setpointy + (stepsize2);
- }
-
- // Van punt 1 naar punt 2.
- if (fabs(setpointx - point1x) <= setpoint_error && fabs(setpointy - point1y) <= setpoint_error && (track == 1 || track == 41)){
- ledg = 1; ledr = 0; ledb = 1; // Turns the Led RED
- setpointy = point2y; // Van punt 1 naar punt 2 op dezelfde y blijven.
- track = 12;
- }
- if (setpointx < point2x && track == 12){
- setpointy = point2y;
- }
// Van punt 2 naar punt 3.
- if (fabs(setpointx - point2x) <= setpoint_error && fabs(setpointy - point2y) <= setpoint_error && (track == 12)){
- ledr = 1; ledg = 0; ledb = 1; // Turns the Led GREEN
- setpointx = point3x;
+ if (fabs(setpointx - point2x) <= 0.3 && fabs(setpointy - point2y) <= 0.3)
+ {
+ //setpointx = setpointx - 0.2;
+ //setpointy = setpointy;
+ ledr = 1;
+ ledg = 0;
track = 23;
}
- if ((setpointy > point3y) && (track == 23))
+ if (setpointy > point3y && track == 23)
{
- setpointy = setpointy + (-stepsize2);
+ //setpointx = setpointx - 0.2; // Van punt 1 naar punt 2 op dezelfde y blijven.
+ //setpointy = setpointy;
track = 23;
- }
+ }
+
// Van punt 3 naar punt 4.
- if ((fabs(setpointx - point3x) <= setpoint_error) && (fabs(setpointy - point3y) <= setpoint_error) && (track == 23))
+ if (setpointy >= point3y - 0.3 && setpointx >= point3x - 0.3 && setpointy <= point3y + 0.3 && setpointx <= point3x + 0.3)
{
- ledg = 1; ledr = 1; ledb = 0; // Turns the Led BLUE
+ setpointx = setpointx - 0.1; // Van punt 1 naar punt 2 op dezelfde y blijven.
setpointy = setpointy;
track = 34;
}
- if (setpointx > point4x && track == 34)
+ if (setpointy > point3y && track == 34)
{
+ setpointx = setpointx - 0.1;
setpointy = setpointy;
- }
-
- // Van punt 4 naar punt 1.
- if ((fabs(setpointx - point4x) <= setpoint_error) && (fabs(setpointy - point4y) <= setpoint_error) && (track == 34))
- {
- ledg = 0; ledr = 0; ledb = 0; // Turns the LED WHITE
- track = 41;
+
}
-
- if (setpointy < point1y && track == 41)
- {
- setpointy = setpointy + (stepsize2); // Van punt 4 naar punt 2 op dezelfde x blijven.
- }
+ */
- return setpointy;
-
-}
-
+}
// -----------------------------------------------------------------
// --------------------------- PI controllers ----------------------
// -----------------------------------------------------------------
-double PI_controller1(double error1)
+double PID_controller1(double error1)
{
static double error_integral1 = 0;
static double error_prev1 = error1; // initialization with this value only done once!
@@ -241,7 +209,7 @@
// Return
return U1;
}
-double PI_controller2(double error2)
+double PID_controller2(double error2)
{
static double error_integral2 = 0;
static double error_prev2 = error2; // initialization with this value only done once!
@@ -289,18 +257,23 @@
// -----------------------------------------------
void motoraansturing()
{
- setpointx = determinedemosetx(setpointx, setpointy);
- setpointy = determinedemosety(setpointx, setpointy);
+ determinedemoset();
q1_diff = makeAngleq1(setpointx, setpointy);
q2_diff = makeAngleq2(setpointx, setpointy);
+ q1_diff_final = makeAngleq1(xfinal, yfinal);
+ q2_diff_final = makeAngleq2(xfinal, yfinal);
theta2 = counts2angle2();
error2 = q2_diff - theta2;
theta1 = counts2angle1();
error1 = q1_diff - theta1; // Setpoint error, te behalen setpoint minus de huidige positie van de as.
- U1 = PI_controller1(error1); // Voltage dat naar de motor gestuurd wordt.
- U2 = PI_controller2(error2);
+ //errors die de setpoint bepalen
+ error1_final = q1_diff_final - theta1;
+ error2_final = q2_diff_final - theta2;
+
+ U1 = PID_controller1(error1); // Voltage dat naar de motor gestuurd wordt.
+ U2 = PID_controller2(error2);
motor1_pwm.write(fabs(U1)); // Motor aansturen
directionM1 = U1 > 0.0f; // Richting van de motor bepalen
@@ -330,13 +303,14 @@
while (true) {
pc.printf("Setpointx: %0.2f, Setpointy: %0.2f, q1_diff: %0.2f, q2_diff: %0.2f, error1: %0.2f, error2: %0.2f, U1: %0.2f, U2: %0.2f\r\n", setpointx,setpointy,q1_diff,q2_diff,error1,error2,U1,U2);
- /**
+
if (track == 1) {
pc.printf("Gaat van home naar positie 1\r\n");
}
else if (track == 12) {
pc.printf("Gaat naar positie 2\r\n");
}
+ /**
else if (track == 23) {
pc.printf("Gaat naar positie 3\r\n");
}
@@ -348,6 +322,6 @@
}
*/
- wait(0.1f);
+ wait(1.0f);
}
}
\ No newline at end of file