First commit of motor_move() API

Author: JeongJun-Lee

neobot/neosoco.py

@@ -16,6 +16,9 @@
 # Free Software Foundation, Inc., 59 Temple Place, Suite 330,
 # Boston, MA  02111-1307  USA
 
+
+import time
+
 from neobot.runner import Runner
 from neobot.util import Util
 from neobot.model import Robot
@@ -25,14 +28,16 @@
 class Neosoco(Robot):
     ID = "kr.neobot.physical.neosoco"
 
-    OUTPUT_1 = 0x00400000
-    OUTPUT_2 = 0x00400001
-    OUTPUT_3 = 0x00400002
-    INPUT_1  = 0x00400003
-    INPUT_2  = 0x00400004
-    INPUT_3  = 0x00400005
-    REMOCTL  = 0x00400006
-    BATTERY  = 0x00400007
+    OUTPUT_1    = 0x00400000
+    OUTPUT_2    = 0x00400001
+    OUTPUT_3    = 0x00400002
+    INPUT_1     = 0x00400003
+    INPUT_2     = 0x00400004
+    INPUT_3     = 0x00400005
+    REMOCTL     = 0x00400006
+    BATTERY     = 0x00400007
+    LEFT_MOTOR  = 0x00400008
+    RIGHT_MOTOR = 0x00400009
 
     LED_OFF = 0
     LED_BLUE = 1
@@ -280,6 +285,23 @@ class Neosoco(Robot):
         "digital_output": IO_MODE_DIGITAL_OUTPUT,
         "digital output": IO_MODE_DIGITAL_OUTPUT
     }
+    _MOTOR_PERCENT_CVT = { 
+        '100': 15,
+        '90': 14,
+        '80': 13,
+        '70': 12,
+        '60': 10,
+        '50': 9,
+        '40': 7,
+        '30': 5,
+        '20': 3,
+        '10': 2,
+        '0': 0
+    }
+    _MOTOR_DIR = { 
+        'forward': 16,
+        'backward': 32
+    }
     _robots = {}
 
     def __init__(self, index=0, port_name=None):
@@ -404,6 +426,30 @@ def led_on(self, port: str, brightness: str):
         else:
             raise TypeError
 
+    def motor_move(self, direction: str):
+        if isinstance(direction, str):
+            if direction.lower() =='forward':
+                self.write(Neosoco.LEFT_MOTOR, self._MOTOR_DIR['forward']+self._MOTOR_PERCENT_CVT['60'])
+                self.write(Neosoco.RIGHT_MOTOR, self._MOTOR_DIR['forward']+self._MOTOR_PERCENT_CVT['60'])
+            elif direction.lower() =='backward':
+                self.write(Neosoco.LEFT_MOTOR, self._MOTOR_DIR['backward']+self._MOTOR_PERCENT_CVT['60'])
+                self.write(Neosoco.RIGHT_MOTOR, self._MOTOR_DIR['backward']+self._MOTOR_PERCENT_CVT['60'])
+            elif direction.lower() =='left':
+                self.write(Neosoco.LEFT_MOTOR, 0)
+                self.write(Neosoco.RIGHT_MOTOR, self._MOTOR_DIR['forward']+self._MOTOR_PERCENT_CVT['60'])
+            elif direction.lower() =='right':
+                self.write(Neosoco.LEFT_MOTOR, self._MOTOR_DIR['forward']+self._MOTOR_PERCENT_CVT['60'])
+                self.write(Neosoco.RIGHT_MOTOR, 0)
+            elif direction.lower() =='stop':
+                self.write(Neosoco.LEFT_MOTOR, 0)
+                self.write(Neosoco.RIGHT_MOTOR, 0)
+            else:
+                Util.print_error('Wrong value of direction')
+                raise ValueError
+        else:
+            raise TypeError
+        time.sleep(0.1) # Since brocast from cotroller is per 100ms
+
     def wheels(self, left_velocity, right_velocity=None):
         self.write(Neosoco.LINE_TRACER_MODE, Neosoco.LINE_TRACER_MODE_OFF)
         if isinstance(left_velocity, (int, float)):

