Diff: bot/tentacle_unit/tentacle_unit.cpp

Revision:

23:37bb9afe9fdc

Child:

26:7258d5ad0bff

--- /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);
+}