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@10:4034134fd7db, 2018-10-31 (annotated)

Committer:
TimLu
Date:
Wed Oct 31 08:18:51 2018 +0000
Revision:
10:4034134fd7db
Parent:
9:d5c561b3ea5a
Child:
11:01372da5a144
Updated

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