Arnoud Meutstege
first publish not working
Dependencies: MODSERIAL mbed ttmath FastPWM Motor_with_encoder biquadFilter
Diff: main.cpp
- Revision:
- 17:10c18ca3368b
- Parent:
- 16:000f2ebbd16c
- Child:
- 18:622e717da184
diff -r 000f2ebbd16c -r 10c18ca3368b main.cpp
--- a/main.cpp Mon Nov 06 22:50:16 2017 +0000
+++ b/main.cpp Mon Nov 06 23:36:22 2017 +0000
@@ -29,18 +29,23 @@
Encoder Encoder1(D12,D13);
Encoder Encoder2(D8,D9);
-DigitalIn button1(D11);
-DigitalIn button2(D12);
-DigitalIn button3(SW2);
-DigitalIn button4(SW3);
-
-
+//callibrating
+DigitalIn button1(SW2); //(x,y) = (..,..)
+DigitalIn button2(SW3); //(x,y) = (..,..)
MODSERIAL pc(USBTX,USBRX);
-
Ticker controller;
-// Constants EMG ----------------------- Start
+// --- EMG ---
+double in1;
+double in2;
+double RA;
+
+double in3;
+double in4;
+double LA;
+
+// Constants Position ----------------------- Start
double X;
double X_maxTreshold = 450;
double X_minTreshold = 20;
@@ -53,15 +58,16 @@
// ---- Motor Constants-------
+double motor1;
+double motor2;
+int SetPosM1;
+int SetPosM2;
+
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 double Pwmperiod = 0.001f;
const double gainM1 = 1/29.17; // 1 / encoder pulses per degree theta
const double gainM2 = 1/105.0; // 1 / pulses per mm
-double motor1;
-double motor2;
-
double reference_motor1 = 0; // reference for Theta
double reference_motor2 = 0;
double pos_M1 = 0; // start angle theta
@@ -71,9 +77,6 @@
//Start constants PID -------------------------------
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;
-const float CONTROLLER_TS = 0.01; //TIME INTERVAL/ hZ
const float M1_KP = 10;
const float M1_KI = 0.5;
@@ -116,7 +119,7 @@
e_prev1 = e1;
e_int1 += Ts*e1;
- if(button3 == 0 || button4 ==0){
+ if(button1 == 0 || button2 ==0){
e1 = 0;
e_prev1 = 0;
e_int1 = 0;
@@ -137,7 +140,7 @@
e_prev2 = e2;
e_int2 += Ts*e2;
- if(button3 == 0 || button4 ==0){
+ if(button1 == 0 || button2 ==0){
e2 = 0;
e_prev2 = 0;
e_int2 = 0;
@@ -147,61 +150,52 @@
return Kp2*e2 + Ki2*e_int2 + Kd2 * e_der2; //PID
}
-//------------Get reference position-----------------START
+// --- Get reference position ---
float Get_X_Position(){
- // X = potMeter1 * potmultiplier; //--------- for Potmerter use
-
-// -- Potmeter use -----------------------------------
- if (potMeter1 < 0.3)
- {
+// --- Altering X value with potmeter ---
+ if (potMeter1 < 0.3){
X = X-0.5;
- }
- else if (potMeter1> 0.7)
- {
+ }
+ else if (potMeter1> 0.7){
X = X+0.5;
- }
- else
- {
+ }
+ else{
X = X;
- }
+ }
+
+// --- Getting reference values from emg input ---
+ in1 = emg1.read();
+ in2 = emg2.read();
+ RA = in1+in2;
- ///*
- double in1 = emg1.read();
- double in2 = emg2.read();
-
- double RA = in1+in2;
-
-
- if (RA < 0.8)
- {
+ if (RA < 0.8){
X = X;
- }
- else if (RA > 1.8)
- {
+ }
+ else if (RA > 1.8){
X = X-0.4;
- }
- else
- {
+ }
+ else{
X = X+0.4;
- }
- // */
-
+ }
+
+// --- Setting max and min boundaries for X ---
if (X >= X_maxTreshold){
X = X_maxTreshold;
}
else if (X <= X_minTreshold){
X = X_minTreshold;
- }
+ }
else{
X = X;
- }
-
- if(button3 == 0){
+ }
+
+ // --- Setting callibration values for X ---
+ if(button1 == 0){
X = X_minTreshold;
}
- else if (button4 == 0){
+ else if (button2 == 0){
X = X_Callibrate;
}
else{
@@ -209,123 +203,106 @@
}
//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(){
- //Y = potMeter2 * potmultiplier; //--------- for Potmerter use
-
-// ---- Potmeter Use--------------------------
-
- if (potMeter2 < 0.3)
- {
+double Get_Y_Position(){
+// --- Altering Y value with potmeter ---
+ if (potMeter2 < 0.3){
Y = Y-0.8;
}
- else if (potMeter2 > 0.7)
- {
+ else if (potMeter2 > 0.7){
Y = Y+0.5;
- }
- else
- {
+ }
+ else{
Y = Y;
- }
-
-
- // /*
- double in3 = emg3.read();
- double in4 = emg4.read();
+ }
+// --- Getting reference values from emg input ---
+ in3 = emg3.read();
+ in4 = emg4.read();
+ LA = in3+in4;
-
- double LA = in3+in4;
-
- if (LA < 0.8)
- {
+ if (LA < 0.8){
Y = Y;
- ledb = 1;
- ledr = 1;
+ ledb = 1; // Implement LED feedback to give feedback to the user whether Y is increasing, decreasing, or stationairy.
+ ledr = 1; // The same is done for X on the transmitting embed, this way there is feedback for both X and Y.
ledg = 0; //Groen
}
- else if (LA > 1.8)
- {
+ else if (LA > 1.8){
Y = Y-0.4;
ledr = 1;
ledg = 1; //Blau
ledb = 0;
- }
- else
- {
+ }
+ else{
Y = Y+0.4;
ledb = 1; //Rood
ledr = 0;
ledg = 1;
- }
- // */
+ }
+// --- Setting max and min boundaries for Y ---
if (Y >= Y_maxTreshold){
Y = Y_maxTreshold;
}
else if (Y <= Y_minTreshold){
Y = Y_minTreshold;
- }
+ }
else{
Y = Y;
- }
-
- if(button3 == 0){
+ }
+ // --- Setting callibration values for Y ---
+ if(button1 == 0){
Y = Y_minTreshold;
}
- else if (button4 == 0){
+ else if (button2 == 0){
Y = Y_Callibrate;
- }
+ }
else{
Y = Y;
- }
-
- //pc.printf("\r (in3,in4):(%f,%f), LA = %f, Y = %f \n",in3, in4, LA, 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
-
- if (button3 == 0){
- int T1 = ((atan(Y_minTreshold/X_minTreshold)*180)/pi)/gainM1;
- Encoder1.setPosition(T1);
+// --- Setting callibration values for the encoder ---
+ if (button1 == 0){
+ SetPosM1 = ((atan(Y_minTreshold/X_minTreshold)*180)/pi)/gainM1;
+ Encoder1.setPosition(SetPosM1);
}
- else if (button4 ==0){
- int T1 = ((atan(Y_Callibrate/X_Callibrate)*180)/pi)/gainM1;
- Encoder1.setPosition(T1);
+ else if (button2 ==0){
+ SetPosM1 = ((atan(Y_Callibrate/X_Callibrate)*180)/pi)/gainM1;
+ Encoder1.setPosition(SetPosM1);
}
else{
}
+// --- getting current position ----
double pos_M1 = gainM1*Encoder1.getPosition(); // current position for theta
double countM1 = Encoder1.getPosition();
pc.baud(115200);
//pc.printf("\r counts encoder: %f\n",countM1);
-
return pos_M1;
}
double motor2_Position(){ //output R
- int R1;
-
- if (button3 == 0){
- R1 = (sqrt(X_minTreshold*X_minTreshold+Y_minTreshold*Y_minTreshold))/gainM2;
- Encoder2.setPosition(R1);
+// --- Setting callibration values for the encoder ---
+ if (button1 == 0){
+ SetPosM2 = (sqrt(X_minTreshold*X_minTreshold+Y_minTreshold*Y_minTreshold))/gainM2;
+ Encoder2.setPosition(SetPosM2);
}
- else if (button4 ==0){
- R1 = (sqrt(X_Callibrate*X_Callibrate+Y_Callibrate*Y_Callibrate))/gainM2;
- Encoder2.setPosition(R1);
+ else if (button2 ==0){
+ SetPosM2 = (sqrt(X_Callibrate*X_Callibrate+Y_Callibrate*Y_Callibrate))/gainM2;
+ Encoder2.setPosition(SetPosM2);
}
else{
}
-
- double pos_M2 = gainM2 *Encoder2.getPosition(); // current position for the radius;
+// --- getting current position ----
+ double pos_M2 = gainM2*Encoder2.getPosition(); // current position for the radius;
double countM2 = Encoder2.getPosition();
pc.baud(115200);
//pc.printf("\r counts encoder: %f\n",countM2);
@@ -346,48 +323,51 @@
pos_M1 = motor1_Position(); // current position for theta
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);
+ double deltaM1 = 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 deltaM2 = 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);
double dTheta = reference_motor1 - pos_M1;
double dRadius = reference_motor2 - pos_M2;
pc.baud(115200);
- //pc.printf("\r DesPosition(X,Y):(%f,%f), posError(dTheta, dError):(%f,%f), (delta1,delta2):(%f,%f)\n",x,y, dTheta ,dRadius,delta1, delta2);
+ //pc.printf("\r DesPosition(X,Y):(%f,%f), posError(dTheta, dError):(%f,%f), (deltaM1,deltaM2):(%f,%f)\n",x,y, dTheta ,dRadius,deltaM1, deltaM2);
pc.printf("\r DesPosition(X,Y):(%f,%f)\n", x,y);
//pc.printf("\r pos(M1,M2):(%f,%f)\n", pos_M1, pos_M2);
//motor1PWM = motor1;
//motor2PWM = motor2;
- if(delta1 > 0.5){
+// --- Direction control motors --- START
+ if(deltaM1 > 0.5){
motor1DC = 0;
}
- else if (delta1< -0.5) {
+ else if (deltaM1< -0.5) {
motor1DC = 1;
}
else{
motor1PWM = 0;
}
-
- motor1 = abs(delta1)/1000.0;
+
+if(deltaM2 > 2.0){
+ motor2DC = 0;
+ }
+ else if (deltaM2<-2.0) {
+ motor2DC = 1;
+ }
+ else{
+ motor2PWM = 0;
+ }
+// --- Direction control motors --- END
+
+// --- PWM values ---
+ motor1 = abs(deltaM1)/1000.0;
if(motor1 >= 0.10) {
motor1 = 0.10;
//pc.baud(115200);
//pc.printf("\r val motor1: %f\n", motor1);
}
-if(delta2 > 2.0){
- motor2DC = 0;
- }
- else if (delta2<-2.0) {
- motor2DC = 1;
- }
- else{
- motor2PWM = 0;
- }
-
- motor2 = abs(delta2)/1000.0;
+motor2 = abs(deltaM2)/1000.0;
if(motor2 >= 0.50) {
motor2 = 0.50;
}
@@ -395,7 +375,7 @@
motor1PWM = motor1 + 0.90;
motor2PWM = motor2 + 0.50;
- //pc.printf("\r delta(1,2):(%f,%f)\n", delta1,delta2);
+ //pc.printf("\r delta(1,2):(%f,%f)\n", deltaM1,deltaM2);
//pc.printf("\r motorvalues (M1,M2):(%f,%f),\n", motor1 + 0.65, motor2 + 0.20);
//pc.printf("\r
}
@@ -412,17 +392,6 @@
bqc.add(&bq1).add(&bq2);
- while(1){
- /*
- double x = Get_X_Position();
- double y = Get_Y_Position();
- double reference_motor1 = atan(y/x);
- int position_Motor1 = motor1_Position();
- double motor1 = PID(reference_motor1 - position_Motor1, 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);
-
- pc.baud(115200);
- pc.printf("\r Position(X)=(%f), Ref(Theta,R): (%f,), Pos(Theta,R):(%i,), Motor Value(M1,M2):(%f,).\n",x, reference_motor1, position_Motor1, motor1);
- */
- }
+ while(1){}
}
\ No newline at end of file