Biorobotica TIC / 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_TEST_LIJN by Tim Luten

main.cpp@9:d5c561b3ea5a, 2018-10-30 (annotated)

Committer:
SilHeuvelink
Date:
Tue Oct 30 16:57:53 2018 +0000
Revision:
9:d5c561b3ea5a
Parent:
8:1efebfebe733
Child:
10:4034134fd7db
Edit 17:00

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);
SilHeuvelink 9:d5c561b3ea5a 18
SilHeuvelink 9:d5c561b3ea5a 19 InterruptIn button1(D10);
SilHeuvelink 9:d5c561b3ea5a 20
SilHeuvelink 9:d5c561b3ea5a 21 Serial pc(USBTX, USBRX);
Hubertus 0:df553b18547d 22
SilHeuvelink 9:d5c561b3ea5a 23
SilHeuvelink 9:d5c561b3ea5a 24 double translatie;
SilHeuvelink 9:d5c561b3ea5a 25 double hoekgraad;
SilHeuvelink 9:d5c561b3ea5a 26 double hoekgraad2;
SilHeuvelink 9:d5c561b3ea5a 27
SilHeuvelink 9:d5c561b3ea5a 28
SilHeuvelink 8:1efebfebe733 29 const float pi = 3.141592653589793; // Value of pi
SilHeuvelink 8:1efebfebe733 30 double gearratio = 3.857142857;
SilHeuvelink 8:1efebfebe733 31 double radiuspulley = 0.015915; // Radius pulley [m]
SilHeuvelink 9:d5c561b3ea5a 32
SilHeuvelink 9:d5c561b3ea5a 33
SilHeuvelink 9:d5c561b3ea5a 34 void EncoderFunc() {
SilHeuvelink 9:d5c561b3ea5a 35 hoekgraad = Encoder1.getPulses() * 0.0857142857; // Angle arm [degree]
SilHeuvelink 9:d5c561b3ea5a 36 // double hoekrad = hoekgraad * 0.0174532925;
SilHeuvelink 9:d5c561b3ea5a 37 hoekgraad2 = Encoder2.getPulses() * 0.0857142857;
SilHeuvelink 9:d5c561b3ea5a 38 // double hoekrad2 = hoekgraad2 * 0.0174532925;
SilHeuvelink 9:d5c561b3ea5a 39 // double hoekarm = hoekgraad2 / gearratio;
SilHeuvelink 9:d5c561b3ea5a 40 translatie = hoekgraad /360.0 * 2.0 * pi * radiuspulley; // Translatie arm [m]
SilHeuvelink 8:1efebfebe733 41 }
Hubertus 4:5ceb8f058874 42
SilHeuvelink 8:1efebfebe733 43 //----------------INVERSE KINEMATICS ---------------------------
SilHeuvelink 9:d5c561b3ea5a 44 double K_v = 1.00; //velocity constant, max 6.667
SilHeuvelink 9:d5c561b3ea5a 45 double L0 = 0.09; // starting length
SilHeuvelink 8:1efebfebe733 46
SilHeuvelink 8:1efebfebe733 47 int main()
SilHeuvelink 8:1efebfebe733 48 {
SilHeuvelink 9:d5c561b3ea5a 49 EncoderTicker.attach(&EncoderFunc, 0.005);
SilHeuvelink 9:d5c561b3ea5a 50 pc.baud(115200);
SilHeuvelink 9:d5c561b3ea5a 51 pc.printf("translatie:%f /n hoekgraad:%f /n hoekgraad2:%f", translatie, hoekgraad, hoekgraad2);
SilHeuvelink 9:d5c561b3ea5a 52
Hubertus 4:5ceb8f058874 53
SilHeuvelink 9:d5c561b3ea5a 54 double p_old_x = (translatie+L0)*cos(hoekgraad2); // Everytime the x-value from encoder calculated
SilHeuvelink 9:d5c561b3ea5a 55 double p_old_y = (translatie+L0)*sin(hoekgraad2); // Everytime the y-value from encoder calculated
Hubertus 4:5ceb8f058874 56
SilHeuvelink 9:d5c561b3ea5a 57 double J_inv_1_1 = -sin(hoekgraad)/translatie; // Construction of inverse Jacobian
SilHeuvelink 8:1efebfebe733 58 double J_inv_1_2 = cos(hoekgraad)/translatie;
SilHeuvelink 8:1efebfebe733 59 double J_inv_2_1 = cos(hoekgraad);
SilHeuvelink 8:1efebfebe733 60 double J_inv_2_2 = sin(hoekgraad);
Hubertus 0:df553b18547d 61
Hubertus 0:df553b18547d 62
SilHeuvelink 8:1efebfebe733 63 // Demo path: rectangular
SilHeuvelink 8:1efebfebe733 64 double x_path[5]; // Matrix heeft 5 elementen: beginnend vanaf element 0 tot en met element 4
SilHeuvelink 8:1efebfebe733 65 x_path[0] = L0;
SilHeuvelink 8:1efebfebe733 66 x_path[1] = L0;
SilHeuvelink 8:1efebfebe733 67 x_path[2] = L0+0.15;
SilHeuvelink 8:1efebfebe733 68 x_path[3] = L0+0.15;
SilHeuvelink 8:1efebfebe733 69 x_path[4] = x_path[0];
SilHeuvelink 8:1efebfebe733 70
SilHeuvelink 8:1efebfebe733 71 double y_path[5];
SilHeuvelink 8:1efebfebe733 72 y_path[0] = 0.0;
SilHeuvelink 8:1efebfebe733 73 y_path[1] = 0.1;
SilHeuvelink 8:1efebfebe733 74 y_path[2] = 0.1;
SilHeuvelink 8:1efebfebe733 75 y_path[3] = 0.0;
SilHeuvelink 8:1efebfebe733 76 y_path[4] = y_path[0];
SilHeuvelink 8:1efebfebe733 77
SilHeuvelink 8:1efebfebe733 78 // for loop
SilHeuvelink 9:d5c561b3ea5a 79
SilHeuvelink 9:d5c561b3ea5a 80
SilHeuvelink 8:1efebfebe733 81 for(int i=0 ; i<=4 ; i++)
SilHeuvelink 8:1efebfebe733 82 {
SilHeuvelink 8:1efebfebe733 83 double p_new_x = x_path[i];
SilHeuvelink 8:1efebfebe733 84 double p_new_y = y_path[i];
Hubertus 4:5ceb8f058874 85
SilHeuvelink 9:d5c561b3ea5a 86 double p_dot_new_x = K_v * (p_new_x - p_old_x); //Snelheid is constante K_V * distance(p_old p_new)
SilHeuvelink 8:1efebfebe733 87 double p_dot_new_y = K_v * (p_new_y - p_old_y);
Hubertus 0:df553b18547d 88
SilHeuvelink 8:1efebfebe733 89 // 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 90
SilHeuvelink 9:d5c561b3ea5a 91 double angle_old = atan(p_old_y/p_old_x)*180/pi; //Dynamische manier om hoek en lengte verandering te bepalen
SilHeuvelink 8:1efebfebe733 92 double L_old = sqrt(pow(p_old_x,2)+pow(p_old_y,2));
SilHeuvelink 8:1efebfebe733 93
SilHeuvelink 8:1efebfebe733 94 double angle_new = atan(p_new_y/p_new_x)*180/pi;
SilHeuvelink 8:1efebfebe733 95 double L_new = sqrt(pow(p_new_x,2)+pow(p_new_y,2));
SilHeuvelink 8:1efebfebe733 96
Hubertus 3:be5ac89a0b53 97
SilHeuvelink 9:d5c561b3ea5a 98 if (angle_new - angle_old <= 0) // als hoekveranding ccw > motor cw > true
SilHeuvelink 8:1efebfebe733 99 {
SilHeuvelink 9:d5c561b3ea5a 100 motor2direction = true;
SilHeuvelink 9:d5c561b3ea5a 101 }
SilHeuvelink 9:d5c561b3ea5a 102 else
SilHeuvelink 9:d5c561b3ea5a 103 {
SilHeuvelink 9:d5c561b3ea5a 104 motor2direction = false;
SilHeuvelink 9:d5c561b3ea5a 105 }
SilHeuvelink 9:d5c561b3ea5a 106
SilHeuvelink 9:d5c561b3ea5a 107 if (L_new - L_old <= 0 )// als lengteverandering negatief > to base (ccw) > true
SilHeuvelink 9:d5c561b3ea5a 108 {
SilHeuvelink 9:d5c561b3ea5a 109 motor1direction = true;
SilHeuvelink 9:d5c561b3ea5a 110 }
SilHeuvelink 9:d5c561b3ea5a 111 else
SilHeuvelink 9:d5c561b3ea5a 112 {
SilHeuvelink 9:d5c561b3ea5a 113 motor1direction = false;
SilHeuvelink 9:d5c561b3ea5a 114 }
SilHeuvelink 8:1efebfebe733 115
SilHeuvelink 8:1efebfebe733 116 while ( (fabs(p_new_x - p_old_x)) > 0.005 || (fabs(p_new_y - p_old_y)) > 0.005 )
SilHeuvelink 8:1efebfebe733 117 {
SilHeuvelink 8:1efebfebe733 118 double q_dot_angle = J_inv_1_1 * p_dot_new_x + J_inv_1_2 * p_dot_new_y; //hoekgraad1
SilHeuvelink 8:1efebfebe733 119 double q_dot_L = J_inv_2_1 * p_dot_new_x + J_inv_2_2 * p_dot_new_y; //translatie
SilHeuvelink 8:1efebfebe733 120 double q_dot_q2 = q_dot_L/radiuspulley; //hoekgraad2 (translatie)
SilHeuvelink 8:1efebfebe733 121 motor1control.write(q_dot_angle);
SilHeuvelink 8:1efebfebe733 122 wait(0.1);
SilHeuvelink 8:1efebfebe733 123 motor2control.write(q_dot_q2);
SilHeuvelink 8:1efebfebe733 124 wait(0.1); // %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Berekening niet tegelijk, eventuele fout? %%%
Hubertus 0:df553b18547d 125 }
Hubertus 0:df553b18547d 126 }
Hubertus 0:df553b18547d 127 }