zhaw_st16b_pes2_10 / Mbed 2 deprecated PES_Official-US

a

Dependencies:   Servo ServoArm mbed

Fork of PES_Official-TestF by zhaw_st16b_pes2_10

Sources/Arm.cpp

Committer:
beacon
Date:
2017-05-02
Revision:
9:ac362674c480

File content as of revision 9:ac362674c480:

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