Diff: Sources/Arm.cpp

Revision:

9:ac362674c480

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Sources/Arm.cpp	Tue May 02 08:39:35 2017 +0000
@@ -0,0 +1,61 @@
+#include "Robot.h"
+#include "Declarations.h"
+
+
+Arm::Arm(){
+}
+
+Arm::Arm(Servo* servoArm){
+    init(servoArm);
+}
+
+void Arm::init(Servo* servoArm){
+    this->servoArm = servoArm;
+    
+    this->servoArm->calibrate(0.008f, 90.0f);
+    this->servoArm->position(COLLECT_POS);
+}
+
+void Arm::collecttodown(int* done){
+    static float pos=COLLECT_POS;
+    if(pos>TAKE_POS) {
+        pos-=1;
+        this->servoArm->position(pos);
+    }
+    else{
+        *done = 1;
+    }
+}
+
+void Arm::downtocollect(int* done){
+    static float pos=TAKE_POS;
+    if(pos<COLLECT_POS) {
+        pos+=1;
+        this->servoArm->position(pos);
+        }
+    else{
+        *done = 1;
+    }
+}
+
+void Arm::collecttoback(int* done){
+    static float pos=COLLECT_POS;
+    if(pos<RELEASE_POS) {
+        pos+=1;
+        this->servoArm->position(pos);
+        }
+    else{
+        *done = 1;
+    }
+}
+
+void Arm::backtocollect(int* done){
+    static float pos=RELEASE_POS;
+    if(pos>COLLECT_POS) {
+        pos-=1;
+        this->servoArm->position(pos);
+        }
+    else{
+        *done = 1;
+    }
+}
\ No newline at end of file