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@12:21441b04ba29, 2018-10-31 (annotated)

Committer:
TimLu
Date:
Wed Oct 31 15:02:48 2018 +0000
Revision:
12:21441b04ba29
Parent:
11:01372da5a144 
TEST

Who changed what in which revision?

UserRevisionLine numberNew contents of line
TimLu 10:4034134fd7db 1 #include "mbed.h"
Hubertus 0:df553b18547d 2 #include "math.h"
Hubertus 0:df553b18547d 3 #include "BiQuad.h"
SilHeuvelink 8:1efebfebe733 4 #include <string>
SilHeuvelink 8:1efebfebe733 5 #include "QEI.h"
Hubertus 3:be5ac89a0b53 6
TimLu 10:4034134fd7db 7 //----------------- INITIAL -------------------------
SilHeuvelink 8:1efebfebe733 8 QEI Encoder1(D12,D13,NC,64,QEI::X2_ENCODING);
SilHeuvelink 8:1efebfebe733 9 QEI Encoder2(D2,D3,NC,64,QEI::X2_ENCODING);
SilHeuvelink 8:1efebfebe733 10 Ticker EncoderTicker;
Hubertus 6:1f722bf6a89b 11
Hubertus 3:be5ac89a0b53 12 DigitalOut motor1direction(D7);
Hubertus 3:be5ac89a0b53 13 PwmOut motor1control(D6);
SilHeuvelink 8:1efebfebe733 14
Hubertus 6:1f722bf6a89b 15 DigitalOut motor2direction(D4);
Hubertus 6:1f722bf6a89b 16 PwmOut motor2control(D5);
SilHeuvelink 9:d5c561b3ea5a 17
SilHeuvelink 9:d5c561b3ea5a 18 Serial pc(USBTX, USBRX);
Hubertus 0:df553b18547d 19
TimLu 10:4034134fd7db 20 double translatie;
TimLu 10:4034134fd7db 21 double hoekgraad;
TimLu 10:4034134fd7db 22 double hoekgraad2;
TimLu 12:21441b04ba29 23 double hoekrad2;
TimLu 12:21441b04ba29 24 double q_dot_angle;
TimLu 12:21441b04ba29 25 double q_dot_L;
TimLu 12:21441b04ba29 26 double q_dot_q2;
TimLu 12:21441b04ba29 27 double J_inv_1_1;
TimLu 12:21441b04ba29 28 double J_inv_1_2;
TimLu 12:21441b04ba29 29 double J_inv_2_1;
TimLu 12:21441b04ba29 30 double J_inv_2_2;
TimLu 12:21441b04ba29 31 double p_old_x;
TimLu 12:21441b04ba29 32 double p_old_y;
TimLu 12:21441b04ba29 33
TimLu 10:4034134fd7db 34 const float pi = 3.141592653589793; // Value of pi
SilHeuvelink 8:1efebfebe733 35 double gearratio = 3.857142857;
TimLu 10:4034134fd7db 36 double radiuspulley = 0.015915; // Radius pulley [m]
TimLu 10:4034134fd7db 37
TimLu 12:21441b04ba29 38 double K_v = 2; //velocity constant, max 6.667 ?
TimLu 10:4034134fd7db 39 double L0 = 0.09; // starting length
SilHeuvelink 9:d5c561b3ea5a 40
TimLu 10:4034134fd7db 41
TimLu 10:4034134fd7db 42 //-----------------GET ENCODER VALUES -------------------------
SilHeuvelink 9:d5c561b3ea5a 43 void EncoderFunc() {
TimLu 12:21441b04ba29 44 hoekgraad = Encoder1.getPulses() * 0.0857142857;
TimLu 12:21441b04ba29 45 // double hoekrad = hoekgraad * 0.0174532925;
TimLu 12:21441b04ba29 46 hoekgraad2 = Encoder2.getPulses() * 0.0857142857; // Angle arm [degree]
TimLu 12:21441b04ba29 47 hoekrad2 = hoekgraad2 * 0.0174532925;
TimLu 12:21441b04ba29 48 // double hoekarm = hoekgraad2 / gearratio;
TimLu 12:21441b04ba29 49 translatie = (hoekgraad / 180.0) * pi * radiuspulley; // Translatie arm [m]
TimLu 12:21441b04ba29 50
TimLu 12:21441b04ba29 51 //---------------- INVERSE KINEMATICS ---------------------------
TimLu 12:21441b04ba29 52 p_old_x = (translatie+L0)*cos(hoekrad2); // Everytime the x-value from encoder calculated
TimLu 12:21441b04ba29 53 p_old_y = (translatie+L0)*sin(hoekrad2); // Everytime the y-value from encoder calculated
TimLu 12:21441b04ba29 54
TimLu 12:21441b04ba29 55 J_inv_1_1 = -sin(hoekrad2)/(translatie+L0); // Construction of inverse Jacobian
TimLu 12:21441b04ba29 56 J_inv_1_2 = cos(hoekrad2)/(translatie+L0);
TimLu 12:21441b04ba29 57 J_inv_2_1 = cos(hoekrad2);
TimLu 12:21441b04ba29 58 J_inv_2_2 = sin(hoekrad2);
TimLu 12:21441b04ba29 59 }
Hubertus 4:5ceb8f058874 60
SilHeuvelink 8:1efebfebe733 61
SilHeuvelink 8:1efebfebe733 62 int main()
SilHeuvelink 8:1efebfebe733 63 {
TimLu 12:21441b04ba29 64 motor2direction = false; // Nu staan motoren toch op het begin allebei in positieve stand?
TimLu 12:21441b04ba29 65 motor2direction = false;
TimLu 10:4034134fd7db 66
TimLu 12:21441b04ba29 67 EncoderTicker.attach(&EncoderFunc, 0.02);
SilHeuvelink 9:d5c561b3ea5a 68
TimLu 12:21441b04ba29 69 // Demo path: rectangular
TimLu 12:21441b04ba29 70 double x_path[2]; // Matrix heeft 5 elementen: beginnend vanaf element 0 tot en met element 4
TimLu 12:21441b04ba29 71 x_path[0] = L0;
TimLu 12:21441b04ba29 72 x_path[1] = L0;
Hubertus 4:5ceb8f058874 73
TimLu 12:21441b04ba29 74 double y_path[2];
TimLu 12:21441b04ba29 75 y_path[0] = 0.0;
TimLu 12:21441b04ba29 76 y_path[1] = 0.10;
TimLu 12:21441b04ba29 77
TimLu 12:21441b04ba29 78 // for loop
Hubertus 0:df553b18547d 79
Hubertus 0:df553b18547d 80
TimLu 12:21441b04ba29 81 for(int i=0 ; i<=1 ; 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 8:1efebfebe733 91
TimLu 12:21441b04ba29 92 double angle_new = atan2(p_new_y,p_new_x)*180/pi;
SilHeuvelink 8:1efebfebe733 93 double L_new = sqrt(pow(p_new_x,2)+pow(p_new_y,2));
SilHeuvelink 8:1efebfebe733 94
Hubertus 3:be5ac89a0b53 95
TimLu 12:21441b04ba29 96 if (angle_new - hoekrad2 <= 0) // als hoekveranding ccw > motor cw > true
SilHeuvelink 8:1efebfebe733 97 {
SilHeuvelink 9:d5c561b3ea5a 98 motor2direction = true;
SilHeuvelink 9:d5c561b3ea5a 99 }
TimLu 12:21441b04ba29 100 else
TimLu 10:4034134fd7db 101 {
TimLu 10:4034134fd7db 102 motor2direction = false;
TimLu 10:4034134fd7db 103 }
SilHeuvelink 9:d5c561b3ea5a 104
TimLu 12:21441b04ba29 105 if (L_new - translatie <= 0 )// als lengteverandering negatief > to base (ccw) > true
SilHeuvelink 9:d5c561b3ea5a 106 {
SilHeuvelink 9:d5c561b3ea5a 107 motor1direction = true;
SilHeuvelink 9:d5c561b3ea5a 108 }
TimLu 12:21441b04ba29 109 else
TimLu 10:4034134fd7db 110 {
TimLu 10:4034134fd7db 111 motor1direction = false;
TimLu 10:4034134fd7db 112 }
SilHeuvelink 8:1efebfebe733 113
TimLu 12:21441b04ba29 114 while ( (fabs(p_new_x - p_old_x)) > 0.05 && (fabs(p_new_y - p_old_y)) > 0.05 )
SilHeuvelink 8:1efebfebe733 115 {
TimLu 12:21441b04ba29 116 q_dot_angle = (J_inv_1_1 * p_dot_new_x + J_inv_1_2 * p_dot_new_y); //hoekrad2
TimLu 12:21441b04ba29 117 q_dot_L = J_inv_2_1 * p_dot_new_x + J_inv_2_2 * p_dot_new_y; //translatie
TimLu 12:21441b04ba29 118 q_dot_q2 = (q_dot_L/radiuspulley); //hoekrad (translatie) in radialen
TimLu 12:21441b04ba29 119 motor1control.write(q_dot_q2);
TimLu 12:21441b04ba29 120 wait(0.1);
TimLu 12:21441b04ba29 121 motor2control.write(q_dot_angle);
TimLu 12:21441b04ba29 122 wait(0.1); // %%%Berekening niet tegelijk, eventuele fout? %%%
TimLu 12:21441b04ba29 123 //Printie op pc
TimLu 12:21441b04ba29 124 pc.baud(115200);
TimLu 12:21441b04ba29 125 pc.printf("q_dot_L = %f\r\n", q_dot_L );
TimLu 12:21441b04ba29 126 pc.printf("q_dot_angle = %f\r\n", q_dot_angle );
TimLu 12:21441b04ba29 127 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 );
TimLu 12:21441b04ba29 128 pc.printf("hoekgraad_trans = %f, translatie = %f, hoekgraad2 = %f \r\n", hoekgraad , translatie, hoekgraad2);
TimLu 10:4034134fd7db 129 } // End of while
TimLu 12:21441b04ba29 130 } // End of for
TimLu 10:4034134fd7db 131 } // End of main()