File content as of revision 0:15a8480061e8:

#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;
    }
}