Biorobotica TIC
/
DEMO_TEST_LIJN
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: Demo_TEST3 QEI biquadFilter mbed
Fork of
DEMO_TEST_LIJN
by 
Tim Luten
Revision 12:21441b04ba29, committed 2018-10-31
- Comitter:
- TimLu
- Date:
- Wed Oct 31 15:02:48 2018 +0000
- Parent:
- 11:01372da5a144
- Commit message:
- TEST
Changed in this revision
| Demo_TEST3.lib | Show annotated file Show diff for this revision Revisions of this file |
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 01372da5a144 -r 21441b04ba29 Demo_TEST3.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Demo_TEST3.lib Wed Oct 31 15:02:48 2018 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/Biorobotica-TIC/code/Demo_TEST3/#01372da5a144
diff -r 01372da5a144 -r 21441b04ba29 main.cpp
--- a/main.cpp Wed Oct 31 11:50:31 2018 +0000
+++ b/main.cpp Wed Oct 31 15:02:48 2018 +0000
@@ -15,71 +15,70 @@
DigitalOut motor2direction(D4);
PwmOut motor2control(D5);
-InterruptIn button1(D10);
-
Serial pc(USBTX, USBRX);
double translatie;
double hoekgraad;
double hoekgraad2;
-
+ double hoekrad2;
+ double q_dot_angle;
+ double q_dot_L;
+ double q_dot_q2;
+ double J_inv_1_1;
+ double J_inv_1_2;
+ double J_inv_2_1;
+ double J_inv_2_2;
+ double p_old_x;
+ double p_old_y;
+
const float pi = 3.141592653589793; // Value of pi
double gearratio = 3.857142857;
double radiuspulley = 0.015915; // Radius pulley [m]
- double K_v = 0.5; //velocity constant, max 6.667 ?
+ double K_v = 2; //velocity constant, max 6.667 ?
double L0 = 0.09; // starting length
//-----------------GET ENCODER VALUES -------------------------
void EncoderFunc() {
- hoekgraad = Encoder1.getPulses() * 0.0857142857;
- // double hoekrad = hoekgraad * 0.0174532925;
- hoekgraad2 = Encoder2.getPulses() * 0.0857142857; // Angle arm [degree]
- // double hoekrad2 = hoekgraad2 * 0.0174532925;
- // double hoekarm = hoekgraad2 / gearratio;
- translatie = hoekgraad / 360.0 * 2.0 * pi * radiuspulley; // Translatie arm [m]
- }
+hoekgraad = Encoder1.getPulses() * 0.0857142857;
+// double hoekrad = hoekgraad * 0.0174532925;
+hoekgraad2 = Encoder2.getPulses() * 0.0857142857; // Angle arm [degree]
+hoekrad2 = hoekgraad2 * 0.0174532925;
+// double hoekarm = hoekgraad2 / gearratio;
+translatie = (hoekgraad / 180.0) * pi * radiuspulley; // Translatie arm [m]
+
+//---------------- INVERSE KINEMATICS ---------------------------
+p_old_x = (translatie+L0)*cos(hoekrad2); // Everytime the x-value from encoder calculated
+p_old_y = (translatie+L0)*sin(hoekrad2); // Everytime the y-value from encoder calculated
+
+J_inv_1_1 = -sin(hoekrad2)/(translatie+L0); // Construction of inverse Jacobian
+J_inv_1_2 = cos(hoekrad2)/(translatie+L0);
+J_inv_2_1 = cos(hoekrad2);
+J_inv_2_2 = sin(hoekrad2);
+}
int main()
{
- motor2direction = false; // Nu staan motoren toch op het begin allebei in positieve stand?
- motor2direction = false;
+motor2direction = false; // Nu staan motoren toch op het begin allebei in positieve stand?
+motor2direction = false;
-EncoderTicker.attach(&EncoderFunc, 0.1);
-pc.baud(115200);
-pc.printf("hoekgraad=%f degrees\t translatie:%f meters /t hoekgraad2:%f degrees /n",hoekgraad, translatie, hoekgraad2);
+EncoderTicker.attach(&EncoderFunc, 0.02);
-//---------------- INVERSE KINEMATICS ---------------------------
-double p_old_x = (translatie+L0)*cos(hoekgraad2); // Everytime the x-value from encoder calculated
-double p_old_y = (translatie+L0)*sin(hoekgraad2); // Everytime the y-value from encoder calculated
+// Demo path: rectangular
+double x_path[2]; // Matrix heeft 5 elementen: beginnend vanaf element 0 tot en met element 4
+x_path[0] = L0;
+x_path[1] = L0;
-double J_inv_1_1 = -sin(hoekgraad2)/(translatie+L0); // Construction of inverse Jacobian
-double J_inv_1_2 = cos(hoekgraad2)/(translatie+L0);
-double J_inv_2_1 = cos(hoekgraad2);
-double J_inv_2_2 = sin(hoekgraad2);
+double y_path[2];
+y_path[0] = 0.0;
+y_path[1] = 0.10;
+
+// for loop
- // Demo path: rectangular
- double x_path[5]; // Matrix heeft 5 elementen: beginnend vanaf element 0 tot en met element 4
- x_path[0] = L0;
- x_path[1] = L0;
- x_path[2] = L0+0.215;
- x_path[3] = L0+0.215;
- x_path[4] = x_path[0];
-
- double y_path[5];
- y_path[0] = 0.0;
- y_path[1] = 0.135;
- y_path[2] = 0.135;
- y_path[3] = 0.0;
- y_path[4] = y_path[0];
-
- // for loop
-
-
-for(int i=0 ; i<=4 ; i++)
+ for(int i=0 ; i<=1 ; i++)
{
double p_new_x = x_path[i];
double p_new_y = y_path[i];
@@ -89,51 +88,44 @@
// printf("x=%f , y=%f , p_dot_new_x=%f , p_dot_new_y=%f\n",p_new_x,p_new_y,p_dot_new_x,p_dot_new_y);
- double angle_old = atan(p_old_y/p_old_x)*180/pi; //Dynamische manier om hoek en lengte verandering te bepalen
- double L_old = sqrt(pow(p_old_x,2)+pow(p_old_y,2));
- double angle_new = atan(p_new_y/p_new_x)*180/pi;
+ double angle_new = atan2(p_new_y,p_new_x)*180/pi;
double L_new = sqrt(pow(p_new_x,2)+pow(p_new_y,2));
- if (angle_new - angle_old <= 0) // als hoekveranding ccw > motor cw > true
+ if (angle_new - hoekrad2 <= 0) // als hoekveranding ccw > motor cw > true
{
motor2direction = true;
}
- else
+ else
{
motor2direction = false;
}
- if (L_new - L_old <= 0 )// als lengteverandering negatief > to base (ccw) > true
+ if (L_new - translatie <= 0 )// als lengteverandering negatief > to base (ccw) > true
{
motor1direction = true;
}
- else
+ else
{
motor1direction = false;
}
- while ( (fabs(p_new_x - p_old_x)) > 0.005 && (fabs(p_new_y - p_old_y)) > 0.005 )
+ while ( (fabs(p_new_x - p_old_x)) > 0.05 && (fabs(p_new_y - p_old_y)) > 0.05 )
{
- double q_dot_angle = (J_inv_1_1 * p_dot_new_x + J_inv_1_2 * p_dot_new_y)*pi/180.0; //hoekgraad2
- double q_dot_L = J_inv_2_1 * p_dot_new_x + J_inv_2_2 * p_dot_new_y; //translatie
- double q_dot_q2 = (q_dot_L/radiuspulley) *pi/180.0; //hoekgraad (translatie) in radialen
- motor1control.write(q_dot_q2);
- wait(0.1);
- motor2control.write(q_dot_angle);
- wait(0.1); // %%%Berekening niet tegelijk, eventuele fout? %%%
-
-
- //Printie op pc
- pc.baud(115200);
- pc.printf("q_dot_L = %f\r\n", q_dot_L );
- pc.printf("q_dot_angle = %f\r\n", q_dot_angle );
- pc.printf("J_inverse = \t %f , %f \r\n \t\t %f , %f \r\n", J_inv_1_1 , J_inv_1_2 , J_inv_2_1 , J_inv_2_2 );
- pc.printf("hoekgraad_trans = %f, translatie = %f, hoekgraad2 = %f \r\n", hoekgraad , translatie, hoekgraad2);
-
-
+ q_dot_angle = (J_inv_1_1 * p_dot_new_x + J_inv_1_2 * p_dot_new_y); //hoekrad2
+ q_dot_L = J_inv_2_1 * p_dot_new_x + J_inv_2_2 * p_dot_new_y; //translatie
+ q_dot_q2 = (q_dot_L/radiuspulley); //hoekrad (translatie) in radialen
+ motor1control.write(q_dot_q2);
+ wait(0.1);
+ motor2control.write(q_dot_angle);
+ wait(0.1); // %%%Berekening niet tegelijk, eventuele fout? %%%
+ //Printie op pc
+ pc.baud(115200);
+ pc.printf("q_dot_L = %f\r\n", q_dot_L );
+ pc.printf("q_dot_angle = %f\r\n", q_dot_angle );
+ pc.printf("J_inverse = \t %f , %f \r\n \t\t %f , %f \r\n", J_inv_1_1 , J_inv_1_2 , J_inv_2_1 , J_inv_2_2 );
+ pc.printf("hoekgraad_trans = %f, translatie = %f, hoekgraad2 = %f \r\n", hoekgraad , translatie, hoekgraad2);
} // End of while
-
- } // End of for
+ } // End of for
} // End of main()
\ No newline at end of file