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@2:a8ee608177ae, 2018-11-01 (annotated)

Committer:
SilHeuvelink
Date:
Thu Nov 01 10:58:42 2018 +0000
Revision:
2:a8ee608177ae
Parent:
0:2a4ed6c6cdc7
Child:
3:f70ec68723df
Sexy shit;

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 2:a8ee608177ae 24 double K_p1 = 0.5;
SilHeuvelink 2:a8ee608177ae 25 double K_p2 = 0.5;
SilHeuvelink 2:a8ee608177ae 26 double p_desired_x = 0.04;
SilHeuvelink 2:a8ee608177ae 27 double p_desired_y = 0.13;
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 2:a8ee608177ae 42 double error_length_angle;
SilHeuvelink 2:a8ee608177ae 43 double error_angle;
SilHeuvelink 0:2a4ed6c6cdc7 44
SilHeuvelink 0:2a4ed6c6cdc7 45 double p_current_x;
SilHeuvelink 0:2a4ed6c6cdc7 46 double p_current_y;
SilHeuvelink 0:2a4ed6c6cdc7 47
SilHeuvelink 0:2a4ed6c6cdc7 48 void EncoderFunc()
SilHeuvelink 0:2a4ed6c6cdc7 49 {
SilHeuvelink 0:2a4ed6c6cdc7 50 angle_trans = Encoder1.getPulses() * 0.0857142857*0.0174532925; // Translation [rad]
SilHeuvelink 0:2a4ed6c6cdc7 51 translatie = angle_trans * r_pulley; // Translatie arm [m]
SilHeuvelink 0:2a4ed6c6cdc7 52 angle = Encoder2.getPulses() * 0.0857142857*0.0174532925/gearratio; // Angle arm [rad]
SilHeuvelink 0:2a4ed6c6cdc7 53 length = translatie+L0;
SilHeuvelink 0:2a4ed6c6cdc7 54
SilHeuvelink 2:a8ee608177ae 55 p_current_x = (length)*cos(angle)-L0;
SilHeuvelink 0:2a4ed6c6cdc7 56 p_current_y = (length)*sin(angle);
SilHeuvelink 0:2a4ed6c6cdc7 57
SilHeuvelink 2:a8ee608177ae 58 //p_dot_x = K_p1*(p_desired_x - p_current_x);
SilHeuvelink 2:a8ee608177ae 59 //p_dot_y = K_p2*(p_desired_y - p_current_y);
SilHeuvelink 2:a8ee608177ae 60
SilHeuvelink 2:a8ee608177ae 61 angle_desired = atan2(p_desired_y,p_desired_x+L0);
SilHeuvelink 2:a8ee608177ae 62 length_desired = sqrt(pow(p_desired_x+L0,2)+pow(p_desired_y,2));
SilHeuvelink 2:a8ee608177ae 63
SilHeuvelink 2:a8ee608177ae 64 error_length_angle = (length_desired-length)/r_pulley;
SilHeuvelink 2:a8ee608177ae 65 error_angle = angle_desired-angle;
SilHeuvelink 0:2a4ed6c6cdc7 66
SilHeuvelink 2:a8ee608177ae 67 motor1_pwm = K_p1*error_length_angle;
SilHeuvelink 2:a8ee608177ae 68 motor2_pwm = K_p2*error_angle;
SilHeuvelink 2:a8ee608177ae 69
SilHeuvelink 2:a8ee608177ae 70 //Motor 1 (Translatie)
SilHeuvelink 2:a8ee608177ae 71 if (error_length_angle >= 0) {
SilHeuvelink 2:a8ee608177ae 72 motor1direction = false; //Positieve bewegingsrichting (clockwise, towards end)
SilHeuvelink 2:a8ee608177ae 73 }
SilHeuvelink 2:a8ee608177ae 74 else {
SilHeuvelink 2:a8ee608177ae 75 motor1direction = true; // Negatieve bewegingsrichting
SilHeuvelink 2:a8ee608177ae 76 }
SilHeuvelink 2:a8ee608177ae 77 motor1control.write(fabs(motor1_pwm));
SilHeuvelink 0:2a4ed6c6cdc7 78
SilHeuvelink 2:a8ee608177ae 79 //Motor 2 (Rotatie)
SilHeuvelink 2:a8ee608177ae 80 if (error_angle >= 0){
SilHeuvelink 2:a8ee608177ae 81 motor2direction = false; // counterclockwise (arm clockwise)
SilHeuvelink 2:a8ee608177ae 82 }
SilHeuvelink 2:a8ee608177ae 83 else {
SilHeuvelink 2:a8ee608177ae 84 motor2direction = true; // clockwise (arm counterclockwise)
SilHeuvelink 2:a8ee608177ae 85 }
SilHeuvelink 0:2a4ed6c6cdc7 86 motor2control.write(fabs(motor2_pwm));
SilHeuvelink 2:a8ee608177ae 87 }
SilHeuvelink 0:2a4ed6c6cdc7 88
SilHeuvelink 2:a8ee608177ae 89 int main() {
SilHeuvelink 2:a8ee608177ae 90
SilHeuvelink 0:2a4ed6c6cdc7 91 EncoderTicker.attach(&EncoderFunc, 0.02);
SilHeuvelink 0:2a4ed6c6cdc7 92 pc.baud(115200);
SilHeuvelink 0:2a4ed6c6cdc7 93 motor1direction = false;
SilHeuvelink 0:2a4ed6c6cdc7 94 motor2direction = false;
SilHeuvelink 0:2a4ed6c6cdc7 95
SilHeuvelink 0:2a4ed6c6cdc7 96 while(true)
SilHeuvelink 0:2a4ed6c6cdc7 97 {
SilHeuvelink 0:2a4ed6c6cdc7 98 wait(0.1);
SilHeuvelink 2:a8ee608177ae 99 pc.printf("angle = %f, length = %f \r\n", angle*180/pi, length);
SilHeuvelink 0:2a4ed6c6cdc7 100 pc.printf("x = %f, y = %f \r\n", p_current_x, p_current_y);
SilHeuvelink 0:2a4ed6c6cdc7 101 pc.printf("motor1_pwm = %f, motor2_pwm = %f \r\n", motor1_pwm, motor2_pwm);
SilHeuvelink 0:2a4ed6c6cdc7 102 }
SilHeuvelink 0:2a4ed6c6cdc7 103 }