NHK2017 octopus robot
Dependencies: 2017NHKpin_config mbed FEP ikarashiMDC PID jy901 omni HMC6352 omni_wheel
Fork of KANI2017v2 by
Diff: bot/tentacle_unit/tentacle_unit.cpp
- Revision:
- 23:37bb9afe9fdc
- Child:
- 26:7258d5ad0bff
diff -r 682cc376da6f -r 37bb9afe9fdc bot/tentacle_unit/tentacle_unit.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/bot/tentacle_unit/tentacle_unit.cpp Wed Sep 13 14:26:47 2017 +0900 @@ -0,0 +1,32 @@ +#include "tentacle_unit.h" + +Tentacle::Tentacle(DigitalOut* RS485Controller, Serial* RS485) : + tentacleMotor({ + {RS485Controller, 1, 0, SM, RS485}, + {RS485Controller, 1, 1, SM, RS485}, + }), + right(Sensor3pin3a, Sensor3pin3b), left(Sensor4pin2a, Sensor4pin2b) +{ + tentacleMotor[0].braking = true; + tentacleMotor[1].braking = true; +} + +void Tentacle::rightMove(float speed) +{ + if(speed*right.getPosition() < 0) speed = 0; + if(speed*right.getPosition() > 0) right.resetPosition(); + tentacleMotor[0].setSpeed(speed); +} + +void Tentacle::leftMove(float speed) +{ + if(speed*left.getPosition() < 0) speed = 0; + if(speed*left.getPosition() > 0) left.resetPosition(); + tentacleMotor[1].setSpeed(speed); +} + +void Tentacle::stop() +{ + tentacleMotor[0].setSpeed(0); + tentacleMotor[1].setSpeed(0); +}