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-03
Revision:
10:f76476943a6c
Parent:
7:6fed0dcae9c1
Child:
11:292bdbd85a9c

File content as of revision 10:f76476943a6c:

#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(COLLECT_POS);
}

void Arm::collecttodown(int* done){
    static float pos=COLLECT_POS;
    if(pos>TAKE_POS) {
        pos-=1;
        this->arm->position(pos);
    }
    else{
        *done = 1;
    }
}

void Arm::downtocollect(int* done){
    static float pos=TAKE_POS;
    if(pos<COLLECT_POS) {
        pos+=1;
        this->arm->position(pos);
        }
    else{
        *done = 1;
    }
}

void Arm::collecttoback(int* done){
    static float pos=COLLECT_POS;
    if(pos<RELEASE_POS) {
        pos+=1;
        this->arm->position(pos);
        }
    else{
        *done = 1;
    }
}

void Arm::backtocollect(int* done){
    static float pos=RELEASE_POS;
    if(pos>COLLECT_POS) {
        pos-=1;
        this->arm->position(pos);
        }
    else{
        *done = 1;
    }
}