Tommie Verouden
/
StateMachine
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: biquadFilter FastPWM MODSERIAL QEI mbed
Diff: main.cpp
- Revision:
- 52:17b3aeacb643
- Parent:
- 51:78c75cc72d17
- Child:
- 53:75076f9705dc
--- a/main.cpp Thu Nov 01 20:30:19 2018 +0000
+++ b/main.cpp Thu Nov 01 21:17:00 2018 +0000
@@ -25,7 +25,6 @@
InterruptIn buttonHome(SW3); // button on K64F
// Potmeters
-AnalogIn pot1(A1);
AnalogIn pot2(A2);
// Motor pins input
@@ -130,8 +129,8 @@
const double xbase = 450-lb; //Length between the motors
//General parameters
-double theta1 = PI*0.49; //Angle of the left motor
-double theta4 = PI*0.49; //Angle of the right motor
+double theta1 = PI/2.0; //Angle of the left motor
+double theta4 = PI/2.0; //Angle of the right motor
double thetaflip = 0; //Angle of the flipping motor
double prefx; //Desired x velocity
double prefy; //Desired y velocity "
@@ -415,6 +414,12 @@
return error;
}
+float angleDesired() // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
+{
+ float angle = (pot2*2.0f*PI)-PI;
+ return angle;
+}
+
// ------------------------- MOTOR FUNCTIONS FOR MOTOR LEFT --------------------
float PIDControllerL(float angleReference,float angleCurrent) // PID controller for the motors, based on the reference value and the current angle of the motor
{
@@ -439,12 +444,6 @@
return u_k + u_i + u_d;
}
-float angleDesiredL() // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
-{
- float angleL = (pot1*2.0f*PI)-PI;
- return angleL;
-}
-
int countsInputCalibratedL() // Gets the counts from encoder 1
{
countsL = encoderL.getPulses()- countsOffsetL + countsCalibration;
@@ -453,7 +452,7 @@
void motorTurnL() // main function for movement of motor 1, all above functions with an extra tab are called
{
- float angleReferenceL = angleDesiredL(); // insert kinematics output here instead of angleDesiredL()
+ float angleReferenceL = theta1; // insert kinematics output here instead of angleDesired()
angleReferenceL = -angleReferenceL; // different minus sign per motor
countsL = countsInputCalibratedL(); // different encoder pins per motor
angleCurrentL = angleCurrent(countsL); // different minus sign per motor
@@ -469,7 +468,7 @@
// This program sets the counts from the motor to the reference counts (an angle of PI/4.0)
// Do this for every motor and after calibrated all motors, press a button
{
- float angleReferenceL = angleDesiredL(); // insert kinematics output here instead of angleDesiredL()
+ float angleReferenceL = angleDesired(); // insert kinematics output here instead of angleDesired()
angleReferenceL = -angleReferenceL; // different minus sign per motor
countsL = countsInputL(); // different encoder pins per motor
angleCurrentL = angleCurrent(countsL); // different minus sign per motor
@@ -512,12 +511,6 @@
return u_k + u_i + u_d;
}
-float angleDesiredR() // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
-{
- float angleR = (pot2*2.0f*PI)-PI;
- return angleR;
-}
-
int countsInputCalibratedR() // Gets the counts from encoder 1
{
countsR = encoderR.getPulses()- countsOffsetR + countsCalibration;
@@ -526,7 +519,7 @@
void motorTurnR() // main function for movement of motor 1, all above functions with an extra tab are called
{
- float angleReferenceR = angleDesiredR(); // insert kinematics output here instead of angleDesiredR()
+ float angleReferenceR = theta4; // insert kinematics output here instead of angleDesired()
angleReferenceR = -angleReferenceR; // different minus sign per motor
countsR = countsInputCalibratedR(); // different encoder pins per motor
angleCurrentR = angleCurrent(countsR); // different minus sign per motor
@@ -542,7 +535,7 @@
// This program sets the counts from the motor to the reference counts (an angle of PI/4.0)
// Do this for every motor and after calibrated all motors, press a button
{
- float angleReferenceR = angleDesiredR(); // insert kinematics output here instead of angleDesiredR()
+ float angleReferenceR = angleDesired(); // insert kinematics output here instead of angleDesired()
angleReferenceR = -angleReferenceR; // different minus sign per motor
countsR = countsInputR(); // different encoder pins per motor
angleCurrentR = angleCurrent(countsR); // different minus sign per motor
@@ -563,6 +556,70 @@
// ------------------------- MOTOR FUNCTIONS FOR MOTOR FLIP --------------------
+float PIDControllerF(float angleReference,float angleCurrent) // PID controller for the motors, based on the reference value and the current angle of the motor
+ { float Kp = 19.24f;
+ float Ki = 1.02f;
+ float Kd = 0.827f;
+ float error = errorCalc(angleReference,angleCurrent);
+ static float errorIntegralF = 0.0;
+ static float errorPreviousF = 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;
+ // Integral part
+ errorIntegralF = errorIntegralF + error * dt;
+ float u_i = Ki * errorIntegralF;
+ // Derivative part
+ float errorDerivative = (error - errorPreviousF)/dt;
+ float errorDerivativeFiltered = PIDFilterF.step(errorDerivative);
+ float u_d = Kd * errorDerivativeFiltered;
+ errorPreviousF = error;
+ // Sum all parts and return it
+ return u_k + u_i + u_d;
+ }
+
+ int countsInputCalibratedF() // Gets the counts from encoder 1
+ { countsF = encoderF.getPulses()- countsOffsetF + countsCalibration;
+ return countsF;
+ }
+
+void motorTurnF() // main function for movement of motor 1, all above functions with an extra tab are called
+{
+ float angleReferenceF = 0.0f; // insert kinematics output here instead of angleDesired()
+ angleReferenceF = -angleReferenceF; // different minus sign per motor
+ countsF = countsInputCalibratedF(); // different encoder pins per motor
+ angleCurrentF = angleCurrent(countsF); // different minus sign per motor
+ errorF = errorCalc(angleReferenceF,angleCurrentF); // same for every motor
+
+ float PIDControlF = PIDControllerF(angleReferenceF,angleCurrentF); // same for every motor
+ pin5 = fabs(PIDControlF); // different pins for every motor
+ pin4 = PIDControlF > 0.0f; // different pins for every motor
+}
+
+void calibrationF() // Partially the same as motorTurnF, only with potmeter input
+// How it works: manually turn motor using potmeters until the robot arm touches the bookholder.
+// This program sets the counts from the motor to the reference counts (an angle of PI/4.0)
+// Do this for every motor and after calibrated all motors, press a button
+{ float angleReferenceF = 0.0f; // insert kinematics output here instead of angleDesired()
+ angleReferenceF = -angleReferenceF; // different minus sign per motor
+ countsF = countsInputF(); // different encoder pins per motor
+ angleCurrentF = angleCurrent(countsF); // different minus sign per motor
+ errorF = errorCalc(angleReferenceF,angleCurrentF); // same for every motor
+
+ if (fabs(errorF) >= 0.01f)
+ { float PIDControlF = PIDControllerF(angleReferenceF,angleCurrentF); // same for every motor
+ pin5 = fabs(PIDControlF); // different pins for every motor
+ pin4 = PIDControlF > 0.0f; // different pins for every motor
+ }
+ else if (fabs(errorF) < 0.01f)
+ { countsOffsetF = countsF;
+ countsF = countsF - countsOffsetF + countsCalibration;
+ //countsF = 0;
+ pin5 = 0.0f;
+ // BUTTON PRESS: TO NEXT STATE
+ }
+}
+
// ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ STATE MACHINE ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
void stateMachine(void)
{
@@ -590,7 +647,7 @@
// --------------------------- transition ----------------------------
// Transition condition: when all motor errors smaller than 0.01,
// start calibrating EMG
- if (errorL < 0.01F && buttonBio1 == true) {
+ if (errorL < 0.01f && buttonBio1 == false) {
// Actions when leaving state
blinkTimer.detach();
@@ -622,7 +679,7 @@
// --------------------------- transition ----------------------------
// Transition condition: when all motor errors smaller than 0.01,
// start calibrating EMG
- if (errorR < && buttonBio2 == true) {
+ if (errorR < 0.01f && buttonBio2 == false) {
// Actions when leaving state
blinkTimer.detach();
@@ -836,9 +893,11 @@
/* */
ReadUseEMG0(); //Start the use of EMG
ReadUseEMG1(); //Start the use of EMG
+ pc.printf("theta1 and 4: %f \t\t %f \n\r",theta1,theta4);
kinematics();
motorTurnL();
motorTurnR();
+
// --------------------------- transition ----------------------------
// Transition condition: with button press, back to homing mode
