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

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

class?DelayCommand（Command）:
????‘‘‘延時(shí)類(lèi)指令‘‘‘
????CMD_TYPE?=?‘CMD_DELAY‘
????def?__init__（self?delay_ms）:
????????self.delay_ms?=?delay_ms?#?延時(shí)的時(shí)間?ms
????????self.time_cnt?=?0?#?已經(jīng)經(jīng)過(guò)的時(shí)間

????def?active_cmd（self）:
????????‘‘‘激活當(dāng)前指令‘‘‘
????????self.is_active?=?True?#?激活該指令
????????self.time_cnt?=?0?#?時(shí)間計(jì)數(shù)清零

????def?update（self）:
????????‘‘‘定時(shí)器回調(diào)函數(shù)?period是Timer的周期（常數(shù)）‘‘‘
????????self.time_cnt?+=?self.TIMER_PERIOD
????????if?self.time_cnt?>?self.delay_ms:
????????????self.is_active?=?False?#?指令執(zhí)行結(jié)束

class?JointMoveCommand（Command）:
????‘‘‘單個(gè)關(guān)節(jié)角度的控制信號(hào)‘‘‘
????CMD_TYPE?=?‘CMD_JOINT_MOVE‘
????def?__init__（self?joint?theta?rspeed=None）:
????????self.is_active?=?False
????????self.joint?=?joint?#?關(guān)節(jié)ID
????????self.theta?=?theta?#?關(guān)節(jié)的目標(biāo)角度
????????if?rspeed?is?None:
????????????self.rspeed?=?config[‘JOINT_DEFAULT_ROTATIONAL_SPEED‘]
????????else:
????????????self.rspeed?=?rspeed?#?轉(zhuǎn)速?rotational?speed

????def?active_cmd（self）:
????????‘‘‘激活當(dāng)前的指令‘‘‘
????????self.is_active?=?True

????def?update（self）:
????????‘‘‘更新關(guān)節(jié)旋轉(zhuǎn)角度‘‘‘
????????#?每次舵機(jī)移動(dòng)的幅度
????????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的絕對(duì)值?一步到位
????????????self.joint.set_radius（self.theta）
????????????self.is_active?=?False?#?標(biāo)記完成了當(dāng)前的指令
????????elif?self.theta?>?self.joint.theta:
????????????self.joint.set_radius（cur_theta?+?step）
????????else:
????????????self.joint.set_radius（cur_theta?-?step）

class?JointMoveBatch（Command）:
????‘‘‘多個(gè)關(guān)節(jié)的控制信號(hào)‘‘‘
????CMD_TYPE?=?‘CMD_JOINT_MOVE_BATCH‘

????def?__init__（self?arm）:
????????#?定義關(guān)節(jié)跟機(jī)械爪
????????self.arm?=?arm?#?機(jī)械臂對(duì)象

????????self.move_batch?=?{}?#?多個(gè)關(guān)節(jié)的弧度控制

????def?add_move（self?joint_id?theta?rspeed=None）:
????????‘‘‘添加一個(gè)關(guān)節(jié)的動(dòng)作‘‘‘
????????#?NOTE:?MicroPython?eval對(duì)local（局部）變量無(wú)效?因此不能使用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）:
????????‘‘

?屬性????????????大小?????日期????時(shí)間???名稱(chēng)
-----------?---------??----------?-----??----
?????目錄???????????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