Simmerl Manuel
/
MRT_Studienarbeit
This program control a 5 axis arm robot from lynx motion
Diff: robot.cpp
- Revision:
- 0:b6608b36efd7
diff -r 000000000000 -r b6608b36efd7 robot.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/robot.cpp Tue Feb 15 07:49:25 2011 +0000 @@ -0,0 +1,59 @@ +#include "robot.h" + +//float speed = 0.02; + +/*void GoToPos1(float speed){ + //1. Arm vorne heben + for(float offset = 0.0; (offset + 0.0011)<0.0018; offset+=0.00002) { + arm_vorne.pulsewidth(0.0011 + offset); // servo position determined by a pulsewidth between 1-2ms + wait(0.02); + } + arm_vorne.pulsewidth(P1_ARM_VORNE); + arm_hinten.pulsewidth(P1_ARM_HINTEN); + klaue.pulsewidth(P1_KLAUE); + heben1.pulsewidth(P1_HEBEN); + heben2.pulsewidth(P1_HEBEN); +}*/ + +float Drive(PwmOut servo, float posAkt, float posNeu, float speed) { + if(posAkt < posNeu){ + for(float offset=0.0; (offset + posAkt)<=posNeu; offset+=0.00001) { + servo.pulsewidth(posAkt + offset); // servo position determined by a pulsewidth between 1-2ms + wait(speed); + } + } else { + for(float offset=0.0; (posAkt - offset)>=posNeu; offset+=0.00001) { + servo.pulsewidth(posAkt - offset); // servo position determined by a pulsewidth between 1-2ms + wait(speed); + } + } + return posNeu; +} +float Drive(PwmOut servo1, PwmOut servo2, float posAkt, float posNeu, float speed) { + if(posAkt < posNeu){ + for(float offset=0.0; (offset + posAkt)<=posNeu; offset+=0.00001) { + servo1.pulsewidth(posAkt + offset); // servo position determined by a pulsewidth between 1-2ms + servo2.pulsewidth(posAkt + offset); // servo position determined by a pulsewidth between 1-2ms + wait(speed); + } + } else { + for(float offset=0.0; (posAkt - offset)>=posNeu; offset+=0.00001) { + servo1.pulsewidth(posAkt - offset); // servo position determined by a pulsewidth between 1-2ms + servo2.pulsewidth(posAkt - offset); // servo position determined by a pulsewidth between 1-2ms + wait(speed); + } + } + return posNeu; +} + +/*void Welcome(void) { + for(float offset=0.0; (offset + 0.0011)<0.0018; offset+=0.00002) { + arm_vorne.pulsewidth(0.0011 + offset); // servo position determined by a pulsewidth between 1-2ms + wait(0.02); + } + for(float offset=0.0; (0.0018 - offset)>0.0011; offset+=0.00002) { + arm_vorne.pulsewidth(0.0018 - offset); // servo position determined by a pulsewidth between 1-2ms + wait(0.02); + } +}*/ +