Do NOT modify!
Dependencies: mbed Servo ServoArm
Fork of PES_Yanick by
Diff: Sources/Arm.cpp
- Revision:
- 9:ac362674c480
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Sources/Arm.cpp Tue May 02 08:39:35 2017 +0000 @@ -0,0 +1,61 @@ +#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; + } +} \ No newline at end of file