Diff: Sources/Arm.cpp

Revision:

6:ba26dd3251b3

Parent:

4:67d7177c213f

Child:

7:6fed0dcae9c1

--- a/Sources/Arm.cpp	Wed Apr 26 08:05:25 2017 +0000
+++ b/Sources/Arm.cpp	Wed Apr 26 14:09:08 2017 +0000
@@ -1,30 +1,61 @@
-/*#include "Robot.h"
+#include "Robot.h"
+#include "Declarations.h"
+
 
-Arm::Arm(Servo joint){
-    init(joint);
+Arm::Arm(){
+}
+
+Arm::Arm(Servo* servoArm){
+    init(servoArm);
 }
 
-Arm::Arm(){
+void Arm::init(Servo* servoArm){
+    this->servoArm = servoArm;
     
+    this->servoArm->calibrate(0.0015f, 180.0f);
+    this->servoArm->position(COLLECT_POS);
 }
 
-void Arm::init(Servo joint){
-    this.joint = joint;
+void Arm::collecttodown(int* done){
+    static float pos=COLLECT_POS;
+    if(pos>TAKE_POS) {
+        pos-=0.5f;
+        this->servoArm->position(pos);
+    }
+    else{
+        *done = 1;
+    }
 }
 
-void Arm::down(){
-    //do stuff
-}
-
-void Arm::neutral(){
-    //do stuff
+void Arm::downtocollect(int* done){
+    static float pos=TAKE_POS;
+    if(pos<COLLECT_POS) {
+        pos+=0.5f;
+        this->servoArm->position(pos);
+        }
+    else{
+        *done = 1;
+    }
 }
 
-void Arm::back(){
-    //do stuff
+void Arm::collecttoback(int* done){
+    static float pos=COLLECT_POS;
+    if(pos<RELEASE_POS) {
+        pos+=0.5f;
+        this->servoArm->position(pos);
+        }
+    else{
+        *done = 1;
+    }
 }
 
-void Arm::setAngle(float angle){
-    //do stuff
-}
-*/
\ No newline at end of file
+void Arm::backtocollect(int* done){
+    static float pos=RELEASE_POS;
+    if(pos>COLLECT_POS) {
+        pos-=0.5f;
+        this->servoArm->position(pos);
+        }
+    else{
+        *done = 1;
+    }
+}
\ No newline at end of file