Marijke Zondag
/
Project_script_union_final_version
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: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of
Project_script_union_final
by 
Jorine Oosting
Revision 33:976be2825a23, committed 2018-11-02
- Comitter:
- Esmee
- Date:
- Fri Nov 02 11:57:51 2018 +0000
- Parent:
- 32:56a8bd82e971
- Child:
- 34:b8b18ba0c336
- Commit message:
- werkend script zonder begrensing encoder;
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Thu Nov 01 18:58:28 2018 +0000
+++ b/main.cpp Fri Nov 02 11:57:51 2018 +0000
@@ -185,7 +185,7 @@
wait(0.001f); //Does there need to be a wait?
}
Mean0 = sum/sizeCal; //Calculate mean of the datapoints in the calibration set (2000 samples)
- Threshold0 = Mean0*0.8; //Threshold calculation calve = 0.8*mean
+ Threshold0 = Mean0*0.5; //Threshold calculation calve = 0.8*mean
break; //Stop. Threshold is calculated, we will use this further in the code
}
case 1: //If calibration state 1:
@@ -348,14 +348,13 @@
// Sum all parts and return it
u2 = u_k2 + u_i2 + u_d2;
}
-
void engine_control1() //Engine 1 is translational engine, connected with left side pins
{
//pc.printf("ik doe het, engine control 1\n\r");
encoder_radians1 = (double)encoder1.getPulses()*(2.0*PI)/8400.0;
//pc.printf("encoder1 %f \n\r", (float)encoder1.getPulses());
//pc.printf("encoder_radians1 %f \n\r",(float) encoder_radians1);
- err1 = q1ref - encoder_radians1;
+ err1 = q1_motor - encoder_radians1;
//pc.printf("err1 = %f\n\r",err1);
PID_control1(); //PID controller function call
@@ -377,7 +376,7 @@
encoder_radians2 = (float)encoder2.getPulses()*(2.0*PI)/8400.0;
//pc.printf("encoder2 %f \n\r",(float)encoder2.getPulses());
//pc.printf("encoder_radians2 %f \n\r",(float)encoder_radians2);
- err2 = q2ref - encoder_radians2;
+ err2 = q2_motor - encoder_radians2;
//pc.printf("err2 = %f\n\r",err2);
PID_control2(); //PID controller function call
//pc.printf("u2 = %f\n\r",u2);
@@ -393,17 +392,55 @@
// }
}
+
+/*void engine_control1() //Engine 1 is translational engine, connected with left side pins
+{
+ encoder_radians1 = (double)encoder1.getPulses()*(2.0*PI)/8400.0;
+ err1 = q1_motor - encoder_radians1;
+ PID_control1(); //PID controller function call
+
+ if(encoder1.getPulses()<12000 && encoder1.getPulses()>-1) //limits translation in counts, eerst 12600
+ {
+ pwmpin1 = fabs(u1);
+ directionpin1.write(u1<0);
+ }
+ else
+ {
+ pwmpin1 = fabs(u1);
+ directionpin1.write(u1>0);
+ }
+}
+
+void engine_control2() //Engine 2 is rotational engine, connected with right side wires
+{
+ encoder_radians2 = (float)encoder2.getPulses()*(2.0*PI)/8400.0;
+ err2 = q2_motor - encoder_radians2;
+ PID_control2(); //PID controller function call
+
+ if(encoder2.getPulses()<-5250 && encoder2.getPulses()>5250) //limits rotation, in counts
+ {
+ pwmpin2 = fabs(u2); //u_total moet nog geschaald worden om in de motor gevoerd te worden!!!
+ directionpin2.write(u2>0);
+ }
+ else
+ {
+ pwmpin2 = fabs(u2); //u_total moet nog geschaald worden om in de motor gevoerd te worden!!!
+ directionpin2.write(u2<0);
+ }
+}
+*/
+
//------------------ Inversed Kinematics --------------------------------//
void inverse_kinematics()
{
- //pc.printf("v_x is %f en v_y is %f\n\r",v_x, v_y);
+ //pc.printf("v_x is %f en v_y is %f\n\r",v_x, v_y);
//Lq1 = q1ref*r_trans;
//Cq2 = q2ref/5.0;
q1_dot = (v_x*cos(q2ref) + v_y*sin(q2ref))/cos(q2ref); //RKI systeem
- q2_dot = v_y/(L3*cos(q2ref)); // Misschien gain toevoegen om te kijken of het dan werkt. Translatie gaat veel trager en moeizamer dan rotatie
+ q2_dot = v_y/(L3*cos(q2ref)); //
q1_ii = q1ref + q1_dot*T; //Omgezet naar motorhoeken
q2_ii = q2ref + q2_dot*T;
@@ -411,16 +448,12 @@
q1ref = q1_ii;
q2ref = q2_ii;
- q1_motor = q1ref*5.0;
- q2_motor = q2ref/r_trans;
-
+ q1_motor = -q1ref/r_trans;
+ q2_motor = q2ref*5.0;
- //pc.printf("q1ref is %f en q2ref is %f\n\r",q1ref, q2ref);
-
-
- //start_control = 1;
engine_control1();
engine_control2();
+
}
void v_des_calculate_qref()
@@ -429,7 +462,7 @@
{
if(movAg1>Threshold1 && movAg0<Threshold0) //If the filtered EMG signal of muscle 1 is higher than the threshold and the switch is off (movAg0)
{
- v_x = 0.5; //movement in +x direction
+ v_x = 0.05; //movement in +x direction
v_y = 0.0;
ledr = 0; //red
@@ -438,7 +471,7 @@
}
else if(movAg2>Threshold2 && movAg0<Threshold0) //If the filtered EMG signal of muscle 2 is higher than the threshold and the switch is off (movAg0)
{
- v_y = 0.5; //Movement in +y direction
+ v_y = 0.05; //Movement in +y direction
v_x = 0.0;
ledr = 1; //Green
@@ -449,7 +482,7 @@
else if(movAg0>Threshold0 && movAg1>Threshold1) //If the filtered EMG signal of muscle 1 is higher than the threshold and the switch is on (movAg0)
{
v_y = 0.0; //Movement in -x direction
- v_x = -0.5;
+ v_x = -0.05;
ledr = 0; //Purple
ledb = 0;
@@ -458,7 +491,7 @@
else if(movAg0>Threshold0 && movAg2>Threshold2) //If the filtered EMG signal of muscle 2 is higher than the threshold and the switch is on (movAg0)
{
- v_y = -0.5; //Movement in -y direction
+ v_y = -0.05; //Movement in -y direction
v_x = 0.0;
ledr = 1; //Blue