Diff: robot.cpp

Revision:

0:b6608b36efd7

--- /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);
+    }
+}*/
+