Biorobotica TIC / Mbed 2 deprecated Practice_Run_1

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

Fork of Practice_Run by Biorobotica TIC

main.cpp@0:2a4ed6c6cdc7, 2018-10-31 (annotated)

Committer:
SilHeuvelink
Date:
Wed Oct 31 17:49:26 2018 +0000
Revision:
0:2a4ed6c6cdc7
Child:
1:907507671a38
Child:
2:a8ee608177ae
Lekker beunen werkt voor een halve meter

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 InterruptIn button1(D10);
SilHeuvelink 0:2a4ed6c6cdc7 19
SilHeuvelink 0:2a4ed6c6cdc7 20 Serial pc(USBTX, USBRX);
SilHeuvelink 0:2a4ed6c6cdc7 21
SilHeuvelink 0:2a4ed6c6cdc7 22 // Definitie constanten
SilHeuvelink 0:2a4ed6c6cdc7 23 double L0 = 0.09;
SilHeuvelink 0:2a4ed6c6cdc7 24 double K_p1 = 0.02;
SilHeuvelink 0:2a4ed6c6cdc7 25 double K_p2 = 0.20;
SilHeuvelink 0:2a4ed6c6cdc7 26 double p_desired_x = 0.2;
SilHeuvelink 0:2a4ed6c6cdc7 27 double p_desired_y = 0.2;
SilHeuvelink 0:2a4ed6c6cdc7 28 double r_pulley = 0.015915;
SilHeuvelink 0:2a4ed6c6cdc7 29 double pi = 3.141592653589793;
SilHeuvelink 0:2a4ed6c6cdc7 30 double gearratio = 3.857142857;
SilHeuvelink 0:2a4ed6c6cdc7 31
SilHeuvelink 0:2a4ed6c6cdc7 32 // Definitie variabelen
SilHeuvelink 0:2a4ed6c6cdc7 33 double angle_trans;
SilHeuvelink 0:2a4ed6c6cdc7 34 double translatie;
SilHeuvelink 0:2a4ed6c6cdc7 35 double angle;
SilHeuvelink 0:2a4ed6c6cdc7 36 double length;
SilHeuvelink 0:2a4ed6c6cdc7 37 double angle_desired;
SilHeuvelink 0:2a4ed6c6cdc7 38 double length_desired;
SilHeuvelink 0:2a4ed6c6cdc7 39 double motor1_pwm;
SilHeuvelink 0:2a4ed6c6cdc7 40 double length_dot;
SilHeuvelink 0:2a4ed6c6cdc7 41 double motor2_pwm;
SilHeuvelink 0:2a4ed6c6cdc7 42
SilHeuvelink 0:2a4ed6c6cdc7 43 double p_current_x;
SilHeuvelink 0:2a4ed6c6cdc7 44 double p_current_y;
SilHeuvelink 0:2a4ed6c6cdc7 45
SilHeuvelink 0:2a4ed6c6cdc7 46 void EncoderFunc()
SilHeuvelink 0:2a4ed6c6cdc7 47 {
SilHeuvelink 0:2a4ed6c6cdc7 48 angle_trans = Encoder1.getPulses() * 0.0857142857*0.0174532925; // Translation [rad]
SilHeuvelink 0:2a4ed6c6cdc7 49 translatie = angle_trans * r_pulley; // Translatie arm [m]
SilHeuvelink 0:2a4ed6c6cdc7 50 angle = Encoder2.getPulses() * 0.0857142857*0.0174532925/gearratio; // Angle arm [rad]
SilHeuvelink 0:2a4ed6c6cdc7 51 length = translatie+L0;
SilHeuvelink 0:2a4ed6c6cdc7 52
SilHeuvelink 0:2a4ed6c6cdc7 53 p_current_x = (length)*cos(angle);
SilHeuvelink 0:2a4ed6c6cdc7 54 p_current_y = (length)*sin(angle);
SilHeuvelink 0:2a4ed6c6cdc7 55
SilHeuvelink 0:2a4ed6c6cdc7 56 //p_dot_x = v_constant*(p_desired_x - p_current_x);
SilHeuvelink 0:2a4ed6c6cdc7 57 //p_dot_y = v_constant*(p_desired_y - p_current_y);
SilHeuvelink 0:2a4ed6c6cdc7 58
SilHeuvelink 0:2a4ed6c6cdc7 59 angle_desired = atan2(p_desired_y,p_desired_x);
SilHeuvelink 0:2a4ed6c6cdc7 60 length_desired = sqrt(pow(p_desired_x,2)+pow(p_desired_y,2));
SilHeuvelink 0:2a4ed6c6cdc7 61
SilHeuvelink 0:2a4ed6c6cdc7 62 motor1_pwm = K_p1*(length_desired-length)/r_pulley;
SilHeuvelink 0:2a4ed6c6cdc7 63 motor2_pwm = K_p2*(angle_desired-angle);
SilHeuvelink 0:2a4ed6c6cdc7 64
SilHeuvelink 0:2a4ed6c6cdc7 65 motor1control.write(fabs(motor1_pwm));
SilHeuvelink 0:2a4ed6c6cdc7 66 motor2control.write(fabs(motor2_pwm));
SilHeuvelink 0:2a4ed6c6cdc7 67 //motor1direction = motor1_pwm < 0;
SilHeuvelink 0:2a4ed6c6cdc7 68 //motor2direction = motor2_pwm < 0;
SilHeuvelink 0:2a4ed6c6cdc7 69 }
SilHeuvelink 0:2a4ed6c6cdc7 70
SilHeuvelink 0:2a4ed6c6cdc7 71
SilHeuvelink 0:2a4ed6c6cdc7 72 int main()
SilHeuvelink 0:2a4ed6c6cdc7 73 {
SilHeuvelink 0:2a4ed6c6cdc7 74 EncoderTicker.attach(&EncoderFunc, 0.02);
SilHeuvelink 0:2a4ed6c6cdc7 75 pc.baud(115200);
SilHeuvelink 0:2a4ed6c6cdc7 76 motor1direction = false;
SilHeuvelink 0:2a4ed6c6cdc7 77 motor2direction = false;
SilHeuvelink 0:2a4ed6c6cdc7 78
SilHeuvelink 0:2a4ed6c6cdc7 79 while(true)
SilHeuvelink 0:2a4ed6c6cdc7 80 {
SilHeuvelink 0:2a4ed6c6cdc7 81 wait(0.1);
SilHeuvelink 0:2a4ed6c6cdc7 82 pc.printf("angle = %f, length = %f \r\n", angle, length);
SilHeuvelink 0:2a4ed6c6cdc7 83 pc.printf("x = %f, y = %f \r\n", p_current_x, p_current_y);
SilHeuvelink 0:2a4ed6c6cdc7 84 pc.printf("motor1_pwm = %f, motor2_pwm = %f \r\n", motor1_pwm, motor2_pwm);
SilHeuvelink 0:2a4ed6c6cdc7 85 }
SilHeuvelink 0:2a4ed6c6cdc7 86 }