Arnoud Meutstege
first publish not working
Dependencies: MODSERIAL mbed ttmath FastPWM Motor_with_encoder biquadFilter
Diff: main.cpp
- Revision:
- 12:02eba9a294d2
- Parent:
- 11:66d0be7efd3f
- Child:
- 13:b5868fd8ffe9
--- a/main.cpp Wed Nov 01 15:30:59 2017 +0000
+++ b/main.cpp Thu Nov 02 11:59:11 2017 +0000
@@ -11,34 +11,57 @@
DigitalOut ledb(LED_BLUE);
DigitalOut ledr(LED_RED);
DigitalOut ledg(LED_GREEN);
+
DigitalOut motor1DC(D7);
DigitalOut motor2DC(D4);
FastPWM motor1PWM(D6);
FastPWM motor2PWM(D5);
-AnalogIn potMeter1(A0);
-AnalogIn potMeter2(A1);
-DigitalIn button1(D11);
-DigitalIn button2(D12);
-Encoder Encoder1(D12,D13);
-Encoder Encoder2(D8,D9);
+AnalogIn potMeter1(A0);
+AnalogIn potMeter2(A1);
+AnalogIn emg1(A2);
+AnalogIn emg2(A3);
+AnalogIn emg3(A4);
+AnalogIn emg4(A5);
+
+DigitalIn button1(D11);
+DigitalIn button2(D12);
+DigitalIn button3(SW2);
+DigitalIn button4(SW3);
+Encoder Encoder1(D12,D13);
+Encoder Encoder2(D8,D9);
MODSERIAL pc(USBTX,USBRX);
Ticker controller;
+// Constants EMG ----------------------- Start
+double X;
+double X_maxTreshold = 560;
+double X_minTreshold = 20;
+const double X_Callibrate = 300;
+
+double Y;
+double Y_maxTreshold = 560;
+double Y_minTreshold = 0;
+const double Y_Callibrate = 300;
+
+
// ---- Motor Constants-------
-float Pwmperiod = 0.001f;
-int potmultiplier = 600; // Multiplier for the pot meter reference which is normally between 0 and 1
-float gainM1 = 1/35.17; // encoder pulses per degree theta
-float gainM2 = 1/109.4; // gain for radius r
+const double pi = 3.1415926535897;
+float Pwmperiod = 0.001f;
+int potmultiplier = 600; // Multiplier for the pot meter reference which is normally between 0 and 1
+const float gearM1 = 6.2;
+const double gainM1 = 1/35.17; // encoder pulses per degree theta
+const double gainM2 = 1/(2*gearM1*pi); // gain for radius r
-volatile float motor1;
-volatile float motor2;
+double motor1;
+double motor2;
+double pos_M1;
+double pos_M2;
//Start constants PID -------------------------------
-const double pi = 3.1415926535897;
-const double M1_TS = 0.01; // (was 0.0001) 0.001 and 0.01 work without biquad filter. // // Sample time (motor - timestep)
+const double M1_TS = 0.01; // (was 0.0001) 0.001 and 0.01 work without biquad filter. // // Sample time (motor - timestep)
//verplaatst
const float RAD_PER_PULSE = (2*pi)/4200;
@@ -50,7 +73,7 @@
double m1_err_int = 0;
double m1_prev_err = 0;
-const float M2_KP = 10;
+const float M2_KP = 5;
const float M2_KI = 0.5;
const float M2_KD = 0.5; //was KP=10 KI=0.5 KD=0.5
double m2_err_int = 0;
@@ -64,6 +87,9 @@
const double M1_F_B2 = 4.0;
double m1_f_v1 = 0;
double m1_f_v2 = 0;
+
+
+
//---------------------------------End of constants PID
//-----------------Start PID part----------------------------START
@@ -74,7 +100,15 @@
//e_der = biquad(e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2);
e_prev1 = e1;
- e_int1 += Ts*e1; // Integral
+ e_int1 += Ts*e1;
+
+ if(button3 == 0 || button4 ==0){
+ e1 = 0;
+ e_prev1 = 0;
+ e_int1 = 0;
+ e_der1 = 0;
+ }
+ // Integral
return Kp1*e1 + Ki1*e_int1 + Kd1 * e_der1; //PID
}
@@ -85,30 +119,181 @@
//e_der = biquad(e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2);
e_prev2 = e2;
- e_int2 += Ts*e2; // Integral
+ e_int2 += Ts*e2;
+
+ if(button3 == 0 || button4 ==0){
+ e2 = 0;
+ e_prev2 = 0;
+ e_int2 = 0;
+ e_der2 = 0;
+ }
+ // Integral
return Kp2*e2 + Ki2*e_int2 + Kd2 * e_der2; //PID
}
//------------Get reference position-----------------START
float Get_X_Position(){
- double X = potMeter1 * potmultiplier;
+ // X = potMeter1 * potmultiplier; //--------- for Potmerter use
+
+// -- Potmeter use -----------------------------------
+ if (potMeter1 < 0.3)
+ {
+ X = X-0.5;
+ }
+ else if (potMeter1> 0.7)
+ {
+ X = X+0.5;
+ }
+ else
+ {
+ X = X;
+ }
+
+
+ /*
+ double in1 = emg1.read();
+ double in2 = emg2.read();
+
+ double RA = in1+in2;
+
+
+ if (RA < 0.5)
+ {
+ X = X;
+ }
+ else if (RA > 1.5)
+ {
+ X = X-0.1;
+ }
+ else
+ {
+ X = X+0.1;
+ }
+ */
+
+ if (X > X_maxTreshold){
+ X = X_maxTreshold;
+ }
+ else if (X < X_minTreshold){
+ X = X_minTreshold;
+ }
+ else{
+ X = X;
+ }
+
+ if(button3 == 0){
+ X = X_minTreshold;
+ }
+ else if (button4 == 0){
+ X = X_Callibrate;
+ }
+ else{
+ X = X;
+ }
+ //pc.baud(115200);
+ //pc.printf("\r (in1,in2):(%f,%f), RA = %f, X = %f \n",in1, in2, RA, X);
+
return X;
}
float Get_Y_Position(){
- double Y = potMeter2 * potmultiplier;
+ //Y = potMeter2 * potmultiplier; //--------- for Potmerter use
+
+// ---- Potmeter Use--------------------------
+ if (potMeter2 < 0.3)
+ {
+ Y = Y-0.5;
+ }
+ else if (potMeter2 > 0.7)
+ {
+ Y = Y+0.5;
+ }
+ else
+ {
+ Y = Y;
+ }
+
+
+ /*
+ double in3 = emg3.read();
+ double in4 = emg4.read();
+
+
+ double LA = in3+in4;
+
+ if (LA < 0.5)
+ {
+ Y = Y;
+ }
+ else if (LA > 1.5)
+ {
+ Y = Y-0.1;
+ }
+ else
+ {
+ Y = Y+0.1;
+ }
+ */
+ if (Y > Y_maxTreshold){
+ Y = Y_maxTreshold;
+ }
+ else if (Y < Y_minTreshold){
+ Y = Y_minTreshold;
+ }
+ else{
+ Y = Y;
+ }
+
+ if(button3 == 0){
+ Y = Y_minTreshold;
+ }
+ else if (button4 == 0){
+ Y = Y_Callibrate;
+ }
+ else{
+ Y = Y;
+ }
+
+ //pc.printf("\r (in3,in4):(%f,%f), LA = %f, Y = %f \n",in3, in4, LA, Y);
+
return Y;
}
//----------------------------------------------------END
//-------------Get current Position-------------------START
double motor1_Position(){ // has as output Theta
- double pos_m1 = gainM1*Encoder1.getPosition(); // current position for theta
- return pos_m1;
+
+ if (button3 == 0){
+ int T1 = ((atan(Y_minTreshold/X_minTreshold)*180)/pi)/gainM1;
+ Encoder1.setPosition(T1);
+ }
+ else if (button4 ==0){
+ int T1 = ((atan(Y_Callibrate/X_Callibrate)*180)/pi)/gainM1;
+ Encoder1.setPosition(T1);
+ }
+ else{
+ }
+ double pos_M1 = gainM1*Encoder1.getPosition(); // current position for theta
+ return pos_M1;
}
double motor2_Position(){ //output R
- double pos_m2 = gainM2 *Encoder2.getPosition(); // current position for the radius;
- return pos_m2;
+ int R1;
+
+ if (button3 == 0){
+ R1 = (sqrt(X_minTreshold*X_minTreshold+Y_minTreshold*Y_minTreshold))/gainM2;
+ Encoder2.setPosition(R1);
+ }
+ else if (button4 ==0){
+ R1 = (sqrt(X_Callibrate*X_Callibrate+Y_Callibrate*Y_Callibrate))/gainM2;
+ Encoder2.setPosition(R1);
+ }
+ else{
+ }
+
+ double pos_M2 = gainM2 *Encoder2.getPosition(); // current position for the radius;
+ pc.baud(115200);
+ // pc.printf("\r R1 = %f, pos_m2 = %f\n", R1,pos_m2);
+ return pos_M2;
}
//-----------------------------------------------------END
@@ -122,9 +307,9 @@
double reference_motor1 = (atan(y/x)*180)/pi; // reference for Theta
double reference_motor2 = sqrt((x*x+y*y)); // reference for radius
- float pos_M1 = motor1_Position(); // current position for theta
- float pos_M2 = motor2_Position(); // current position for the radius
-
+ double pos_M1 = motor1_Position(); // current position for theta
+ double pos_M2 = motor2_Position(); // current position for the radius
+
double delta1 = PID1(reference_motor1 - pos_M1, M1_KP, M1_KI, M1_KD, M1_TS, m1_err_int, m1_prev_err, m1_f_v1, m1_f_v2, M1_F_A1, M1_F_A2, M1_F_B0, M1_F_B1, M1_F_B2);
double delta2 = PID2(reference_motor2 - pos_M2, M2_KP, M2_KI, M2_KD, M1_TS, m2_err_int, m2_prev_err, m1_f_v1, m1_f_v2, M1_F_A1, M1_F_A2, M1_F_B0, M1_F_B1, M1_F_B2);
@@ -132,7 +317,8 @@
double dRadius = reference_motor2 - pos_M2;
pc.baud(115200);
- pc.printf("\r DesPosition(X,Y):(%f,%f), pos Error(dTheta, dError):(%f,%f)\n",x,y, dTheta ,dRadius);
+ //pc.printf("\r DesPosition(X,Y):(%f,%f), posError(dTheta, dError):(%f,%f), (delta1,delta2):(%f,%f), posM(M1,M2):(%f,%f) \n",x,y, dTheta ,dRadius,delta1, delta2, pos_M1,pos_M2);
+ pc.printf("\r pos(M1,M2):(%f,%f)\n", pos_M1, pos_M2);
//motor1PWM = motor1;
//motor2PWM = motor2;
@@ -161,8 +347,8 @@
}
motor1 = abs(delta1)/1000.0;
- if(motor1 >= 0.50) {
- motor1 = 0.50;
+ if(motor1 >= 0.35) {
+ motor1 = 0.35;
//pc.baud(115200);
//pc.printf("\r val motor1: %f\n", motor1);
}
@@ -195,11 +381,11 @@
motor2 = 0.50;
}
- motor1PWM = motor1 + 0.20;
- motor2PWM = motor2 + 0.20;
+ motor1PWM = motor1 + 0.65;
+ motor2PWM = motor2 + 0.50;
//pc.printf("\r delta(1,2):(%f,%f)\n", delta1,delta2);
- //pc.printf("\r motorvalues (M1,M2):(%f,%f),\n", motor1, motor2);
+ //pc.printf("\r motorvalues (M1,M2):(%f,%f),\n", motor1 + 0.65, motor2 + 0.20);
//pc.printf("\r
}