Diff: bot/tentacle_unit/tentacle_unit.cpp

Revision:

55:ccf2ac8f6f32

Parent:

54:857390145ac4

--- a/bot/tentacle_unit/tentacle_unit.cpp	Thu Nov 23 14:59:59 2017 +0900
+++ b/bot/tentacle_unit/tentacle_unit.cpp	Tue Nov 28 17:58:04 2017 +0900
@@ -2,11 +2,13 @@
 
 Tentacle::Tentacle(DigitalOut* RS485Controller, Serial* RS485) :
 	tentacleMotor({
-    {RS485Controller, 1, 3, SM, RS485},
-    {RS485Controller, 1, 1, SM, RS485},
+    {RS485Controller, 0, 3, SM, RS485},
+    {RS485Controller, 0, 0, SM, RS485},
   }),
-  right(LIMIT0, LIMIT1),left(LIMIT2,LIMIT3),
-	debugSerial(USBTX, USBRX, 115200)
+	// right(LIMIT1,LIMIT0),
+  // left(LIMIT3,LIMIT4)
+	front({{LIMIT1},{LIMIT3}}),
+	back({{LIMIT0},{LIMIT4}})
 {
 	tentacleMotor[0].braking = true;
   tentacleMotor[1].braking = true;
@@ -14,15 +16,19 @@
 
 void Tentacle::rightMove(float speed)
 {
-  /*if(speed*right.getPosition() < 0)    speed = 0;
-  if(speed*right.getPosition() > 0)    right.resetPosition();*/
+	if(speed*front[0]>0) speed=0;
+  if(speed*back[0]<0) speed=0;
+  // 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();*/
+	if(speed*front[1]>0) speed=0;
+  if(speed*back[1]<0) speed=0;
+  // if(speed*left.getPosition() < 0)    speed = 0;
+  // if(speed*left.getPosition() > 0)    left.resetPosition();
   tentacleMotor[1].setSpeed(speed);
 }
 
@@ -31,8 +37,3 @@
   tentacleMotor[0].setSpeed(0);
   tentacleMotor[1].setSpeed(0);
 }
-
-void Tentacle::debugPrint()
-{
-	debugSerial.printf("right:%d left:%d\r\n",right.getPosition(),left.getPosition());
-}