Skip to content

Commit

Permalink
fixed wrong wait for CMD start in acc/dec profile CMDs
Browse files Browse the repository at this point in the history
  • Loading branch information
@DietrichChristopeit
DietrichChristopeit committed May 17, 2021
1 parent 4ba505a commit b2fdb5b
Show file tree
Hide file tree
Showing 6 changed files with 255 additions and 248 deletions.
2 changes: 1 addition & 1 deletion .idea/misc.xml
View file

Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.

88 changes: 45 additions & 43 deletions MainProgs/Experiment_CMDs.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,12 +27,10 @@
from legoBTLE.device.SingleMotor import SingleMotor
from legoBTLE.device.SynchronizedMotor import SynchronizedMotor
from legoBTLE.legoWP.types import C
from legoBTLE.legoWP.types import CCW
from legoBTLE.legoWP.types import CW
from legoBTLE.legoWP.types import MESSAGE_STATUS
from legoBTLE.legoWP.types import MOVEMENT
from legoBTLE.legoWP.types import PORT
from legoBTLE.networking.prettyprint.debug import debug_info
from legoBTLE.networking.prettyprint.debug import debug_info_footer
from legoBTLE.networking.prettyprint.debug import prg_out_msg
from legoBTLE.user.Experiment import Experiment
Expand Down Expand Up @@ -155,55 +153,59 @@ async def main():
debug_info_footer(f"DEVICE SETUP DONE", debug=True)
# ############################ ENTER USER PROGRAMS HERE - START ##########################
# ############################### carlibrate Motor STR ##################################
prg_out_msg(f"\r\n\r\n{20*'*' } STARTING CUSTOMER MAIN PROGRAM {20*'*'}")
prg_out_msg('Starting motors in 5')
await asyncio.sleep(5)

# ########################## BEGIN PROGRAM STEERING CALIBRATION #########################

speed = 40
await STR.START_MOVE_DEGREES(cmd_id='1st EXTREME', on_stalled=STR.STOP(cmd_id='1st STOP'), degrees=180,
speed=CCW(speed), abs_max_power=20, on_completion=MOVEMENT.COAST)
await asyncio.sleep(1.0)
await STR.SET_POSITION(0, cmd_id='1st SET_POS')
speed = 40
prg_out_msg(f"JUST CHECKING: 1st POS_RESET IN DEG: \t {STR.port_value.m_port_value_DEG}")
await STR.START_MOVE_DEGREES(cmd_id='2nd EXTREME', on_stalled=STR.STOP(cmd_id='2nd STOP'), degrees=288,
speed=CW(speed), abs_max_power=20, on_completion=MOVEMENT.COAST)
await asyncio.sleep(1.0)
max_deg = abs(STR.port_value.m_port_value_DEG)
debug_info(f"{C.BOLD}{C.FAIL}MAX: {max_deg}", debug=True)
mid = int(round((max_deg / 2)))
STR.max_steering_angle = abs(mid)
prg_out_msg(f"MID POS: {mid}")
await STR.SET_POSITION(0, cmd_id='2nd SET_POS')
prg_out_msg(f"JUST CHECKING 2nd POS_RESET: POS IN DEG: \t {STR.port_value.m_port_value_DEG}")

await STR.START_MOVE_DEGREES(on_stalled=STR.STOP(cmd_id='3rd STOP'), degrees=mid, speed=CCW(speed), abs_max_power=100,
cmd_id='GO_ZERO', on_completion=MOVEMENT.COAST)
await STR.SET_POSITION(0)
prg_out_msg(f"JUST CHECKING 3rd POS_RESET: POS IN DEG: \t {STR.port_value.m_port_value_DEG}")
prg_out_msg(f"FINISHED FINISHED FINISHED")
await STR.START_MOVE_DEGREES(on_stalled=STR.STOP(cmd_id='30° RIGHT STOP'), degrees=30, speed=CW(40), abs_max_power=40,
cmd_id='30° RIGHT')
prg_out_msg(f"JUST CHECKING '30° RIGHT': POS IN DEG: \t {STR.port_value.m_port_value_DEG}")
await STR.GOTO_ABS_POS(on_stalled=STR.STOP(cmd_id='4th STOP'), position=0, speed=CCW(40), abs_max_power=40,
cmd_id='0° mid 1.')
await STR.GOTO_ABS_POS(on_stalled=STR.STOP(cmd_id='5th STOP'), position=0, speed=CW(40), abs_max_power=40,
cmd_id='0° mid 2.')
prg_out_msg(f"JUST CHECKING '0°' 2.: POS IN DEG: \t {STR.port_value.m_port_value_DEG}")

# await RWD.START_SPEED_TIME(5000, CW(80), on_stalled=RWD.STOP(cmd_id='RWD STOP', debug=True), cmd_id='RWD SPEED TIME')