neobot/neosoco_neobot.py

@@ -125,7 +125,9 @@ def _create_model(self):
         dict[Neosoco.INPUT_3] = self._input_3_device = self._add_device(Neosoco.INPUT_3, "Input3", DeviceType.SENSOR, DataType.INTEGER, 1, 0, 255, 0)
         dict[Neosoco.REMOCTL] = self._remoctl_device = self._add_device(Neosoco.REMOCTL, "RemoteCtl", DeviceType.SENSOR, DataType.INTEGER, 1, 0, 255, 0)
         dict[Neosoco.BATTERY] = self._battery_device = self._add_device(Neosoco.BATTERY, "Battery", DeviceType.SENSOR, DataType.INTEGER, 1, 0, 255, 0)
-       
+        dict[Neosoco.LEFT_MOTOR] = self._left_motor_device = self._add_device(Neosoco.LEFT_MOTOR, "LeftMotor", DeviceType.EFFECTOR, DataType.INTEGER, 1, 0, 47, 0)
+        dict[Neosoco.RIGHT_MOTOR] = self._right_motor_device = self._add_device(Neosoco.RIGHT_MOTOR, "RightMotor", DeviceType.EFFECTOR, DataType.INTEGER, 1, 0, 47, 0)
+
     def find_device_by_id(self, device_id):
         return self._device_dict.get(device_id)
 
@@ -135,7 +137,7 @@ def _run(self):
                 if self._receive(self._connector):
                     self._send(self._connector)
                     self._releasing = False
-                time.sleep(0.01)
+                time.sleep(0.005)
         except:
             pass
 
@@ -177,6 +179,12 @@ def _dispose(self):
     def _reset(self):
         super(NeosocoNeobot, self)._reset()
 
+        self._output_1 = 0
+        self._output_2 = 0
+        self._output_3 = 0
+        self._left_motor = 0
+        self._right_motor = 0
+
         self._left_wheel = 0
         self._right_wheel = 0
         self._buzzer = 0
@@ -221,6 +229,8 @@ def _request_motoring_data(self):
             self._output_1 = self._output_1_device.read()
             self._output_2 = self._output_2_device.read()
             self._output_3 = self._output_3_device.read()
+            self._left_motor = self._left_motor_device.read()
+            self._right_motor = self._right_motor_device.read()
         self._clear_written()
 
     def _color_to_rgb(self, color):
@@ -246,8 +256,8 @@ def _encode_motoring_packet(self, address):
             result += self._to_hex(self._output_1) # OUT1
             result += self._to_hex(self._output_2) # OUT2
             result += self._to_hex(self._output_3) # OUT3
-            result += self._to_hex(0) # MLA
-            result += self._to_hex(0) # MRA
+            result += self._to_hex(self._left_motor)  # MLA
+            result += self._to_hex(self._right_motor) # MRA
             result += self._to_hex(0) # BUZZER
             result += self._to_hex(0) # FND
             result += self._to_hex(0) # Not Used

test.py

@@ -3,15 +3,23 @@
 n =  Neosoco()
 
 # case1) Turn on LED with 100% brightness during 1s
-# n.led_on("out1", '100')
+# n.led_on('out1', '100')
 # wait(1000)
 
-n.set_value("out1", 255)
-wait(1000)
+# case2) Turn on LED with 100% brightness during 1s
+# n.set_value('out1', 255)
+# wait(1000)
 
-# case2) Turn on LED when the distance is under 10cm
+# case3) Turn on LED when the distance is under 10cm
 # while True:
 #   if n.get_value('in1') < 10:
-#     n.led_on("out1", 255)
+#     n.led_on('out1', '100')
 #   else:
-#     n.led_on("out1", 0)
+#     n.set_value('out1', 0)
+
+# case4) Move forth and back during 1s and stop
+n.motor_move('forward')
+wait(500)
+n.motor_move('backward')
+wait(500)
+n.motor_move('stop')