mevlid yildirim
/
BioRobotics_BMT
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: mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
Revision 15:849e0fc5d3a8, committed 2019-10-29
- Comitter:
- fb07
- Date:
- Tue Oct 29 20:21:54 2019 +0000
- Parent:
- 14:236ae2d7ec41
- Child:
- 16:1be144329f05
- Commit message:
- Motor 2 also goes to the home position in this revision
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Tue Oct 29 20:07:26 2019 +0000
+++ b/main.cpp Tue Oct 29 20:21:54 2019 +0000
@@ -103,6 +103,9 @@
double kp_motor2;
double Ki_motor2;
double Kd_motor2;
+ double Up_motor2;
+ double Ui_motor2;
+ double Ud_motor2;
double positie_motor2; //counts encoder
double error1_motor2;
@@ -122,15 +125,15 @@
void HIDScope() //voor HIDscope
{
scope.set(0, positie_motor1);
- scope.set(1, error1_motor1); //nog te definieren wat we willen weergeven
- scope.set(2, P_motor1); //nog te definieren wat we willen weergeven
- scope.set(3, Up_motor1);
+ scope.set(1, positie_motor2); //nog te definieren wat we willen weergeven
+ // scope.set(2, P_motor1); //nog te definieren wat we willen weergeven
+ // scope.set(3, Up_motor1);
// scope.set(4, Ui_motor1);
// scope.set(5, Uk_motor1);
-
scope.send();
}
+
// 4.x Encoder motor1 ****************************************************************
double fencoder_motor1() // bepaalt de positie van de motor
{
@@ -195,12 +198,12 @@
Up_motor1 = kp_motor1 * error1_motor1;
//Integral part
- Ki_motor1 = 0.0001; // moet nog getweaked worden
+ Ki_motor1 = 0.001; // moet nog getweaked worden
error_integral_motor1 = error_integral_motor1 + (Ts*error1_motor1); // integrale fout + (de sample tijd * fout)
Ui_motor1 = Ki_motor1 * error_integral_motor1; // (fout * integrale fout)
//Derivative part
- Kd_motor1 = 0.01 ;// moet nog getweaked worden
+ Kd_motor1 = 0.001 ;// moet nog getweaked worden
error1_derivative_motor1 = (error1_motor1-error1_prev_motor1)/Ts; // (Fout - de vorige fout) / tijdstap = afgeleide
error1_derivative_filtered_motor1 = LowPassFilter.step(error1_derivative_motor1); //derivative wordt gefiltered
Ud_motor1 = Kd_motor1 * error1_derivative_filtered_motor1; // (afgeleide gain) * (afgeleide gefilterde fout)
@@ -214,25 +217,25 @@
}
// 4.2b PID-Controller motor 2**************************************************
- double PID_controller_motor2()
+ double PID_controller_motor2(double &error_integral_motor2, double &error1_prev_motor2)
{
//Proportional part
kp_motor2 = 0.01 ; // moet nog getweaked worden
- double Up_motor2 = kp_motor2 * error1_motor2;
+ Up_motor2 = kp_motor2 * error1_motor2;
//Integral part
- Ki_motor2 = 0.0001; // moet nog getweaked worden
+ Ki_motor2 = 0.001; // moet nog getweaked worden
error_integral_motor2 = error_integral_motor2 + (Ts*error1_motor2); // integrale fout + (de sample tijd * fout)
- double Ui_motor2 = Ki_motor2 * error_integral_motor2; //de fout keer de integrale fout
+ Ui_motor2 = Ki_motor2 * error_integral_motor2; //de fout keer de integrale fout
//Derivative part
- Kd_motor2 = 0.01 ;// moet nog getweaked worden
+ Kd_motor2 = 0.001 ;// moet nog getweaked worden
error1_derivative_motor2 = (error1_motor2 - error1_prev_motor2)/Ts;
error1_derivative_filtered_motor2 = LowPassFilter.step(error1_derivative_motor2); //derivative wordt gefiltered, dit later aanpassen
- double Ud_motor2 = Kd_motor2 * error1_derivative_filtered_motor2;
+ Ud_motor2 = Kd_motor2 * error1_derivative_filtered_motor2;
error1_prev_motor2 = error1_motor2;
- double P_motor2 = Up_motor2 + Ui_motor2 + Ud_motor2; //sommatie van de u's
+ P_motor2 = Up_motor2 + Ui_motor2 + Ud_motor2; //sommatie van de u's
return P_motor2;
}
@@ -257,7 +260,27 @@
{
motor1 = fabs(P_motor1);
}
+ }
+ double motor2_pwm()
+ {
+
+ if (P_motor2 >=0 ) // Als de stuursignaal groter is als 0, dan clockwise rotatie, anders counterclockwise rotatie
+ {
+ motor2DirectionPin=2; // Clockwise rotation
+ }
+ else
+ {
+ motor2DirectionPin=0; // Counterclockwise rotation
+ }
+ if (fabs(P_motor2) > 0.99 ) // als de absolute waarde van de motorsnelheid groter is als 1, terug schalen naar 1, anders de absolute waarde van de snelheid. (Bij een waarde lager als 0 draait de motor niet)
+ {
+ motor2 = 0.99 ;
+ }
+ else
+ {
+ motor2 = fabs(P_motor2);
+ }
}
void motor1_controller(void)
@@ -266,7 +289,11 @@
motor1_pwm();
}
-
+ void motor2_controller(void)
+ {
+ error1_motor2 = (Yref_motor2 - positie_motor2);
+ motor2_pwm();
+ }
// 4.3 State-Machine *******************************************************
@@ -286,7 +313,8 @@
pc.printf("\r\n Motor Calibration is done!");
encoder_motor1.reset();
encoder_motor2.reset();
- Yref_motor1=10000;
+
+
State=StartWait;
}
else {;} //pc.printf("\r\n Motor Calibration is not done!");}
@@ -298,7 +326,9 @@
led_blue.write(0);
led_red.write(1);
led_green.write(1);
- Yref_motor1=10000;
+
+ Yref_motor1=5000;
+ Yref_motor2=2000;
if(button1==0) {State=EMGCalibration;}
if(button2==0) {State=Demo;}
break;
@@ -315,6 +345,7 @@
led2=1;
motor1_controller();
+ motor2_controller();
// State=Operating;
break;
case Demo:
@@ -371,6 +402,7 @@
fencoder_motor1() ;
fencoder_motor2() ;
PID_controller_motor1(error_integral_motor1, error1_prev_motor1);
+PID_controller_motor2(error_integral_motor2, error1_prev_motor2);
state_machine() ;
// 5.1 Measure Analog and Digital input signals ********************************