Code: Select all
subroutine __RotateMotor0
brtst EQ, __rotate_Done0, __rotate_angle0
sign __rotate_thePower0, __rotate_angle0
abs __rotate_theAngle0, __rotate_angle0
mul __rotate_thePower0, __rotate_thePower0, __rotate_power0 // convert __rotate_power to negative value if __rotate_angle is negative.
set __rotate_theUF0, UF_UPDATE_TACHO_LIMIT+UF_UPDATE_SPEED+UF_UPDATE_MODE+UF_UPDATE_PID_VALUES
brtst EQ, __rotate_NoSync0, __rotate_sync0
set __rotate_theOM0, OUT_MODE_MOTORON+OUT_MODE_BRAKE+OUT_MODE_REGULATED
set __rotate_theRM0, OUT_REGMODE_SYNC
mov __rotate_theTurnPct0, __rotate_turnpct0
brtst EQ, __rotate_Start0, __rotate_theTurnPct0
add __rotate_theUF0, __rotate_theUF0, UF_UPDATE_RESET_BLOCK_COUNT
jmp __rotate_Start0
__rotate_NoSync0:
set __rotate_theOM0, OUT_MODE_MOTORON+OUT_MODE_BRAKE
set __rotate_theRM0, OUT_REGMODE_IDLE
set __rotate_theTurnPct0, 0
__rotate_Start0:
set __rotate_theRS0, OUT_RUNSTATE_RUNNING
setout __rotate_ports0, OutputModeField, __rotate_theOM0, RegModeField, __rotate_theRM0, TachoLimitField, __rotate_theAngle0, RunStateField, __rotate_theRS0, RegPValueField, __rotate_theRVP0, RegIValueField, __rotate_theRVI0, RegDValueField, __rotate_theRVD0, PowerField, __rotate_thePower0, TurnRatioField, __rotate_turnpct0, UpdateFlagsField, __rotate_theUF0
wait 0 // let the motor(s) start turning
// Waits till the angle is reached
index __rotate_firstPort0, __rotate_ports0, NA
__rotate_Running0:
getout __rotate_power0, __rotate_firstPort0, PowerField
brtst EQ, __rotate_doneRunning0, __rotate_power0
getout __rotate_rs0, __rotate_firstPort0, RunStateField
brcmp EQ, __rotate_Running0, __rotate_rs0, OUT_RUNSTATE_RUNNING
__rotate_doneRunning0:
brtst EQ, __rotate_Reset0, __rotate_stop0 // skip the speed regulation phase if __rotate_stop is false
// Regulates for speed = 0
set __rotate_theOM0, OUT_MODE_MOTORON+OUT_MODE_BRAKE+OUT_MODE_REGULATED
set __rotate_theUF0, UF_UPDATE_TACHO_LIMIT+UF_UPDATE_SPEED+UF_UPDATE_MODE
setout __rotate_ports0, OutputModeField, __rotate_theOM0, RegModeField, OUT_REGMODE_SPEED, RunStateField, __rotate_theRS0, PowerField, 0, TurnRatioField, 0, TachoLimitField, 0, UpdateFlagsField, __rotate_theUF0
wait 0 // let the firmware stop the motor(s)
// Verifies that motor doesn't rotate for 50ms, else loops
getout __rotate_RotCount0, __rotate_firstPort0, TachoCountField
__rotate_Stabilize0:
mov __rotate_OldRotCount0, __rotate_RotCount0
wait 50
// check rotation
getout __rotate_RotCount0, __rotate_firstPort0, TachoCountField
brcmp NEQ, __rotate_Stabilize0, __rotate_OldRotCount0, __rotate_RotCount0
// now remove the brake
set __rotate_theOM0, OUT_MODE_COAST+OUT_MODE_REGULATED
setout __rotate_ports0, RegModeField, __rotate_theRM0, RunStateField, OUT_RUNSTATE_IDLE, OutputModeField, __rotate_theOM0, UpdateFlagsField, UF_UPDATE_MODE
wait 0 // let the firmware release the motor(s)
__rotate_Reset0:
// maybe reset the block rotation count
brtst EQ, __rotate_Done0, __rotate_theTurnPct0
setout __rotate_ports0, UpdateFlagsField, UF_UPDATE_RESET_BLOCK_COUNT
__rotate_Done0:
return
ends
This is pretty easy to translate into NXC, if you need to. I will likely change these low level routines to use speed regulation if the rotate_sync variable is true and you are only controlling a single motor.