zhaw_st16b_pes2_10 / Mbed 2 deprecated PES_Official

Do NOT modify!

Dependencies:   mbed Servo ServoArm

Fork of PES_Yanick by zhaw_st16b_pes2_10

Sources/Arm.cpp

Committer:
EpicG10
Date:
2017-05-02
Revision:
7:6fed0dcae9c1
Parent:
6:ba26dd3251b3
Child:
10:f76476943a6c

File content as of revision 7:6fed0dcae9c1:

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