Simmerl Manuel
This program control a 5 axis arm robot from lynx motion
robot.cpp@0:b6608b36efd7, 2011-02-15 (annotated)
- Committer:
- msimmerl
- Date:
- Tue Feb 15 07:49:25 2011 +0000
- Revision:
- 0:b6608b36efd7
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| msimmerl | 0:b6608b36efd7 | 1 | #include "robot.h" |
| msimmerl | 0:b6608b36efd7 | 2 | |
| msimmerl | 0:b6608b36efd7 | 3 | //float speed = 0.02; |
| msimmerl | 0:b6608b36efd7 | 4 | |
| msimmerl | 0:b6608b36efd7 | 5 | /*void GoToPos1(float speed){ |
| msimmerl | 0:b6608b36efd7 | 6 | //1. Arm vorne heben |
| msimmerl | 0:b6608b36efd7 | 7 | for(float offset = 0.0; (offset + 0.0011)<0.0018; offset+=0.00002) { |
| msimmerl | 0:b6608b36efd7 | 8 | arm_vorne.pulsewidth(0.0011 + offset); // servo position determined by a pulsewidth between 1-2ms |
| msimmerl | 0:b6608b36efd7 | 9 | wait(0.02); |
| msimmerl | 0:b6608b36efd7 | 10 | } |
| msimmerl | 0:b6608b36efd7 | 11 | arm_vorne.pulsewidth(P1_ARM_VORNE); |
| msimmerl | 0:b6608b36efd7 | 12 | arm_hinten.pulsewidth(P1_ARM_HINTEN); |
| msimmerl | 0:b6608b36efd7 | 13 | klaue.pulsewidth(P1_KLAUE); |
| msimmerl | 0:b6608b36efd7 | 14 | heben1.pulsewidth(P1_HEBEN); |
| msimmerl | 0:b6608b36efd7 | 15 | heben2.pulsewidth(P1_HEBEN); |
| msimmerl | 0:b6608b36efd7 | 16 | }*/ |
| msimmerl | 0:b6608b36efd7 | 17 | |
| msimmerl | 0:b6608b36efd7 | 18 | float Drive(PwmOut servo, float posAkt, float posNeu, float speed) { |
| msimmerl | 0:b6608b36efd7 | 19 | if(posAkt < posNeu){ |
| msimmerl | 0:b6608b36efd7 | 20 | for(float offset=0.0; (offset + posAkt)<=posNeu; offset+=0.00001) { |
| msimmerl | 0:b6608b36efd7 | 21 | servo.pulsewidth(posAkt + offset); // servo position determined by a pulsewidth between 1-2ms |
| msimmerl | 0:b6608b36efd7 | 22 | wait(speed); |
| msimmerl | 0:b6608b36efd7 | 23 | } |
| msimmerl | 0:b6608b36efd7 | 24 | } else { |
| msimmerl | 0:b6608b36efd7 | 25 | for(float offset=0.0; (posAkt - offset)>=posNeu; offset+=0.00001) { |
| msimmerl | 0:b6608b36efd7 | 26 | servo.pulsewidth(posAkt - offset); // servo position determined by a pulsewidth between 1-2ms |
| msimmerl | 0:b6608b36efd7 | 27 | wait(speed); |
| msimmerl | 0:b6608b36efd7 | 28 | } |
| msimmerl | 0:b6608b36efd7 | 29 | } |
| msimmerl | 0:b6608b36efd7 | 30 | return posNeu; |
| msimmerl | 0:b6608b36efd7 | 31 | } |
| msimmerl | 0:b6608b36efd7 | 32 | float Drive(PwmOut servo1, PwmOut servo2, float posAkt, float posNeu, float speed) { |
| msimmerl | 0:b6608b36efd7 | 33 | if(posAkt < posNeu){ |
| msimmerl | 0:b6608b36efd7 | 34 | for(float offset=0.0; (offset + posAkt)<=posNeu; offset+=0.00001) { |
| msimmerl | 0:b6608b36efd7 | 35 | servo1.pulsewidth(posAkt + offset); // servo position determined by a pulsewidth between 1-2ms |
| msimmerl | 0:b6608b36efd7 | 36 | servo2.pulsewidth(posAkt + offset); // servo position determined by a pulsewidth between 1-2ms |
| msimmerl | 0:b6608b36efd7 | 37 | wait(speed); |
| msimmerl | 0:b6608b36efd7 | 38 | } |
| msimmerl | 0:b6608b36efd7 | 39 | } else { |
| msimmerl | 0:b6608b36efd7 | 40 | for(float offset=0.0; (posAkt - offset)>=posNeu; offset+=0.00001) { |
| msimmerl | 0:b6608b36efd7 | 41 | servo1.pulsewidth(posAkt - offset); // servo position determined by a pulsewidth between 1-2ms |
| msimmerl | 0:b6608b36efd7 | 42 | servo2.pulsewidth(posAkt - offset); // servo position determined by a pulsewidth between 1-2ms |
| msimmerl | 0:b6608b36efd7 | 43 | wait(speed); |
| msimmerl | 0:b6608b36efd7 | 44 | } |
| msimmerl | 0:b6608b36efd7 | 45 | } |
| msimmerl | 0:b6608b36efd7 | 46 | return posNeu; |
| msimmerl | 0:b6608b36efd7 | 47 | } |
| msimmerl | 0:b6608b36efd7 | 48 | |
| msimmerl | 0:b6608b36efd7 | 49 | /*void Welcome(void) { |
| msimmerl | 0:b6608b36efd7 | 50 | for(float offset=0.0; (offset + 0.0011)<0.0018; offset+=0.00002) { |
| msimmerl | 0:b6608b36efd7 | 51 | arm_vorne.pulsewidth(0.0011 + offset); // servo position determined by a pulsewidth between 1-2ms |
| msimmerl | 0:b6608b36efd7 | 52 | wait(0.02); |
| msimmerl | 0:b6608b36efd7 | 53 | } |
| msimmerl | 0:b6608b36efd7 | 54 | for(float offset=0.0; (0.0018 - offset)>0.0011; offset+=0.00002) { |
| msimmerl | 0:b6608b36efd7 | 55 | arm_vorne.pulsewidth(0.0018 - offset); // servo position determined by a pulsewidth between 1-2ms |
| msimmerl | 0:b6608b36efd7 | 56 | wait(0.02); |
| msimmerl | 0:b6608b36efd7 | 57 | } |
| msimmerl | 0:b6608b36efd7 | 58 | }*/ |
| msimmerl | 0:b6608b36efd7 | 59 |