Arm.cpp

00001 #include "Robot.h"
00002 #include "Declarations.h"
00003 
00004 
00005 Arm::Arm(){
00006 }
00007 
00008 Arm::Arm(ServoArm* arm){
00009     init(arm);
00010 }
00011 
00012 void Arm::init(ServoArm* arm){
00013     this->arm = arm;
00014     
00015     this->arm->calibrate(0.0015f, 180.0f);
00016     this->arm->position(RELEASE_POS);
00017 }
00018 
00019 int Arm::collectToDown(){
00020     static float pos=COLLECT_POS;
00021     if(pos>TAKE_POS) {
00022         pos-=3;
00023         this->arm->position(pos);
00024         return 0;
00025     }
00026     else{
00027         pos = COLLECT_POS;
00028         return 1;
00029     }
00030 }
00031 
00032 int Arm::downToCollect(){
00033     static float pos=TAKE_POS;
00034     if(pos<COLLECT_POS) {
00035         pos+=3;
00036         this->arm->position(pos);
00037         return 0;
00038     }
00039     else{
00040         pos = TAKE_POS;
00041         return 1;
00042     }
00043 }
00044 
00045 int Arm::collectToBack(){
00046     static float pos=COLLECT_POS;
00047     if(pos<RELEASE_POS) {
00048         pos+=3;
00049         this->arm->position(pos);
00050         return 0;
00051         }
00052     else{
00053         pos = COLLECT_POS;
00054         return 1;
00055     }
00056 }
00057 
00058 int Arm::backToCollect(){
00059     static float pos=RELEASE_POS;
00060     if(pos>COLLECT_POS) {
00061         pos-=3;
00062         this->arm->position(pos);
00063         return 0;
00064         }
00065     else{
00066         pos = RELEASE_POS;
00067         return 1;
00068     }
00069 }