Tim Luten
/
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_TEST3
by 
Biorobotica TIC
Revision 10:4034134fd7db, committed 2018-10-31
- Comitter:
- TimLu
- Date:
- Wed Oct 31 08:18:51 2018 +0000
- Parent:
- 9:d5c561b3ea5a
- Child:
- 11:01372da5a144
- Commit message:
- Updated
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Tue Oct 30 16:57:53 2018 +0000
+++ b/main.cpp Wed Oct 31 08:18:51 2018 +0000
@@ -1,11 +1,11 @@
-#include "mbed.h"
+ #include "mbed.h"
#include "math.h"
#include "BiQuad.h"
#include "Servo.h"
#include <string>
#include "QEI.h"
-//-----------------GET ENCODER VALUES -------------------------
+//----------------- INITIAL -------------------------
QEI Encoder1(D12,D13,NC,64,QEI::X2_ENCODING);
QEI Encoder2(D2,D3,NC,64,QEI::X2_ENCODING);
Ticker EncoderTicker;
@@ -20,62 +20,64 @@
Serial pc(USBTX, USBRX);
-
- double translatie;
- double hoekgraad;
- double hoekgraad2;
-
-
- const float pi = 3.141592653589793; // Value of pi
+ double translatie;
+ double hoekgraad;
+ double hoekgraad2;
+
+ const float pi = 3.141592653589793; // Value of pi
double gearratio = 3.857142857;
- double radiuspulley = 0.015915; // Radius pulley [m]
+ double radiuspulley = 0.015915; // Radius pulley [m]
+
+ double K_v = 1.00; //velocity constant, max 6.667 ?
+ double L0 = 0.09; // starting length
-
+
+//-----------------GET ENCODER VALUES -------------------------
void EncoderFunc() {
- hoekgraad = Encoder1.getPulses() * 0.0857142857; // Angle arm [degree]
- // double hoekrad = hoekgraad * 0.0174532925;
- hoekgraad2 = Encoder2.getPulses() * 0.0857142857;
- // double hoekrad2 = hoekgraad2 * 0.0174532925;
- // double hoekarm = hoekgraad2 / gearratio;
+ 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]
}
-//----------------INVERSE KINEMATICS ---------------------------
-double K_v = 1.00; //velocity constant, max 6.667
-double L0 = 0.09; // starting length
int main()
{
+ motor2direction = false; // Nu staan motoren toch op het begin allebei in positieve stand?
+ motor2direction = false;
+
EncoderTicker.attach(&EncoderFunc, 0.005);
pc.baud(115200);
-pc.printf("translatie:%f /n hoekgraad:%f /n hoekgraad2:%f", translatie, hoekgraad, hoekgraad2);
+pc.printf("hoekgraad=%f degrees\t translatie:%f meters /t hoekgraad2:%f degrees /n",hoekgraad, translatie, hoekgraad2);
-
+//---------------- 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
-double J_inv_1_1 = -sin(hoekgraad)/translatie; // Construction of inverse Jacobian
-double J_inv_1_2 = cos(hoekgraad)/translatie;
-double J_inv_2_1 = cos(hoekgraad);
-double J_inv_2_2 = sin(hoekgraad);
+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);
-// Demo path: rectangular
+ // 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.15;
- x_path[3] = L0+0.15;
+ 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.1;
- y_path[2] = 0.1;
+ y_path[1] = 0.135;
+ y_path[2] = 0.135;
y_path[3] = 0.0;
y_path[4] = y_path[0];
-// for loop
+ // for loop
for(int i=0 ; i<=4 ; i++)
@@ -99,29 +101,37 @@
{
motor2direction = true;
}
- else
- {
- motor2direction = false;
- }
+ else
+ {
+ motor2direction = false;
+ }
- if (L_new - L_old <= 0 )// als lengteverandering negatief > to base (ccw) > true
+ if (L_new - L_old <= 0 )// als lengteverandering negatief > to base (ccw) > true
{
motor1direction = true;
}
- else
- {
- motor1direction = false;
- }
+ 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.005 && (fabs(p_new_y - p_old_y)) > 0.005 )
{
- double q_dot_angle = J_inv_1_1 * p_dot_new_x + J_inv_1_2 * p_dot_new_y; //hoekgraad1
+ 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; //hoekgraad2 (translatie)
- motor1control.write(q_dot_angle);
+ 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_q2);
- wait(0.1); // %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Berekening niet tegelijk, eventuele fout? %%%
- }
- }
-}
\ No newline at end of file
+ motor2control.write(q_dot_angle);
+ wait(0.1); // %%%Berekening niet tegelijk, eventuele fout? %%%
+
+ void setMotor1(float motorValue) {
+ // Given motorValue<=1, writes the velocity to the pwm control.
+ // MotorValues outside range are truncated to within range.
+ motor1control.write(fabs(motorValue) > 1 ? 1 : fabs(motorValue));
+
+
+ } // End of while
+
+ } // End of for
+} // End of main()
\ No newline at end of file