Astrid Schut / Mbed 2 deprecated EMG10

lal

Dependencies:   mbed

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

Committer:
aschut
Date:
Wed Oct 31 16:12:43 2018 +0000
Revision:
0:e0567b87d8ab
voor flix;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
aschut 0:e0567b87d8ab 1 // EMG naar HOEK
aschut 0:e0567b87d8ab 2
aschut 0:e0567b87d8ab 3 //----------------~INITIATING-------------------------
aschut 0:e0567b87d8ab 4 #include "mbed.h"
aschut 0:e0567b87d8ab 5
aschut 0:e0567b87d8ab 6 // KINEMATICS -- DEPENDENCIES
aschut 0:e0567b87d8ab 7 #include "stdio.h"
aschut 0:e0567b87d8ab 8 #define _USE_MATH_DEFINES
aschut 0:e0567b87d8ab 9 #include <math.h>
aschut 0:e0567b87d8ab 10 #define M_PI 3.14159265358979323846 /* pi */
aschut 0:e0567b87d8ab 11
aschut 0:e0567b87d8ab 12 //----------------GLOBALS--------------------------
aschut 0:e0567b87d8ab 13
aschut 0:e0567b87d8ab 14 //CONSTANTS KINEMATICS
aschut 0:e0567b87d8ab 15 // constants
aschut 0:e0567b87d8ab 16 const float la = 0.256; // lengte actieve arm
aschut 0:e0567b87d8ab 17 const float lp = 0.21; // lengte passieve arm
aschut 0:e0567b87d8ab 18 const float rp = 0.052; // straal van midden end effector tot hoekpunt
aschut 0:e0567b87d8ab 19 const float rm = 0.23; // straal van global midden tot motor
aschut 0:e0567b87d8ab 20 const float a = 0.09; // zijde van de driehoek
aschut 0:e0567b87d8ab 21 const float xas = 0.40; // afstand van motor 1 tot motor 3
aschut 0:e0567b87d8ab 22 const float yas = 0.346; // afstand van xas tot motor 2
aschut 0:e0567b87d8ab 23 const float thetap = 0; // rotatiehoek van de end effector
aschut 0:e0567b87d8ab 24
aschut 0:e0567b87d8ab 25 // motor locatie
aschut 0:e0567b87d8ab 26 const int a1x = 0; //x locatie motor 1
aschut 0:e0567b87d8ab 27 const int a1y = 0; //y locatie motor 1
aschut 0:e0567b87d8ab 28 const float a2x = (0.5)*xas; // x locatie motor 2
aschut 0:e0567b87d8ab 29 const float a2y = yas; // y locatie motor 2
aschut 0:e0567b87d8ab 30 const float a3x = xas; // x locatie motor 3
aschut 0:e0567b87d8ab 31 const int a3y = 0; // y locatie motor 3
aschut 0:e0567b87d8ab 32
aschut 0:e0567b87d8ab 33 // script voor het bepalen van de desired position aan de hand van emg (1/0)
aschut 0:e0567b87d8ab 34
aschut 0:e0567b87d8ab 35 // EMG OUTPUT
aschut 0:e0567b87d8ab 36 int EMGxplus;
aschut 0:e0567b87d8ab 37 int EMGxmin ;
aschut 0:e0567b87d8ab 38 int EMGyplus;
aschut 0:e0567b87d8ab 39 int EMGymin ;
aschut 0:e0567b87d8ab 40
aschut 0:e0567b87d8ab 41 // Dit moet experimenteel geperfectioneerd worden
aschut 0:e0567b87d8ab 42 float tijdstap = 0.05; //nu wss heel langzaam, kan miss omhoog
aschut 0:e0567b87d8ab 43 float v = 0.1; // snelheid kan wss ook hoger
aschut 0:e0567b87d8ab 44
aschut 0:e0567b87d8ab 45 float px = 0.2; //starting x
aschut 0:e0567b87d8ab 46 float py = 0.155; // starting y
aschut 0:e0567b87d8ab 47
aschut 0:e0567b87d8ab 48 // limits (since no forward kinematics)
aschut 0:e0567b87d8ab 49 float upperxlim = 0.36; //niet helemaal naar requierments ff kijken of ie groter kan
aschut 0:e0567b87d8ab 50 float lowerxlim = 0.04;
aschut 0:e0567b87d8ab 51 float upperylim = 0.30;
aschut 0:e0567b87d8ab 52 float lowerylim = 0.04;
aschut 0:e0567b87d8ab 53
aschut 0:e0567b87d8ab 54
aschut 0:e0567b87d8ab 55 //----------------FUNCTIONS--------------------------
aschut 0:e0567b87d8ab 56
aschut 0:e0567b87d8ab 57 // ~~~~~~~~~~~~~~ROBOT KINEMATICS ~~~~~~~~~~~~~~~~~~
aschut 0:e0567b87d8ab 58
aschut 0:e0567b87d8ab 59 // functie x positie
aschut 0:e0567b87d8ab 60 float positionx(int EMGxplus,int EMGxmin)
aschut 0:e0567b87d8ab 61 {
aschut 0:e0567b87d8ab 62 float EMGx = EMGxplus - EMGxmin;
aschut 0:e0567b87d8ab 63
aschut 0:e0567b87d8ab 64 float verplaatsingx = EMGx * tijdstap * v;
aschut 0:e0567b87d8ab 65 float pxnieuw = px + verplaatsingx;
aschut 0:e0567b87d8ab 66 // x limit
aschut 0:e0567b87d8ab 67 if (pxnieuw <= upperxlim && pxnieuw >= lowerxlim)
aschut 0:e0567b87d8ab 68 {
aschut 0:e0567b87d8ab 69 px = pxnieuw;
aschut 0:e0567b87d8ab 70 }
aschut 0:e0567b87d8ab 71 else
aschut 0:e0567b87d8ab 72 {
aschut 0:e0567b87d8ab 73 if (pxnieuw >= lowerxlim)
aschut 0:e0567b87d8ab 74 {
aschut 0:e0567b87d8ab 75 px = upperxlim;
aschut 0:e0567b87d8ab 76 }
aschut 0:e0567b87d8ab 77 else
aschut 0:e0567b87d8ab 78 {
aschut 0:e0567b87d8ab 79 px = lowerxlim;
aschut 0:e0567b87d8ab 80 }
aschut 0:e0567b87d8ab 81 }
aschut 0:e0567b87d8ab 82 //printf("X eindpunt (%f) en verplaatsing: (%f)\n\r",px,verplaatsingx);
aschut 0:e0567b87d8ab 83 return px;
aschut 0:e0567b87d8ab 84 }
aschut 0:e0567b87d8ab 85
aschut 0:e0567b87d8ab 86
aschut 0:e0567b87d8ab 87 // functie y positie
aschut 0:e0567b87d8ab 88 float positiony(int EMGyplus,int EMGymin)
aschut 0:e0567b87d8ab 89 {
aschut 0:e0567b87d8ab 90 float EMGy = EMGyplus - EMGymin;
aschut 0:e0567b87d8ab 91
aschut 0:e0567b87d8ab 92 float verplaatsingy = EMGy * tijdstap * v;
aschut 0:e0567b87d8ab 93 float pynieuw = py + verplaatsingy;
aschut 0:e0567b87d8ab 94
aschut 0:e0567b87d8ab 95 // y limit
aschut 0:e0567b87d8ab 96 if (pynieuw <= upperylim && pynieuw >= lowerylim)
aschut 0:e0567b87d8ab 97 {
aschut 0:e0567b87d8ab 98 py = pynieuw;
aschut 0:e0567b87d8ab 99 }
aschut 0:e0567b87d8ab 100 else
aschut 0:e0567b87d8ab 101 {
aschut 0:e0567b87d8ab 102 if (pynieuw >= lowerylim)
aschut 0:e0567b87d8ab 103 {
aschut 0:e0567b87d8ab 104 py = upperylim;
aschut 0:e0567b87d8ab 105 }
aschut 0:e0567b87d8ab 106 else
aschut 0:e0567b87d8ab 107 {
aschut 0:e0567b87d8ab 108 py = lowerylim;
aschut 0:e0567b87d8ab 109 }
aschut 0:e0567b87d8ab 110 }
aschut 0:e0567b87d8ab 111 //printf("Y eindpunt (%f) en verplaatsing: (%f) \n\r",py,verplaatsingy);
aschut 0:e0567b87d8ab 112 return (py);
aschut 0:e0567b87d8ab 113 }
aschut 0:e0567b87d8ab 114
aschut 0:e0567b87d8ab 115
aschut 0:e0567b87d8ab 116
aschut 0:e0567b87d8ab 117 //----------------------MAIN---------------------------------
aschut 0:e0567b87d8ab 118 int main()
aschut 0:e0567b87d8ab 119 {
aschut 0:e0567b87d8ab 120
aschut 0:e0567b87d8ab 121
aschut 0:e0567b87d8ab 122 // berekenen positie
aschut 0:e0567b87d8ab 123 float px = positionx(1,0); // EMG: +x, -x
aschut 0:e0567b87d8ab 124 float py = positiony(1,0); // EMG: +y, -y
aschut 0:e0567b87d8ab 125 //printf("positie (%f,%f)\n\r",px,py);
aschut 0:e0567b87d8ab 126
aschut 0:e0567b87d8ab 127
aschut 0:e0567b87d8ab 128 }