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

Committer:
SilHeuvelink
Date:
Thu Nov 01 14:33:27 2018 +0000
Revision:
3:f70ec68723df
Parent:
2:a8ee608177ae 
Gefixt!;

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 3:f70ec68723df 18 InterruptIn casebutton1(PTA4); //Button for switching the cases
SilHeuvelink 3:f70ec68723df 19 InterruptIn casebutton2(PTC6); //Button for switching the cases
SilHeuvelink 0:2a4ed6c6cdc7 20
SilHeuvelink 3:f70ec68723df 21 InterruptIn button1(D10); //Button for switching x-axes and for getting zero and max
SilHeuvelink 3:f70ec68723df 22 InterruptIn button2(D11);
SilHeuvelink 0:2a4ed6c6cdc7 23 Serial pc(USBTX, USBRX);
SilHeuvelink 0:2a4ed6c6cdc7 24
SilHeuvelink 0:2a4ed6c6cdc7 25 // Definitie constanten
SilHeuvelink 0:2a4ed6c6cdc7 26 double L0 = 0.09;
SilHeuvelink 3:f70ec68723df 27 double K_p1 = 2.0;
SilHeuvelink 3:f70ec68723df 28 double K_p2 = 7.0;
SilHeuvelink 3:f70ec68723df 29 double v_x = 0.02; // speed in m/s
SilHeuvelink 3:f70ec68723df 30 double v_y = 0.02; // speed in m/s
SilHeuvelink 3:f70ec68723df 31
SilHeuvelink 3:f70ec68723df 32 double motorValue1;
SilHeuvelink 3:f70ec68723df 33 double motorValue2;
SilHeuvelink 3:f70ec68723df 34
SilHeuvelink 0:2a4ed6c6cdc7 35 double r_pulley = 0.015915;
SilHeuvelink 0:2a4ed6c6cdc7 36 double pi = 3.141592653589793;
SilHeuvelink 0:2a4ed6c6cdc7 37 double gearratio = 3.857142857;
SilHeuvelink 0:2a4ed6c6cdc7 38
SilHeuvelink 3:f70ec68723df 39 Ticker motor1ticker;
SilHeuvelink 3:f70ec68723df 40 Ticker motor2ticker;
SilHeuvelink 3:f70ec68723df 41
SilHeuvelink 0:2a4ed6c6cdc7 42 // Definitie variabelen
SilHeuvelink 0:2a4ed6c6cdc7 43 double angle_trans;
SilHeuvelink 0:2a4ed6c6cdc7 44 double translatie;
SilHeuvelink 0:2a4ed6c6cdc7 45 double angle;
SilHeuvelink 0:2a4ed6c6cdc7 46 double length;
SilHeuvelink 0:2a4ed6c6cdc7 47 double angle_desired;
SilHeuvelink 0:2a4ed6c6cdc7 48 double length_desired;
SilHeuvelink 0:2a4ed6c6cdc7 49 double motor1_pwm;
SilHeuvelink 0:2a4ed6c6cdc7 50 double length_dot;
SilHeuvelink 0:2a4ed6c6cdc7 51 double motor2_pwm;
SilHeuvelink 2:a8ee608177ae 52 double error_length_angle;
SilHeuvelink 2:a8ee608177ae 53 double error_angle;
SilHeuvelink 3:f70ec68723df 54 double p_desired_x = 0.002;
SilHeuvelink 3:f70ec68723df 55 double p_desired_y = 0.002;
SilHeuvelink 0:2a4ed6c6cdc7 56 double p_current_x;
SilHeuvelink 0:2a4ed6c6cdc7 57 double p_current_y;
SilHeuvelink 0:2a4ed6c6cdc7 58
SilHeuvelink 3:f70ec68723df 59 void setMotor1() {
SilHeuvelink 3:f70ec68723df 60 motor1control.write(motorValue1);
SilHeuvelink 3:f70ec68723df 61 }
SilHeuvelink 3:f70ec68723df 62
SilHeuvelink 3:f70ec68723df 63 void setMotor2() {
SilHeuvelink 3:f70ec68723df 64 motor2control.write(motorValue2);
SilHeuvelink 3:f70ec68723df 65 }
SilHeuvelink 3:f70ec68723df 66
SilHeuvelink 0:2a4ed6c6cdc7 67 void EncoderFunc()
SilHeuvelink 0:2a4ed6c6cdc7 68 {
SilHeuvelink 0:2a4ed6c6cdc7 69 angle_trans = Encoder1.getPulses() * 0.0857142857*0.0174532925; // Translation [rad]
SilHeuvelink 0:2a4ed6c6cdc7 70 translatie = angle_trans * r_pulley; // Translatie arm [m]
SilHeuvelink 0:2a4ed6c6cdc7 71 angle = Encoder2.getPulses() * 0.0857142857*0.0174532925/gearratio; // Angle arm [rad]
SilHeuvelink 0:2a4ed6c6cdc7 72 length = translatie+L0;
SilHeuvelink 3:f70ec68723df 73
SilHeuvelink 3:f70ec68723df 74 if (button2 <=0.5){ //Button ingedrukt
SilHeuvelink 3:f70ec68723df 75 p_desired_x = p_desired_x + v_x*0.02;
SilHeuvelink 3:f70ec68723df 76 }
SilHeuvelink 3:f70ec68723df 77 else {
SilHeuvelink 3:f70ec68723df 78 p_desired_x = p_desired_x;
SilHeuvelink 3:f70ec68723df 79 }
SilHeuvelink 3:f70ec68723df 80 if (button1 <=0.5){ //Button ingedrukt
SilHeuvelink 3:f70ec68723df 81 p_desired_y = p_desired_y + v_y*0.02;
SilHeuvelink 3:f70ec68723df 82 }
SilHeuvelink 3:f70ec68723df 83 else {
SilHeuvelink 3:f70ec68723df 84 p_desired_y = p_desired_y;
SilHeuvelink 3:f70ec68723df 85 }
SilHeuvelink 3:f70ec68723df 86
SilHeuvelink 3:f70ec68723df 87 if (casebutton2 == 0) {
SilHeuvelink 3:f70ec68723df 88 v_x = -v_x;
SilHeuvelink 3:f70ec68723df 89 }
SilHeuvelink 3:f70ec68723df 90 if (casebutton1 == 0) {
SilHeuvelink 3:f70ec68723df 91 v_y = -v_y;
SilHeuvelink 3:f70ec68723df 92 }
SilHeuvelink 0:2a4ed6c6cdc7 93
SilHeuvelink 2:a8ee608177ae 94 p_current_x = (length)*cos(angle)-L0;
SilHeuvelink 0:2a4ed6c6cdc7 95 p_current_y = (length)*sin(angle);
SilHeuvelink 0:2a4ed6c6cdc7 96
SilHeuvelink 2:a8ee608177ae 97 //p_dot_x = K_p1*(p_desired_x - p_current_x);
SilHeuvelink 2:a8ee608177ae 98 //p_dot_y = K_p2*(p_desired_y - p_current_y);
SilHeuvelink 2:a8ee608177ae 99
SilHeuvelink 2:a8ee608177ae 100 angle_desired = atan2(p_desired_y,p_desired_x+L0);
SilHeuvelink 2:a8ee608177ae 101 length_desired = sqrt(pow(p_desired_x+L0,2)+pow(p_desired_y,2));
SilHeuvelink 2:a8ee608177ae 102
SilHeuvelink 2:a8ee608177ae 103 error_length_angle = (length_desired-length)/r_pulley;
SilHeuvelink 2:a8ee608177ae 104 error_angle = angle_desired-angle;
SilHeuvelink 0:2a4ed6c6cdc7 105
SilHeuvelink 2:a8ee608177ae 106 motor1_pwm = K_p1*error_length_angle;
SilHeuvelink 2:a8ee608177ae 107 motor2_pwm = K_p2*error_angle;
SilHeuvelink 2:a8ee608177ae 108
SilHeuvelink 2:a8ee608177ae 109 //Motor 1 (Translatie)
SilHeuvelink 3:f70ec68723df 110 if (motor1_pwm >= 0) {
SilHeuvelink 2:a8ee608177ae 111 motor1direction = false; //Positieve bewegingsrichting (clockwise, towards end)
SilHeuvelink 2:a8ee608177ae 112 }
SilHeuvelink 2:a8ee608177ae 113 else {
SilHeuvelink 2:a8ee608177ae 114 motor1direction = true; // Negatieve bewegingsrichting
SilHeuvelink 2:a8ee608177ae 115 }
SilHeuvelink 3:f70ec68723df 116 motorValue1 = fabs(motor1_pwm);
SilHeuvelink 0:2a4ed6c6cdc7 117
SilHeuvelink 2:a8ee608177ae 118 //Motor 2 (Rotatie)
SilHeuvelink 3:f70ec68723df 119 if (motor2_pwm >= 0){
SilHeuvelink 2:a8ee608177ae 120 motor2direction = false; // counterclockwise (arm clockwise)
SilHeuvelink 2:a8ee608177ae 121 }
SilHeuvelink 2:a8ee608177ae 122 else {
SilHeuvelink 2:a8ee608177ae 123 motor2direction = true; // clockwise (arm counterclockwise)
SilHeuvelink 2:a8ee608177ae 124 }
SilHeuvelink 3:f70ec68723df 125 motorValue2 = fabs(motor2_pwm);
SilHeuvelink 2:a8ee608177ae 126 }
SilHeuvelink 0:2a4ed6c6cdc7 127
SilHeuvelink 2:a8ee608177ae 128 int main() {
SilHeuvelink 2:a8ee608177ae 129
SilHeuvelink 3:f70ec68723df 130 motor1ticker.attach(&setMotor1, 0.02);
SilHeuvelink 3:f70ec68723df 131 motor2ticker.attach(&setMotor2, 0.02);
SilHeuvelink 3:f70ec68723df 132
SilHeuvelink 0:2a4ed6c6cdc7 133 EncoderTicker.attach(&EncoderFunc, 0.02);
SilHeuvelink 0:2a4ed6c6cdc7 134 pc.baud(115200);
SilHeuvelink 0:2a4ed6c6cdc7 135 motor1direction = false;
SilHeuvelink 0:2a4ed6c6cdc7 136 motor2direction = false;
SilHeuvelink 0:2a4ed6c6cdc7 137
SilHeuvelink 0:2a4ed6c6cdc7 138 while(true)
SilHeuvelink 0:2a4ed6c6cdc7 139 {
SilHeuvelink 0:2a4ed6c6cdc7 140 wait(0.1);
SilHeuvelink 2:a8ee608177ae 141 pc.printf("angle = %f, length = %f \r\n", angle*180/pi, length);
SilHeuvelink 0:2a4ed6c6cdc7 142 pc.printf("x = %f, y = %f \r\n", p_current_x, p_current_y);
SilHeuvelink 0:2a4ed6c6cdc7 143 pc.printf("motor1_pwm = %f, motor2_pwm = %f \r\n", motor1_pwm, motor2_pwm);
SilHeuvelink 3:f70ec68723df 144 pc.printf("p_desired_x = %f, p_desired_y = %f \r\n", p_desired_x, p_desired_y);
SilHeuvelink 3:f70ec68723df 145
SilHeuvelink 0:2a4ed6c6cdc7 146 }
SilHeuvelink 0:2a4ed6c6cdc7 147 }