Biorobotica TIC / Mbed 2 deprecated Practice_Run

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:   QEI biquadFilter mbed

main.cpp@1:907507671a38, 2018-10-31 (annotated)

Committer:
TimLu
Date:
Wed Oct 31 19:13:22 2018 +0000
Revision:
1:907507671a38
Parent:
0:2a4ed6c6cdc7 
Laatste versie

Who changed what in which revision?

UserRevisionLine numberNew contents of line
SilHeuvelink 0:2a4ed6c6cdc7 1 #include "mbed.h"
SilHeuvelink 0:2a4ed6c6cdc7 2 #include "math.h"
SilHeuvelink 0:2a4ed6c6cdc7 3 #include "BiQuad.h"
SilHeuvelink 0:2a4ed6c6cdc7 4 #include <string>
SilHeuvelink 0:2a4ed6c6cdc7 5 #include "QEI.h"
SilHeuvelink 0:2a4ed6c6cdc7 6
SilHeuvelink 0:2a4ed6c6cdc7 7 //----------------- INITIAL -------------------------
SilHeuvelink 0:2a4ed6c6cdc7 8 QEI Encoder1(D12,D13,NC,64,QEI::X2_ENCODING);
SilHeuvelink 0:2a4ed6c6cdc7 9 QEI Encoder2(D2,D3,NC,64,QEI::X2_ENCODING);
SilHeuvelink 0:2a4ed6c6cdc7 10 Ticker EncoderTicker;
SilHeuvelink 0:2a4ed6c6cdc7 11
SilHeuvelink 0:2a4ed6c6cdc7 12 DigitalOut motor1direction(D7);
SilHeuvelink 0:2a4ed6c6cdc7 13 PwmOut motor1control(D6);
SilHeuvelink 0:2a4ed6c6cdc7 14
SilHeuvelink 0:2a4ed6c6cdc7 15 DigitalOut motor2direction(D4);
SilHeuvelink 0:2a4ed6c6cdc7 16 PwmOut motor2control(D5);
SilHeuvelink 0:2a4ed6c6cdc7 17
SilHeuvelink 0:2a4ed6c6cdc7 18 Serial pc(USBTX, USBRX);
SilHeuvelink 0:2a4ed6c6cdc7 19
SilHeuvelink 0:2a4ed6c6cdc7 20 // Definitie constanten
SilHeuvelink 0:2a4ed6c6cdc7 21 double L0 = 0.09;
TimLu 1:907507671a38 22 double K_p1 = 0.30;
TimLu 1:907507671a38 23 double K_p2 = 0.30;
TimLu 1:907507671a38 24 double p_desired_x = 0.15;
TimLu 1:907507671a38 25 double p_desired_y = 0.05;
SilHeuvelink 0:2a4ed6c6cdc7 26 double r_pulley = 0.015915;
SilHeuvelink 0:2a4ed6c6cdc7 27 double pi = 3.141592653589793;
SilHeuvelink 0:2a4ed6c6cdc7 28 double gearratio = 3.857142857;
SilHeuvelink 0:2a4ed6c6cdc7 29
SilHeuvelink 0:2a4ed6c6cdc7 30 // Definitie variabelen
SilHeuvelink 0:2a4ed6c6cdc7 31 double angle_trans;
SilHeuvelink 0:2a4ed6c6cdc7 32 double translatie;
SilHeuvelink 0:2a4ed6c6cdc7 33 double angle;
SilHeuvelink 0:2a4ed6c6cdc7 34 double length;
SilHeuvelink 0:2a4ed6c6cdc7 35 double angle_desired;
SilHeuvelink 0:2a4ed6c6cdc7 36 double length_desired;
SilHeuvelink 0:2a4ed6c6cdc7 37 double motor1_pwm;
SilHeuvelink 0:2a4ed6c6cdc7 38 double length_dot;
SilHeuvelink 0:2a4ed6c6cdc7 39 double motor2_pwm;
SilHeuvelink 0:2a4ed6c6cdc7 40
SilHeuvelink 0:2a4ed6c6cdc7 41 double p_current_x;
SilHeuvelink 0:2a4ed6c6cdc7 42 double p_current_y;
SilHeuvelink 0:2a4ed6c6cdc7 43
SilHeuvelink 0:2a4ed6c6cdc7 44 void EncoderFunc()
SilHeuvelink 0:2a4ed6c6cdc7 45 {
SilHeuvelink 0:2a4ed6c6cdc7 46 angle_trans = Encoder1.getPulses() * 0.0857142857*0.0174532925; // Translation [rad]
SilHeuvelink 0:2a4ed6c6cdc7 47 translatie = angle_trans * r_pulley; // Translatie arm [m]
SilHeuvelink 0:2a4ed6c6cdc7 48 angle = Encoder2.getPulses() * 0.0857142857*0.0174532925/gearratio; // Angle arm [rad]
SilHeuvelink 0:2a4ed6c6cdc7 49 length = translatie+L0;
SilHeuvelink 0:2a4ed6c6cdc7 50
SilHeuvelink 0:2a4ed6c6cdc7 51 p_current_x = (length)*cos(angle);
SilHeuvelink 0:2a4ed6c6cdc7 52 p_current_y = (length)*sin(angle);
SilHeuvelink 0:2a4ed6c6cdc7 53
SilHeuvelink 0:2a4ed6c6cdc7 54 //p_dot_x = v_constant*(p_desired_x - p_current_x);
SilHeuvelink 0:2a4ed6c6cdc7 55 //p_dot_y = v_constant*(p_desired_y - p_current_y);
SilHeuvelink 0:2a4ed6c6cdc7 56
TimLu 1:907507671a38 57 angle_desired = atan2(p_desired_y,p_desired_x+L0);
TimLu 1:907507671a38 58 length_desired = sqrt(pow(p_desired_x+L0,2)+pow(p_desired_y,2));
SilHeuvelink 0:2a4ed6c6cdc7 59
SilHeuvelink 0:2a4ed6c6cdc7 60 motor1_pwm = K_p1*(length_desired-length)/r_pulley;
SilHeuvelink 0:2a4ed6c6cdc7 61 motor2_pwm = K_p2*(angle_desired-angle);
SilHeuvelink 0:2a4ed6c6cdc7 62
TimLu 1:907507671a38 63 motor1control.write((motor1_pwm));
TimLu 1:907507671a38 64 motor2control.write((motor2_pwm));
SilHeuvelink 0:2a4ed6c6cdc7 65 //motor1direction = motor1_pwm < 0;
SilHeuvelink 0:2a4ed6c6cdc7 66 //motor2direction = motor2_pwm < 0;
SilHeuvelink 0:2a4ed6c6cdc7 67 }
SilHeuvelink 0:2a4ed6c6cdc7 68
SilHeuvelink 0:2a4ed6c6cdc7 69
SilHeuvelink 0:2a4ed6c6cdc7 70 int main()
SilHeuvelink 0:2a4ed6c6cdc7 71 {
SilHeuvelink 0:2a4ed6c6cdc7 72 EncoderTicker.attach(&EncoderFunc, 0.02);
SilHeuvelink 0:2a4ed6c6cdc7 73 pc.baud(115200);
SilHeuvelink 0:2a4ed6c6cdc7 74 motor1direction = false;
SilHeuvelink 0:2a4ed6c6cdc7 75 motor2direction = false;
SilHeuvelink 0:2a4ed6c6cdc7 76
SilHeuvelink 0:2a4ed6c6cdc7 77 while(true)
SilHeuvelink 0:2a4ed6c6cdc7 78 {
SilHeuvelink 0:2a4ed6c6cdc7 79 wait(0.1);
SilHeuvelink 0:2a4ed6c6cdc7 80 pc.printf("angle = %f, length = %f \r\n", angle, length);
SilHeuvelink 0:2a4ed6c6cdc7 81 pc.printf("x = %f, y = %f \r\n", p_current_x, p_current_y);
TimLu 1:907507671a38 82 pc.printf("motor1_pwm = %f, motor2_pwm = %f \r\n\r\n", motor1_pwm, motor2_pwm);
SilHeuvelink 0:2a4ed6c6cdc7 83 }
SilHeuvelink 0:2a4ed6c6cdc7 84 }