Tim Luten / Mbed 2 deprecated 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

main.cpp@8:1efebfebe733, 2018-10-30 (annotated)

Committer:
SilHeuvelink
Date:
Tue Oct 30 12:17:35 2018 +0000
Revision:
8:1efebfebe733
Parent:
7:b53f0c4cf2b9
Child:
9:d5c561b3ea5a
test1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Hubertus 0:df553b18547d 1 #include "mbed.h"
Hubertus 0:df553b18547d 2 #include "math.h"
Hubertus 0:df553b18547d 3 #include "BiQuad.h"
Hubertus 6:1f722bf6a89b 4 #include "Servo.h"
SilHeuvelink 8:1efebfebe733 5 #include <string>
SilHeuvelink 8:1efebfebe733 6 #include "QEI.h"
Hubertus 3:be5ac89a0b53 7
SilHeuvelink 8:1efebfebe733 8 //-----------------GET ENCODER VALUES -------------------------
SilHeuvelink 8:1efebfebe733 9 QEI Encoder1(D12,D13,NC,64,QEI::X2_ENCODING);
SilHeuvelink 8:1efebfebe733 10 QEI Encoder2(D2,D3,NC,64,QEI::X2_ENCODING);
SilHeuvelink 8:1efebfebe733 11 Ticker EncoderTicker;
Hubertus 6:1f722bf6a89b 12
Hubertus 3:be5ac89a0b53 13 DigitalOut motor1direction(D7);
Hubertus 3:be5ac89a0b53 14 PwmOut motor1control(D6);
SilHeuvelink 8:1efebfebe733 15
Hubertus 6:1f722bf6a89b 16 DigitalOut motor2direction(D4);
Hubertus 6:1f722bf6a89b 17 PwmOut motor2control(D5);
Hubertus 0:df553b18547d 18
SilHeuvelink 8:1efebfebe733 19 void EncoderFunc() {
SilHeuvelink 8:1efebfebe733 20
SilHeuvelink 8:1efebfebe733 21 const float pi = 3.141592653589793; // Value of pi
SilHeuvelink 8:1efebfebe733 22 double gearratio = 3.857142857;
SilHeuvelink 8:1efebfebe733 23 double radiuspulley = 0.015915; // Radius pulley [m]
SilHeuvelink 8:1efebfebe733 24 double hoekgraad = Encoder1.getPulses() * 0.0857142857; // Angle arm [degree]
SilHeuvelink 8:1efebfebe733 25 double hoekrad = hoekgraad * 0.0174532925;
SilHeuvelink 8:1efebfebe733 26 double hoekgraad2 = Encoder2.getPulses() * 0.0857142857;
SilHeuvelink 8:1efebfebe733 27 double hoekrad2 = hoekgraad2 * 0.0174532925;
SilHeuvelink 8:1efebfebe733 28 double hoekarm = hoekgraad2 / gearratio;
SilHeuvelink 8:1efebfebe733 29 double translatie = hoekgraad /360 * 2 * pi * radiuspulley; // Translatie arm [m]
SilHeuvelink 8:1efebfebe733 30 }
Hubertus 4:5ceb8f058874 31
SilHeuvelink 8:1efebfebe733 32 //----------------INVERSE KINEMATICS ---------------------------
SilHeuvelink 8:1efebfebe733 33 double K_v = 1; // Velocity constant (VALUE???) Maximaal 6.6667
SilHeuvelink 8:1efebfebe733 34 double L0 = 0.09;
SilHeuvelink 8:1efebfebe733 35
SilHeuvelink 8:1efebfebe733 36 int main()
SilHeuvelink 8:1efebfebe733 37 {
SilHeuvelink 8:1efebfebe733 38 EncoderTicker.attach(&EncoderFunc, 1);
Hubertus 4:5ceb8f058874 39
SilHeuvelink 8:1efebfebe733 40 double p_old_x = (translatie+L0)*cos(hoekgraad); // Everytime the x-value from encoder calculated
SilHeuvelink 8:1efebfebe733 41 double p_old_y = (translatie+L0)*sin(hoekgraad); // Everytime the y-value from encoder calculated
Hubertus 4:5ceb8f058874 42
SilHeuvelink 8:1efebfebe733 43 double J_inv_1_1 = -sin(hoekgraad)/translatie;
SilHeuvelink 8:1efebfebe733 44 double J_inv_1_2 = cos(hoekgraad)/translatie;
SilHeuvelink 8:1efebfebe733 45 double J_inv_2_1 = cos(hoekgraad);
SilHeuvelink 8:1efebfebe733 46 double J_inv_2_2 = sin(hoekgraad);
Hubertus 0:df553b18547d 47
Hubertus 0:df553b18547d 48
SilHeuvelink 8:1efebfebe733 49 // Demo path: rectangular
SilHeuvelink 8:1efebfebe733 50 double x_path[5]; // Matrix heeft 5 elementen: beginnend vanaf element 0 tot en met element 4
SilHeuvelink 8:1efebfebe733 51 x_path[0] = L0;
SilHeuvelink 8:1efebfebe733 52 x_path[1] = L0;
SilHeuvelink 8:1efebfebe733 53 x_path[2] = L0+0.15;
SilHeuvelink 8:1efebfebe733 54 x_path[3] = L0+0.15;
SilHeuvelink 8:1efebfebe733 55 x_path[4] = x_path[0];
SilHeuvelink 8:1efebfebe733 56
SilHeuvelink 8:1efebfebe733 57 double y_path[5];
SilHeuvelink 8:1efebfebe733 58 y_path[0] = 0.0;
SilHeuvelink 8:1efebfebe733 59 y_path[1] = 0.1;
SilHeuvelink 8:1efebfebe733 60 y_path[2] = 0.1;
SilHeuvelink 8:1efebfebe733 61 y_path[3] = 0.0;
SilHeuvelink 8:1efebfebe733 62 y_path[4] = y_path[0];
SilHeuvelink 8:1efebfebe733 63
SilHeuvelink 8:1efebfebe733 64 // for loop
SilHeuvelink 8:1efebfebe733 65 for(int i=0 ; i<=4 ; i++)
SilHeuvelink 8:1efebfebe733 66 {
SilHeuvelink 8:1efebfebe733 67 double p_new_x = x_path[i];
SilHeuvelink 8:1efebfebe733 68 double p_new_y = y_path[i];
Hubertus 4:5ceb8f058874 69
SilHeuvelink 8:1efebfebe733 70 double p_dot_new_x = K_v * (p_new_x - p_old_x);
SilHeuvelink 8:1efebfebe733 71 double p_dot_new_y = K_v * (p_new_y - p_old_y);
Hubertus 0:df553b18547d 72
SilHeuvelink 8:1efebfebe733 73 // 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);
Hubertus 4:5ceb8f058874 74
SilHeuvelink 8:1efebfebe733 75 double angle_old = atan(p_old_y/p_old_x)*180/pi;
SilHeuvelink 8:1efebfebe733 76 double L_old = sqrt(pow(p_old_x,2)+pow(p_old_y,2));
SilHeuvelink 8:1efebfebe733 77
SilHeuvelink 8:1efebfebe733 78 double angle_new = atan(p_new_y/p_new_x)*180/pi;
SilHeuvelink 8:1efebfebe733 79 double L_new = sqrt(pow(p_new_x,2)+pow(p_new_y,2));
SilHeuvelink 8:1efebfebe733 80
SilHeuvelink 8:1efebfebe733 81 if (angle_new - angle_old) <= 0
SilHeuvelink 8:1efebfebe733 82 {
SilHeuvelink 8:1efebfebe733 83 motor1direction.write(direction1 = !direction1);
SilHeuvelink 8:1efebfebe733 84 }
Hubertus 3:be5ac89a0b53 85
SilHeuvelink 8:1efebfebe733 86 if (L_new - L_old) <= 0
SilHeuvelink 8:1efebfebe733 87 {
SilHeuvelink 8:1efebfebe733 88 motor2direction.write(direction2 = !direction2);
SilHeuvelink 8:1efebfebe733 89 }
SilHeuvelink 8:1efebfebe733 90
SilHeuvelink 8:1efebfebe733 91 while ( (fabs(p_new_x - p_old_x)) > 0.005 || (fabs(p_new_y - p_old_y)) > 0.005 )
SilHeuvelink 8:1efebfebe733 92 {
SilHeuvelink 8:1efebfebe733 93 double q_dot_angle = J_inv_1_1 * p_dot_new_x + J_inv_1_2 * p_dot_new_y; //hoekgraad1
SilHeuvelink 8:1efebfebe733 94 double q_dot_L = J_inv_2_1 * p_dot_new_x + J_inv_2_2 * p_dot_new_y; //translatie
SilHeuvelink 8:1efebfebe733 95 double q_dot_q2 = q_dot_L/radiuspulley; //hoekgraad2 (translatie)
SilHeuvelink 8:1efebfebe733 96 motor1control.write(q_dot_angle);
SilHeuvelink 8:1efebfebe733 97 wait(0.1);
SilHeuvelink 8:1efebfebe733 98 motor2control.write(q_dot_q2);
SilHeuvelink 8:1efebfebe733 99 wait(0.1); // %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Berekening niet tegelijk, eventuele fout? %%%
Hubertus 0:df553b18547d 100 }
Hubertus 0:df553b18547d 101 }
Hubertus 0:df553b18547d 102 }