![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
lal
main.cpp
- Committer:
- aschut
- Date:
- 2018-10-31
- Revision:
- 0:e0567b87d8ab
File content as of revision 0:e0567b87d8ab:
// EMG naar HOEK //----------------~INITIATING------------------------- #include "mbed.h" // KINEMATICS -- DEPENDENCIES #include "stdio.h" #define _USE_MATH_DEFINES #include <math.h> #define M_PI 3.14159265358979323846 /* pi */ //----------------GLOBALS-------------------------- //CONSTANTS KINEMATICS // constants const float la = 0.256; // lengte actieve arm const float lp = 0.21; // lengte passieve arm const float rp = 0.052; // straal van midden end effector tot hoekpunt const float rm = 0.23; // straal van global midden tot motor const float a = 0.09; // zijde van de driehoek const float xas = 0.40; // afstand van motor 1 tot motor 3 const float yas = 0.346; // afstand van xas tot motor 2 const float thetap = 0; // rotatiehoek van de end effector // motor locatie const int a1x = 0; //x locatie motor 1 const int a1y = 0; //y locatie motor 1 const float a2x = (0.5)*xas; // x locatie motor 2 const float a2y = yas; // y locatie motor 2 const float a3x = xas; // x locatie motor 3 const int a3y = 0; // y locatie motor 3 // script voor het bepalen van de desired position aan de hand van emg (1/0) // EMG OUTPUT int EMGxplus; int EMGxmin ; int EMGyplus; int EMGymin ; // Dit moet experimenteel geperfectioneerd worden float tijdstap = 0.05; //nu wss heel langzaam, kan miss omhoog float v = 0.1; // snelheid kan wss ook hoger float px = 0.2; //starting x float py = 0.155; // starting y // limits (since no forward kinematics) float upperxlim = 0.36; //niet helemaal naar requierments ff kijken of ie groter kan float lowerxlim = 0.04; float upperylim = 0.30; float lowerylim = 0.04; //----------------FUNCTIONS-------------------------- // ~~~~~~~~~~~~~~ROBOT KINEMATICS ~~~~~~~~~~~~~~~~~~ // functie x positie float positionx(int EMGxplus,int EMGxmin) { float EMGx = EMGxplus - EMGxmin; float verplaatsingx = EMGx * tijdstap * v; float pxnieuw = px + verplaatsingx; // x limit if (pxnieuw <= upperxlim && pxnieuw >= lowerxlim) { px = pxnieuw; } else { if (pxnieuw >= lowerxlim) { px = upperxlim; } else { px = lowerxlim; } } //printf("X eindpunt (%f) en verplaatsing: (%f)\n\r",px,verplaatsingx); return px; } // functie y positie float positiony(int EMGyplus,int EMGymin) { float EMGy = EMGyplus - EMGymin; float verplaatsingy = EMGy * tijdstap * v; float pynieuw = py + verplaatsingy; // y limit if (pynieuw <= upperylim && pynieuw >= lowerylim) { py = pynieuw; } else { if (pynieuw >= lowerylim) { py = upperylim; } else { py = lowerylim; } } //printf("Y eindpunt (%f) en verplaatsing: (%f) \n\r",py,verplaatsingy); return (py); } //----------------------MAIN--------------------------------- int main() { // berekenen positie float px = positionx(1,0); // EMG: +x, -x float py = positiony(1,0); // EMG: +y, -y //printf("positie (%f,%f)\n\r",px,py); }