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@11:01372da5a144, 2018-10-31 (annotated)

Committer:
TimLu
Date:
Wed Oct 31 11:50:31 2018 +0000
Revision:
11:01372da5a144
Parent:
10:4034134fd7db
Child:
12:21441b04ba29
Poop;

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