Add velocity sample

SampleProgram/README.md

@@ -15,6 +15,10 @@ This directory is licensed under the Apache 2.0 License, see [SampleProgram / LI
 
 Each peripheral example has sample code files written Shell Script, C and Python.
 
+RT Software Tutorialsでは動画つきで説明しています。
+
+https://rt-net.github.io/tutorials/raspimouse/driver/samples.html
+
 ## Shell Script
 
 ```sh
@@ -96,3 +100,35 @@ Read the lightsensors values.
 モータを回して、パルスカウンタの値を読み込みます。
 
 Drive the motors and read the pulse counters values.
+
+# Step7
+
+車体速度 $v_{fw}$ 、
+車体角速度 $v_{rot}$ で指令するサンプルプログラムです。
+
+サンプルでは車体速度と車体角速度を指定しての移動が実装されています。
+
+## 解説
+
+ホイールの直径を $\phi$ [m]、車体のトレッドを $t$ [m]、モータ1回転のための制御信号を $p$ [Hz]とします。
+
+左右のモータへの制御信号を $\omega_{fw}$ [Hz]で入力したときの、
+車体の並進方向の速度を $v_{fw}$ [m/s]とします。  
+このときのそれぞれの関係は以下のように表現できます。
+
+$$v_{fw} : \omega_{fw} = \pi \phi  : p$$
+
+旋回方向についても同様に考えます。  
+車体が1回転するときのホイールが円弧を描くように移動する距離は $\pi t$ [m]で計算できます。
+モータ1回転でホイールの表面が移動する距離は $\pi \phi$ [m]で計算できます。  
+左右のモータへの制御信号をそれぞれ $-\omega_{rot}$ [Hz]と $\omega_{rot}$ [Hz]としたとき、
+ロボットが旋回するときの角速度を $v_{rot}$ [rad/s]とします。  
+このときのそれぞれの関係は以下のように表現できます。
+
+$$v_{rot} : \omega_{rot} = \frac{2\pi} {\pi t / \pi \phi}  : p$$
+
+これらを整理するとモータ制御信号は以下の2つの式にまとめられます。
+
+$$\omega_{fw} = \frac{p} {\pi \phi} v_{fw}$$
+
+$$\omega_{rot} = \frac {tp}{2\pi \phi} v_{rot}$$

SampleProgram/step7.py

@@ -0,0 +1,109 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+# Copyright 2022 RT Corporation
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import math
+import time
+
+class Motor:
+    def __init__(self, wheel_diamiter=0.048, wheel_tread=0.0925, pulse_per_rotate=400):
+        # ハードウェアパラメータ
+        self.wheel_diamiter = wheel_diamiter
+        self.wheel_tread = wheel_tread
+        self.pulse_per_rotate = pulse_per_rotate
+
+    # デバイスファイルを通してモータの回転角度を指定
+    def _set_motor_speed(self, left, right):
+        try:
+            with open('/dev/rtmotor_raw_l0', 'w') as lf, \
+                    open('/dev/rtmotor_raw_r0', 'w') as rf:
+                lf.write(str(int(left)))
+                rf.write(str(int(right)))
+        except Exception as e:
+            print(e)
+
+    # デバイスファイルからモータの電源をON/OFF
+    def _set_motor_power(self, mode):
+        try:
+            with open('/dev/rtmotoren0', 'w') as f:
+                f.write('1' if mode else '0')
+        except Exception as e:
+            print(e)
+
+    # 車体速度（並進方向の移動速度および旋回の角速度）からモータ指令値を計算
+    def _calc_speed(self, linear, angular):
+        """
+        linear: robot linear speed (m/s)
+        angular: robot angular speed (rad/s)
+        """
+        left_speed = self.meter_to_pulse(linear) - self.angle_to_pulse(angular)
+        right_speed = self.meter_to_pulse(linear) + self.angle_to_pulse(angular)
+        print("target speed (left: {}, right: {})".format(left_speed, right_speed))
+        return left_speed, right_speed
+
+    # ラズパイマウスのモータパルスでの距離をメートル系に変換
+    def pulse_to_meter(self, pulse):
+        return math.pi * self.wheel_diamiter * pulse / self.pulse_per_rotate
+
+    # メートル系での距離をラズパイマウスのモータパルスに変換
+    def meter_to_pulse(self, meter):
+        return int(self.pulse_per_rotate * meter / (math.pi * self.wheel_diamiter))
+
+    # ラズパイマウスの車体の旋回角度をラズパイマウスのモータパルスに変換
+    def angle_to_pulse(self, angle):
+        return int(angle * self.pulse_per_rotate * self.wheel_tread / self.wheel_diamiter / (2.0 * math.pi))
+
+    # 走行させるための関数
+    def run(self, linear_speed, angular_speed):
+        """
+        linear_speed: m/s
+        angular_speed: rad/s
+        """
+        (speed_l, speed_r) = self._calc_speed(linear_speed, angular_speed)
+
+        self._set_motor_speed(speed_l, speed_r)
+
+
+if __name__ == '__main__':
+    motor = Motor()
+    print("Motor On")
+    motor._set_motor_power(1)
+
+    print("Turn 3.14[rad/s]")
+    motor.run(linear_speed=0, angular_speed=3.14)
+    time.sleep(1)
+    print("Turn -3.14[rad/s]")
+    motor.run(linear_speed=0, angular_speed=-3.14)
+    time.sleep(1)
+    print("Move forward at 0.2[m/s]")
+    motor.run(linear_speed=0.2, angular_speed=0)
+    time.sleep(1)
+    print("Move backward 0.2[m/s]")
+    motor.run(linear_speed=-0.2, angular_speed=0)
+    time.sleep(1)
+    print("Turn 1.57[rad/s] + Move forward 0.3[m/s]")
+    motor.run(linear_speed=0.3, angular_speed=1.57)
+    time.sleep(1)
+    print("Turn -1.57[rad/s] + Move backward 0.3[m/s]")
+    motor.run(linear_speed=-0.3, angular_speed=-1.57)
+    time.sleep(1)
+
+    print("Stop")
+    motor.run(linear_speed=0, angular_speed=0)
+    time.sleep(0.1)
+
+    print("Motor Off")
+    motor._set_motor_power(0)