Revision 50:522bb6eebda6, committed 2018-11-01
- Comitter:
- efvanmarrewijk
- Date:
- Thu Nov 01 08:46:58 2018 +0000
- Parent:
- 49:48363ca21a15
- Commit message:
- Latest version just before merge, without calibration code and with code for movement of 3 motors, 3rd motor is TURNED OFF (after calibration it can be turned on again)
Changed in this revision
diff -r 48363ca21a15 -r 522bb6eebda6 main.cpp
--- a/main.cpp Wed Oct 31 20:55:06 2018 +0000
+++ b/main.cpp Thu Nov 01 08:46:58 2018 +0000
@@ -43,7 +43,7 @@
Ticker encoders;
// Global variables
-const float pi = 3.14159265358979f;
+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.002f;
@@ -85,13 +85,13 @@
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;
+ 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;
}
- while (angle < -2.0f*pi)
- { angle = angle+2.0f*pi;
+ while (angle < -2.0f*PI)
+ { angle = angle+2.0f*PI;
}
return angle;
}
@@ -112,11 +112,11 @@
}
float PIDcontrollerl(float refvalue,float CurAngle) // PID controller for the motors, based on the reference value and the current angle of the motor
- { Kp = Kpcontr();
- //float Kp = 10.42f;
+ { //Kp = Kpcontr();
+ float Kp = 19.24f;
float Ki = 1.02f;
- //float Kd = 0.0493f;
- Kd = Kdcontr();
+ float Kd = 0.827f;
+ //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!
@@ -136,29 +136,29 @@
}
float DesiredAnglel() // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
- { float angle = (pot1*2.0f*pi)-pi;
+ { float angle = (pot1*2.0f*PI)-PI;
return angle;
}
void turnl() // main function for movement of motor 1, all above functions with an extra tab are called
{
- float refvaluel = 0.5f*pi;
- //float 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
- errorl = ErrorCalc(refvaluel,currentanglel); // same for every motor
+ //float refvaluel = 0.5f*PI;
+ float 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
+ 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
+ pin6 = fabs(PIDcontroll); // different pins for every motor
+ pin7 = PIDcontroll > 0.0f; // 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();
+ { //Kp = Kpcontr();
+ float Kp = 19.24f;
+ float Ki = 1.02f;
+ float Kd = 0.827f;
+ //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!
@@ -178,29 +178,29 @@
}
float DesiredAngler() // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
- { float angle = (pot2*2.0f*pi)-pi;
+ { float angle = (pot2*2.0f*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
- 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
- errorr = ErrorCalc(refvaluer,currentangler); // same for every motor
+ //float refvaluer = PI/4.0f;
+ float refvaluer = -DesiredAngler(); // DONT CHANGE THIS MINUS SIGN: 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
+ 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
+ pin5 = fabs(PIDcontrolr); // different pins for every motor
+ pin4 = PIDcontrolr > 0.0f; // 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();
+ { //Kp = Kpcontr();
+ float Kp = 19.24f;
+ float Ki = 1.02f;
+ float Kd = 0.827f;
+ //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!
@@ -220,27 +220,21 @@
}
float DesiredAnglef() // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
- { float angle = (pot2*2.0f*pi)-pi;
+ { float angle = (pot2*2.0f*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
- 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
- errorf = ErrorCalc(refvaluef,currentanglef); // same for every motor
+ float refvaluef = 0.0f;
+ //float refvaluef = -DesiredAnglef(); // DONT CHANGE THIS MINUS SIGN: 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
+ 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
-}
-
-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;
- // minus sign to correct for direction convention
- return MotorPosition;
+ pin3 = fabs(PIDcontrolf); // different pins for every motor
+ pin2 = PIDcontrolf > 0.0f; // different pins for every motor
}
// Main program
@@ -250,16 +244,16 @@
pin3.period_us(15); // If you give a period on one pin, c++ gives all pins this period
motorl.attach(turnl,dt);
- //motorr.attach(turnr,dt);
- //motorf.attach(turnf,dt);
+ motorr.attach(turnr,dt);
+ //motorf.attach(turnf,dt); // DONT TURN THIS ON, UNLESS THE CALIBRATION MODE WORKS
emergencybutton.rise(Emergency); //If the button is pressed, stop program
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);
+ 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);
- pc.printf("Kp: %f \t\t Kd: %f \t\t angle: %f \t\t error: %f\r\n",Kp,Kd,currentanglel,errorl);
+ //pc.printf("Kp: %f \t\t Kd: %f \t\t angle: %f \t\t error: %f\r\n",Kp,Kd,currentanglel,errorl);
wait(0.1f);
}
}
