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-04-26
Revision:
6:ba26dd3251b3
Parent:
4:67d7177c213f
Child:
7:6fed0dcae9c1

File content as of revision 6:ba26dd3251b3:

#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.0015f, 180.0f);
    this->servoArm->position(COLLECT_POS);
}

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::downtocollect(int* done){
    static float pos=TAKE_POS;
    if(pos<COLLECT_POS) {
        pos+=0.5f;
        this->servoArm->position(pos);
        }
    else{
        *done = 1;
    }
}

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::backtocollect(int* done){
    static float pos=RELEASE_POS;
    if(pos>COLLECT_POS) {
        pos-=0.5f;
        this->servoArm->position(pos);
        }
    else{
        *done = 1;
    }
}