Diff: Sources/Arm.cpp

Revision:

0:15a8480061e8

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Sources/Arm.cpp	Mon May 22 11:24:46 2017 +0000
@@ -0,0 +1,95 @@
+#include "Robot.h"
+#include "Declarations.h"
+
+
+Arm::Arm(){
+}
+
+Arm::Arm(ServoArm* arm){
+    init(arm);
+}
+
+void Arm::init(ServoArm* arm){
+    this->arm = arm;
+    
+    this->arm->calibrate(0.0015f, 180.0f);
+    this->arm->position(RELEASE_POS);
+}
+
+int Arm::collectToDown(){
+    static float pos=COLLECT_POS;
+    if(pos>TAKE_POS) {
+        pos-=3;
+        this->arm->position(pos);
+        return 0;
+    }
+    else{
+        pos = COLLECT_POS;
+        return 1;
+    }
+}
+
+int Arm::downToCollect(){
+    static float pos=TAKE_POS;
+    if(pos<COLLECT_POS) {
+        pos+=3;
+        this->arm->position(pos);
+        return 0;
+    }
+    else{
+        pos = TAKE_POS;
+        return 1;
+    }
+}
+
+int Arm::collectToBack(){
+    static float pos=COLLECT_POS;
+    if(pos<RELEASE_POS) {
+        pos+=3;
+        this->arm->position(pos);
+        return 0;
+        }
+    else{
+        pos = COLLECT_POS;
+        return 1;
+    }
+}
+
+int Arm::backToCollect(){
+    static float pos=RELEASE_POS;
+    if(pos>COLLECT_POS) {
+        pos-=3;
+        this->arm->position(pos);
+        return 0;
+        }
+    else{
+        pos = RELEASE_POS;
+        return 1;
+    }
+}
+
+int Arm::backToDown(){
+    static float pos = RELEASE_POS;
+    if( pos > TAKE_POS ){
+        pos -= 0.0015f;
+        this->arm->position(pos);
+        return 0;
+    }
+    else{
+        pos = RELEASE_POS;
+        return 1;
+    }
+}
+
+int Arm::downToBack(){
+    static float pos = TAKE_POS;
+    if( pos < RELEASE_POS ){
+        pos += 0.0015f;
+        this->arm->position(pos);
+        return 0;
+    }
+    else{
+        pos = TAKE_POS;
+        return 1;
+    }
+}
\ No newline at end of file