Ramon Waninge
/
Milestone1
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: FastPWM mbed QEI biquadFilter HIDScope MODSERIAL
Diff: main.cpp
- Revision:
- 44:de6b4eac5cb7
- Parent:
- 43:e8f2193822b7
diff -r e8f2193822b7 -r de6b4eac5cb7 main.cpp
--- a/main.cpp Wed Oct 31 16:32:36 2018 +0000
+++ b/main.cpp Wed Oct 31 17:32:25 2018 +0000
@@ -43,17 +43,17 @@
Ticker encoders;
// Global variables
-const float pi = 3.14159265358979f;
-float u3 = 0.0f; // Normalised variable for the movement of motor 3
-const float fCountsRad = 4200.0f;
-const float dt = 0.001f;
+const double pi = 3.14159265358979;
+double u3 = 0.0; // Normalised variable for the movement of motor 3
+const double fCountsRad = 4200.0;
+const double dt = 0.001;
-float currentanglel;
-float errorl;
-float currentangler;
-float errorr;
-float currentanglef;
-float errorf;
+double currentanglel;
+double errorl;
+double currentangler;
+double errorr;
+double currentanglef;
+double errorf;
// Functions
void Emergency() // Emgergency, if SW2 on biorobotics is pressed, everything will instantly abort and a red light goes on
@@ -83,160 +83,160 @@
return countsf;
}
- float CurrentAngle(float counts) // Calculates the current angle of the motor (between -2*pi to 2*pi) based on the counts from the encoder
- { float angle = ((float)counts*2.0f*pi)/fCountsRad;
- while (angle > 2.0f*pi)
- { angle = angle-2.0f*pi;
+ double CurrentAngle(double counts) // Calculates the current angle of the motor (between -2*pi to 2*pi) based on the counts from the encoder
+ { double angle = ((double)counts*2.0*pi)/fCountsRad;
+ while (angle > 2.0*pi)
+ { angle = angle-2.0*pi;
}
- while (angle < -2.0f*pi)
- { angle = angle+2.0f*pi;
+ while (angle < -2.0*pi)
+ { angle = angle+2.0*pi;
}
return angle;
}
- float ErrorCalc(float refvalue,float CurAngle) // Calculates the error of the system, based on the current angle and the reference value
- { float error = refvalue - CurAngle;
+ double ErrorCalc(double refvalue,double CurAngle) // Calculates the error of the system, based on the current angle and the reference value
+ { double error = refvalue - CurAngle;
return error;
}
- float Kpcontr() // Sets the Kp value for the controller dependent on the scaled angle of potmeter 2
- { float Kp = 20.0f*pot2;
+ double Kpcontr() // Sets the Kp value for the controller dependent on the scaled angle of potmeter 2
+ { double Kp = 20.0*pot2;
return Kp;
}
- float Kdcontr() // Sets the Kd value for the controller dependent on the scaled angle of potmeter 1
- { float Kd = 0.25f*pot1;
+ double Kdcontr() // Sets the Kd value for the controller dependent on the scaled angle of potmeter 1
+ { double Kd = 0.25*pot1;
return Kd;
}
- float PIDcontrollerl(float refvalue,float CurAngle) // PID controller for the motors, based on the reference value and the current angle of the motor
- { //float Kp = Kpcontr();
- float Kp = 10.42f;
- float Ki = 1.02f;
- float Kd = 0.0493f;
- //float Kd = Kdcontr();
- float error = ErrorCalc(refvalue,CurAngle);
- static float error_integrall = 0.0;
- static float error_prevl = error; // initialization with this value only done once!
+ double PIDcontrollerl(double refvalue,double CurAngle) // PID controller for the motors, based on the reference value and the current angle of the motor
+ { //double Kp = Kpcontr();
+ double Kp = 10.42;
+ double Ki = 1.02;
+ double Kd = 0.0493;
+ //double Kd = Kdcontr();
+ double error = ErrorCalc(refvalue,CurAngle);
+ static double error_integrall = 0.0;
+ static double error_prevl = error; // initialization with this value only done once!
static BiQuad PIDfilterl(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
// Proportional part:
- float u_k = Kp * error;
+ double u_k = Kp * error;
// Integral part
error_integrall = error_integrall + error * dt;
- float u_i = Ki * error_integrall;
+ double u_i = Ki * error_integrall;
// Derivative part
- float error_derivative = (error - error_prevl)/dt;
- float filtered_error_derivative = PIDfilterl.step(error_derivative);
- float u_d = Kd * filtered_error_derivative;
+ double error_derivative = (error - error_prevl)/dt;
+ double filtered_error_derivative = PIDfilterl.step(error_derivative);
+ double u_d = Kd * filtered_error_derivative;
error_prevl = error;
// Sum all parts and return it
return u_k + u_i + u_d;
}
- float DesiredAnglel() // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
- { float angle = (pot1*2.0f*pi)-pi;
+ double DesiredAnglel() // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
+ { double angle = (pot1*2.0*pi)-pi;
return angle;
}
void turnl() // main function for movement of motor 1, all above functions with an extra tab are called
{
- //float refvaluel = pi/4.0f;
- float refvaluel = DesiredAnglel(); // different minus sign per motor
+ //double refvaluel = pi/4.0f;
+ double refvaluel = DesiredAnglel(); // different minus sign per motor
int countsl = Countslinput(); // different encoder pins per motor
currentanglel = CurrentAngle(countsl); // different minus sign per motor
- float PIDcontroll = PIDcontrollerl(refvaluel,currentanglel); // same for every motor
+ double PIDcontroll = PIDcontrollerl(refvaluel,currentanglel); // same for every motor
errorl = ErrorCalc(refvaluel,currentanglel); // same for every motor
pin6 = fabs(PIDcontroll); // different pins for every motor
- pin7 = PIDcontroll > 0.0f; // different pins for every motor
+ pin7 = PIDcontroll > 0.0; // different pins for every motor
}
- float PIDcontrollerr(float refvalue,float CurAngle) // PID controller for the motors, based on the reference value and the current angle of the motor
- { //float Kp = Kpcontr();
- float Kp = 10.42f;
- float Ki = 1.02f;
- float Kd = 0.0493f;
- //float Kd = Kdcontr();
- float error = ErrorCalc(refvalue,CurAngle);
- static float error_integralr = 0.0;
- static float error_prevr = error; // initialization with this value only done once!
+ double PIDcontrollerr(double refvalue,double CurAngle) // PID controller for the motors, based on the reference value and the current angle of the motor
+ { //double Kp = Kpcontr();
+ double Kp = 10.42;
+ double Ki = 1.02;
+ double Kd = 0.0493;
+ //double Kd = Kdcontr();
+ double error = ErrorCalc(refvalue,CurAngle);
+ static double error_integralr = 0.0;
+ static double error_prevr = error; // initialization with this value only done once!
static BiQuad PIDfilterr(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
// Proportional part:
- float u_k = Kp * error;
+ double u_k = Kp * error;
// Integral part
error_integralr = error_integralr + error * dt;
- float u_i = Ki * error_integralr;
+ double u_i = Ki * error_integralr;
// Derivative part
- float error_derivative = (error - error_prevr)/dt;
- float filtered_error_derivative = PIDfilterr.step(error_derivative);
- float u_d = Kd * filtered_error_derivative;
+ double error_derivative = (error - error_prevr)/dt;
+ double filtered_error_derivative = PIDfilterr.step(error_derivative);
+ double u_d = Kd * filtered_error_derivative;
error_prevr = error;
// Sum all parts and return it
return u_k + u_i + u_d;
}
- float DesiredAngler() // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
- { float angle = (pot2*2.0f*pi)-pi;
+ double DesiredAngler() // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
+ { double angle = (pot2*2.0*pi)-pi;
return angle;
}
void turnr() // main function for movement of motor 1, all above functions with an extra tab are called
{
- float refvaluer = pi/4.0f;
- //float refvaluer = DesiredAngler(); // different minus sign per motor
+ double refvaluer = pi/4.0;
+ //double refvaluer = DesiredAngler(); // different minus sign per motor
int countsr = Countsrinput(); // different encoder pins per motor
currentangler = CurrentAngle(countsr); // different minus sign per motor
- float PIDcontrolr = PIDcontrollerr(refvaluer,currentangler); // same for every motor
+ double PIDcontrolr = PIDcontrollerr(refvaluer,currentangler); // same for every motor
errorr = ErrorCalc(refvaluer,currentangler); // same for every motor
pin5 = fabs(PIDcontrolr); // different pins for every motor
- pin4 = PIDcontrolr > 0.0f; // different pins for every motor
+ pin4 = PIDcontrolr > 0.0; // different pins for every motor
}
- float PIDcontrollerf(float refvalue,float CurAngle) // PID controller for the motors, based on the reference value and the current angle of the motor
- { //float Kp = Kpcontr();
- float Kp = 10.42f;
- float Ki = 1.02f;
- float Kd = 0.0493f;
- //float Kd = Kdcontr();
- float error = ErrorCalc(refvalue,CurAngle);
- static float error_integralf = 0.0;
- static float error_prevf = error; // initialization with this value only done once!
+ double PIDcontrollerf(double refvalue,double CurAngle) // PID controller for the motors, based on the reference value and the current angle of the motor
+ { //double Kp = Kpcontr();
+ double Kp = 10.42;
+ double Ki = 1.02;
+ double Kd = 0.0493;
+ //double Kd = Kdcontr();
+ double error = ErrorCalc(refvalue,CurAngle);
+ static double error_integralf = 0.0;
+ static double error_prevf = error; // initialization with this value only done once!
static BiQuad PIDfilterf(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
// Proportional part:
- float u_k = Kp * error;
+ double u_k = Kp * error;
// Integral part
error_integralf = error_integralf + error * dt;
- float u_i = Ki * error_integralf;
+ double u_i = Ki * error_integralf;
// Derivative part
- float error_derivative = (error - error_prevf)/dt;
- float filtered_error_derivative = PIDfilterf.step(error_derivative);
- float u_d = Kd * filtered_error_derivative;
+ double error_derivative = (error - error_prevf)/dt;
+ double filtered_error_derivative = PIDfilterf.step(error_derivative);
+ double u_d = Kd * filtered_error_derivative;
error_prevf = error;
// Sum all parts and return it
return u_k + u_i + u_d;
}
- float DesiredAnglef() // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
- { float angle = (pot2*2.0f*pi)-pi;
+ double DesiredAnglef() // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
+ { double angle = (pot2*2.0*pi)-pi;
return angle;
}
void turnf() // main function for movement of motor 1, all above functions with an extra tab are called
{
- //float refvaluef = pi/4.0f;
- float refvaluef = DesiredAnglef(); // different minus sign per motor
+ //double refvaluef = pi/4.0f;
+ double refvaluef = DesiredAnglef(); // different minus sign per motor
int countsf = Countsfinput(); // different encoder pins per motor
currentanglef = CurrentAngle(countsf); // different minus sign per motor
- float PIDcontrolf = PIDcontrollerf(refvaluef,currentanglef); // same for every motor
+ double PIDcontrolf = PIDcontrollerf(refvaluef,currentanglef); // same for every motor
errorf = ErrorCalc(refvaluef,currentanglef); // same for every motor
pin3 = fabs(PIDcontrolf); // different pins for every motor
- pin2 = PIDcontrolf > 0.0f; // different pins for every motor
+ pin2 = PIDcontrolf > 0.0; // different pins for every motor
}
-float ActualPosition(int counts, int offsetcounts) // After calibration, this function is used to return the counts (and thus the angle of the system) to 0
-{ float MotorPosition = - (counts - offsetcounts) / fCountsRad;
+double ActualPosition(int counts, int offsetcounts) // After calibration, this function is used to return the counts (and thus the angle of the system) to 0
+{ double MotorPosition = - (counts - offsetcounts) / fCountsRad;
// minus sign to correct for direction convention
return MotorPosition;
}
@@ -255,7 +255,7 @@
while (true)
{
- pc.printf("angle l/r/f: \t %f \t\t %f \t\t %f \t\t error l/r/f: \t %f \t\t %f \t\t %f\r\n",currentanglel,currentangler,currentanglef,errorl,errorr,errorf);
- wait(0.1f);
+ pc.printf("angle l/r/d: \t %d \t\t %d \t\t %d \t\t error l/r/f: \t %d \t\t %d \t\t %d\r\n",currentanglel,currentangler,currentanglef,errorl,errorr,errorf);
+ wait(0.1);
}
}
