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:
- 43:e8f2193822b7
- Parent:
- 40:1be9dfad0a10
- Child:
- 44:de6b4eac5cb7
- Child:
- 47:bad50d34561c
- Child:
- 48:36cdeaac67c5
--- a/main.cpp Wed Oct 31 13:42:39 2018 +0000
+++ b/main.cpp Wed Oct 31 16:32:36 2018 +0000
@@ -22,7 +22,7 @@
// Output
DigitalOut pin2(D2); // Motor 3 direction = motor flip
-FastPWM pin3(D3); // Motor 3 pwm = motor flip
+FastPWM pin3(A5); // Motor 3 pwm = motor flip
DigitalOut pin4(D4); // Motor 2 direction = motor right
FastPWM pin5(D5); // Motor 2 pwm = motor right
FastPWM pin6(D6); // Motor 1 pwm = motor left
@@ -36,8 +36,10 @@
MODSERIAL pc(USBTX, USBRX);
QEI Encoderl(D9,D8,NC,4200); // Counterclockwise motor rotation is the positive direction
QEI Encoderr(D10,D11,NC,4200); // Counterclockwise motor rotation is the positive direction
-QEI Encoderf(D13,D12,NC,4200); // Counterclockwise motor rotation is the positive direction
-Ticker motor;
+QEI Encoderf(D12,D13,NC,4200); // Counterclockwise motor rotation is the positive direction
+Ticker motorl;
+Ticker motorr;
+Ticker motorf;
Ticker encoders;
// Global variables
@@ -46,6 +48,13 @@
const float fCountsRad = 4200.0f;
const float dt = 0.001f;
+float currentanglel;
+float errorl;
+float currentangler;
+float errorr;
+float currentanglef;
+float errorf;
+
// Functions
void Emergency() // Emgergency, if SW2 on biorobotics is pressed, everything will instantly abort and a red light goes on
{ greenled = 1; // Red light on
@@ -99,82 +108,131 @@
{ float Kd = 0.25f*pot1;
return Kd;
}
-
- float PIDcontroller(float refvalue,float CurAngle) // PID controller for the motors, based on the reference value and the current angle of the motor
+
+ 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!
+ static BiQuad PIDfilterl(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
+ // Proportional part:
+ float u_k = Kp * error;
+ // Integral part
+ error_integrall = error_integrall + error * dt;
+ float 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;
+ 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;
+ 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
+ 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
+}
+
+ 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_integral = 0.0;
- static float error_prev = error; // initialization with this value only done once!
- static BiQuad PIDfilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
+ static float error_integralr = 0.0;
+ static float 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;
// Integral part
- error_integral = error_integral + error * dt;
- float u_i = Ki * error_integral;
+ error_integralr = error_integralr + error * dt;
+ float u_i = Ki * error_integralr;
// Derivative part
- float error_derivative = (error - error_prev)/dt;
- float filtered_error_derivative = PIDfilter.step(error_derivative);
+ float error_derivative = (error - error_prevr)/dt;
+ float filtered_error_derivative = PIDfilterr.step(error_derivative);
float u_d = Kd * filtered_error_derivative;
- error_prev = error;
+ error_prevr = error;
// Sum all parts and return it
- pc.printf ("Kp = %f Kd = %f",Kp,Kd);
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;
+ return angle;
+ }
- float DesiredAnglel() // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
- { float angle = (pot1*2.0f*pi)-pi;
- return angle;
- }
-
- float DesiredAngler() // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
- { float angle = (pot2*2.0f*pi)-pi;
- return angle;
- }
-
-float* turnl() // main function for movement of motor 1, all above functions with an extra tab are called
+void turnr() // main function for movement of motor 1, all above functions with an extra tab are called
{
- //float refvalue = pi/4.0f;
- float refvalue = DesiredAnglel(); // different minus sign per motor
- int counts = Countslinput(); // different encoder pins per motor
- float currentangle = CurrentAngle(counts); // different minus sign per motor
- float PIDcontrol = PIDcontroller(refvalue,currentangle); // same for every motor
- float error = ErrorCalc(refvalue,currentangle); // same for every motor
+ 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
- pin6 = fabs(PIDcontrol); // different pins for every motor
- pin7 = PIDcontrol > 0.0f; // different pins for every motor
- //pin6 = 0.4+0.6*fabs(PIDcontr); //geschaald
- //pin6 = fabs(PIDcontr)/pi;
- //pc.printf(" error: %f ref: %f angle: %f \r\n",error,refvalue,currentangle);
- float* outcome;
- float turninfo[3] = {error, refvalue, currentangle};
- //float ( &fillarr( float (&outcome)[3] ) )[3] { // no decay; argument must be size 3
- outcome = turninfo;
- return outcome;
+ pin5 = fabs(PIDcontrolr); // different pins for every motor
+ pin4 = PIDcontrolr > 0.0f; // different pins for every motor
}
-float* turnr() // main function for movement of motor 1, all above functions with an extra tab are called
-{
- //float refvalue = pi/4.0f;
- float refvalue = DesiredAngler(); // different minus sign per motor
- int counts = Countsrinput(); // different encoder pins per motor
- float currentangle = CurrentAngle(counts); // different minus sign per motor
- float PIDcontrol = PIDcontroller(refvalue,currentangle); // same for every motor
- float error = ErrorCalc(refvalue,currentangle); // same 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!
+ static BiQuad PIDfilterf(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
+ // Proportional part:
+ float u_k = Kp * error;
+ // Integral part
+ error_integralf = error_integralf + error * dt;
+ float 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;
+ error_prevf = error;
+ // Sum all parts and return it
+ return u_k + u_i + u_d;
+ }
- pin5 = fabs(PIDcontrol); // different pins for every motor
- pin4 = PIDcontrol > 0.0f; // different pins for every motor
- //pin6 = 0.4+0.6*fabs(PIDcontr); //geschaald
- //pin6 = fabs(PIDcontr)/pi;
- //pc.printf(" error: %f ref: %f angle: %f \r\n",error,refvalue,currentangle);
- float* outcome;
- float turninfo[3] = {error, refvalue, currentangle};
- //float ( &fillarr( float (&outcome)[3] ) )[3] { // no decay; argument must be size 3
- outcome = turninfo;
- return outcome;
+ float DesiredAnglef() // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
+ { 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
+
+ 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
@@ -189,29 +247,15 @@
pc.baud(115200);
pin3.period_us(15); // If you give a period on one pin, c++ gives all pins this period
- //float motoroutcome1 = motor.attach(turn1,dt);
+ motorl.attach(turnl,dt);
+ motorr.attach(turnr,dt);
+ motorf.attach(turnf,dt);
emergencybutton.rise(Emergency); //If the button is pressed, stop program
while (true)
{
- turnl();
- turnr();
- wait(dt);
- //float* motoroutcomel = turnl();
- //float motorl1 = motoroutcomel[0];
- //float motorl2 = motoroutcomel[1];
- //float motorl3 = motoroutcomel[2];
- //pc.printf(" errorl: %f refl: %f anglel: %f \r\n",motorl1,motorl2,motorl3);
-
- //float* motoroutcomer = turnr();
- //float motorr1 = motoroutcomer[0];
- //float motorr2 = motoroutcomer[1];
- //float motorr3 = motoroutcomer[2];
- //pc.printf(" errorr: %f refr: %f angler: %f \r\n",motorr1,motorr2,motorr3);
-
- //wait(dt);
- //wait(dt*10);
- //wait(printingfreq); --> still needs to be defined
+ 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);
}
}
