‘‘‘
機械臂指令管理
‘‘‘
#?import?ucollections
import?uheapq
from?configs?import?config

class?Command:
????‘‘‘指令基類‘‘‘
????is_active?=?False?#?指令是否有效
????CMD_TYPE?=?‘CMD_TYPE‘?#?指令的類型
????TIMER_PERIOD?=?config[‘TIMER_PERIOD‘]
????def?active_cmd（self）:
????????self.is_active?=?True

class?DelayCommand（Command）:
????‘‘‘延時類指令‘‘‘
????CMD_TYPE?=?‘CMD_DELAY‘
????def?__init__（self?delay_ms）:
????????self.delay_ms?=?delay_ms?#?延時的時間?ms
????????self.time_cnt?=?0?#?已經經過的時間

????def?active_cmd（self）:
????????‘‘‘激活當前指令‘‘‘
????????self.is_active?=?True?#?激活該指令
????????self.time_cnt?=?0?#?時間計數清零

????def?update（self）:
????????‘‘‘定時器回調函數?period是Timer的周期（常數）‘‘‘
????????self.time_cnt?+=?self.TIMER_PERIOD
????????if?self.time_cnt?>?self.delay_ms:
????????????self.is_active?=?False?#?指令執行結束

class?JointMoveCommand（Command）:
????‘‘‘單個關節角度的控制信號‘‘‘
????CMD_TYPE?=?‘CMD_JOINT_MOVE‘
????def?__init__（self?joint?theta?rspeed=None）:
????????self.is_active?=?False
????????self.joint?=?joint?#?關節ID
????????self.theta?=?theta?#?關節的目標角度
????????if?rspeed?is?None:
????????????self.rspeed?=?config[‘JOINT_DEFAULT_ROTATIONAL_SPEED‘]
????????else:
????????????self.rspeed?=?rspeed?#?轉速?rotational?speed

????def?active_cmd（self）:
????????‘‘‘激活當前的指令‘‘‘
????????self.is_active?=?True

????def?update（self）:
????????‘‘‘更新關節旋轉角度‘‘‘
????????#?每次舵機移動的幅度
????????step?=?（self.rspeed?/?1000）?*?self.TIMER_PERIOD
????????cur_theta?=?self.joint.get_radius（）
????????if?abs（self.theta?-?cur_theta）?<=?step*15:
???????????step=step/（2.7+（abs（self.theta?-?cur_theta）/step/10）*（-1））
????????if?abs（self.theta?-?cur_theta）?<=?step:
????????????#?如果差距小于step的絕對值?一步到位
????????????self.joint.set_radius（self.theta）
????????????self.is_active?=?False?#?標記完成了當前的指令
????????elif?self.theta?>?self.joint.theta:
????????????self.joint.set_radius（cur_theta?+?step）
????????else:
????????????self.joint.set_radius（cur_theta?-?step）

class?JointMoveBatch（Command）:
????‘‘‘多個關節的控制信號‘‘‘
????CMD_TYPE?=?‘CMD_JOINT_MOVE_BATCH‘

????def?__init__（self?arm）:
????????#?定義關節跟機械爪
????????self.arm?=?arm?#?機械臂對象

????????self.move_batch?=?{}?#?多個關節的弧度控制

????def?add_move（self?joint_id?theta?rspeed=None）:
????????‘‘‘添加一個關節的動作‘‘‘
????????#?NOTE:?MicroPython?eval對local（局部）變量無效?因此不能使用self
????????#?joint?=?eval（‘self.arm.{}‘.format（joint_id）?globals（）{‘self‘:?self}）
????????joint?=?None
????????if?joint_id?==?‘joint1‘:
????????????joint?=?self.arm.joint1
????????elif?joint_id?==?‘joint2‘:
????????????joint?=?self.arm.joint2
????????elif?joint_id?==?‘joint3‘:
????????????joint?=?self.arm.joint3
????????elif?joint_id?==?‘joint4‘:
????????????joint?=?self.arm.joint4
????????self.move_batch[joint_id]?=?JointMoveCommand（joint?theta?rspeed）

????def?active_cmd（self）:
????????‘‘‘激活指令‘‘‘
????????self.is_active?=?True
????????for?joint_move_cmd?in?self.move_batch.values（）:
????????????joint_move_cmd.is_active?=?True

????def?update（self）:
????????‘‘

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????目錄???????????0??2019-04-06?15:57??備份\
?????文件????????5730??2019-03-26?20:16??備份\command.py
?????文件????????1410??2019-03-29?11:08??備份\configs.py
?????文件????????3374??2019-03-26?20:26??備份\joints.py
?????文件????????1485??2019-02-10?09:10??備份\pca9685.py
?????文件???????12097??2019-03-26?20:08??備份\robot_arm.py
?????文件????????2461??2019-03-15?17:06??備份\servos.py
?????文件????????7723??2019-04-02?09:34??備份\[比賽2]mv_car5.3?[不檢查顏色的版本]?-?副本.py