c
Dependencies: Servo ServoArm mbed
Fork of PES_PIXY_Official by
Diff: Sources/Arm.cpp
- Revision:
- 0:15a8480061e8
- Child:
- 1:fd3cef0f116d
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Sources/Arm.cpp Mon May 22 11:24:46 2017 +0000 @@ -0,0 +1,95 @@ +#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; + } +} \ No newline at end of file