# ############################## END PROGRAM STEERING CALIBRATION #################
# speed = 40
# await STR.START_MOVE_DEGREES(cmd_id='1st EXTREME', on_stalled=STR.STOP(cmd_id='1st STOP'), degrees=180,
# speed=CCW(speed), abs_max_power=20, on_completion=MOVEMENT.COAST)
# await asyncio.sleep(1.0)
# await STR.SET_POSITION(0, cmd_id='1st SET_POS')
# speed = 40
# prg_out_msg(f"JUST CHECKING: 1st POS_RESET IN DEG: \t {STR.port_value.m_port_value_DEG}")
# await STR.START_MOVE_DEGREES(cmd_id='2nd EXTREME', on_stalled=STR.STOP(cmd_id='2nd STOP'), degrees=288,
# speed=CW(speed), abs_max_power=20, on_completion=MOVEMENT.COAST)
# await asyncio.sleep(1.0)
# max_deg = abs(STR.port_value.m_port_value_DEG)
# debug_info(f"{C.BOLD}{C.FAIL}MAX: {max_deg}", debug=True)
# mid = int(round((max_deg / 2)))
# STR.max_steering_angle = abs(mid)
# prg_out_msg(f"MID POS: {mid}")
# await STR.SET_POSITION(0, cmd_id='2nd SET_POS')
# prg_out_msg(f"JUST CHECKING 2nd POS_RESET: POS IN DEG: \t {STR.port_value.m_port_value_DEG}")
#
# await STR.START_MOVE_DEGREES(on_stalled=STR.STOP(cmd_id='3rd STOP'), degrees=mid, speed=CCW(speed), abs_max_power=100,
# cmd_id='GO_ZERO', on_completion=MOVEMENT.COAST)
# await STR.SET_POSITION(0)
# prg_out_msg(f"JUST CHECKING 3rd POS_RESET: POS IN DEG: \t {STR.port_value.m_port_value_DEG}")
# prg_out_msg(f"FINISHED FINISHED FINISHED")
# await STR.START_MOVE_DEGREES(on_stalled=STR.STOP(cmd_id='30° RIGHT STOP'), degrees=30, speed=CW(40), abs_max_power=40,
# cmd_id='30° RIGHT')
# prg_out_msg(f"JUST CHECKING '30° RIGHT': POS IN DEG: \t {STR.port_value.m_port_value_DEG}")
# await STR.GOTO_ABS_POS(on_stalled=STR.STOP(cmd_id='4th STOP'), position=0, speed=CCW(40), abs_max_power=40,
# cmd_id='0° mid 1.')
# await STR.GOTO_ABS_POS(on_stalled=STR.STOP(cmd_id='5th STOP'), position=0, speed=CW(40), abs_max_power=40,
# cmd_id='0° mid 2.')
# prg_out_msg(f"JUST CHECKING '0°' 2.: POS IN DEG: \t {STR.port_value.m_port_value_DEG}")
#
# # await RWD.START_SPEED_TIME(5000, CW(80), on_stalled=RWD.STOP(cmd_id='RWD STOP', debug=True), cmd_id='RWD SPEED TIME')
#
#
# # ############################## END PROGRAM STEERING CALIBRATION #################

# ############################## Drive For 10 meter ################################
prg_out_msg(f"DRIVE for 1m")
prg_out_msg(f"DRIVE for 10m")
await FWD.SET_ACC_PROFILE(ms_to_full_speed=4000, profile_nr=1, debug=True)
await FWD.SET_DEC_PROFILE(ms_to_zero_speed=5000, profile_nr=1, debug=True)
await FWD.START_MOVE_DISTANCE(100000, CW(100), abs_max_power=100, on_completion=MOVEMENT.HOLD, use_profile=1, use_acc_profile=MOVEMENT.USE_ACC_PROFILE, use_dec_profile=MOVEMENT.USE_DEC_PROFILE)
# ############################# END: DRIVE FOR 1 m ################################

await FWD.START_MOVE_DISTANCE(distance=10000, speed=CW(100), abs_max_power=100,
on_completion=MOVEMENT.HOLD, use_profile=1,
use_acc_profile=MOVEMENT.USE_ACC_PROFILE, use_dec_profile=MOVEMENT.USE_DEC_PROFILE,
debug=True)
# ############################# END: DRIVE FOR 10 m ################################
prg_out_msg(f"\r\n\r\n{20 * '*'} END CUSTOMER MAIN PROGRAM {20 * '*'}")
# ############################ ENTER USER PROGRAMS HERE - END ##########################
while True:
prg_out_msg(f"JUST CHECKING '0° LEFT': POS IN DEG: \t {STR.last_value.m_port_value_DEG}")
Expand All @@ -219,6 +221,6 @@ async def main():
loopy = asyncio.get_event_loop()

t0 = datetime.timestamp(datetime.now())
asyncio.run(main(), debug=False)
asyncio.run(main(), debug=True)
loopy.run_forever()
print(f"Overall RUNTIME: {datetime.timestamp(datetime.now()) - t0}")
8 changes: 8 additions & 0 deletions Pipfile
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,8 +17,16 @@ sphinx-autodoc-typehints = "*"
autodoc = "*"
sphinx-rtd-theme = "*"
colorlog = "*"
docutils = "<0.17.1"
decorator = "==5.0.9"
idna = "==3.1"
jinja2 = "<=3.0.0"
markupsafe = "*"

[dev-packages]

[requires]
python_version = "3.9"

[pipenv]
allow_prereleases = true
Loading

0 comments on commit b2fdb5b

Please sign in to comment.