\ \\\\\\\\\\\\\\\\\\ TURRET CONTROL FUNCTIONS \\\\\\\\\\\\\\\\\\\\\\\\ \ Requires RTXEB patches (rtxeb_patches.fs), real time clock functions \ (rtc.fs), and sensor functions (sensors.fs) be loaded first HEX \ All numerical values are in hexidecimal format \ It is important to record the last state of the stepper motor prior to \ turning it off. Otherwise, the motor will jerk when started up with a \ random winding setting. The problem is determining what the initial rotor \ position is at powerup so the proper initial stepper state can be selected. VARIABLE TURRET_MOTOR_STATE \ Shadow storage for RTX2001A ASIC bus port 0x19 0 TURRET_MOTOR_STATE ! \ Initialize the turret motor state to zero VARIABLE FINAL_TURRET_STATE 9 FINAL_TURRET_STATE ! \ These functions assume that the turret bits stored in the variable \ FINAL_TURRET_STATE have been set to one of the acceptable phase values for \ the turret stepping functions that will be called. : TURRET-MOTOR-ON ( -- ) FINAL_TURRET_STATE @ \ Load final turret phase state from last use DUP 19 G! \ Copy the bits and write to ASIC bus port 0x19 TURRET_MOTOR_STATE ! \ Save the new turret motor state value ; : TURRET-MOTOR-OFF ( -- ) TURRET_MOTOR_STATE DUP \ Load and copy the turret motor state pointer @ \ Load the turret motor state value FINAL_TURRET_STATE ! \ Save final turret phase state for future use 0 DUP 19 G! \ Copy zero bits and write to ASIC bus port 0x19 SWAP ! \ Save the new turret motor state value ; \ This defining word assumes that the turret bits stored in the variable \ MOTOR_STATES have been set to one of the acceptable phase values that are \ listed when the new command is defined (see FULL-STEP and HALF-STEP below). \ The stack values expected when the defining word is invoked are the phase \ values listed in proper order and the total number of phases on the stack. : CONTROL-TURRET-MOTOR CREATE ( state[n] state[n-1] ... state[2] state[1] n -- ) OVER >R \ Put copy of first motor setting on return stack DUP PICK \ Get a copy of the last motor setting SWAP DUP , \ Save the motor data length FOR , NEXT \ Save most of the motor data R> , \ Retrieve first motor setting and save it DOES> ( dir -- ) SWAP DUP ABS 1 \ Check the direction flag for a value of 1 or -1 <> ABORT" Direction value must be 1 or -1!" SWAP TURRET_MOTOR_STATE \ Load address of shadow storage SWAP OVER @ \ Make a copy and load current motor shadow value SWAP @+2 \ Load in the count value; point to the next cell 2+ \ Skip to the first control bits comparison value SWAP 0 DO \ Iterate on the number of comparisons OVER SWAP \ Get a copy of the turret motor control bits @+2 \ Load in comparison value; point to the next cell -ROT \ Bring up the comparison value and control bits = IF \ If they are equal stop looping 2- \ Point to matched comparison value's address SWAP DROP \ Dispose of the control bits; no longer needed ROT \ Bring up the direction flag 2* + @ \ Use it to index to the next control bit values DUP 19 G! \ Make a copy and write it to ASIC bus port 0x18 SWAP ! \ Swap and save to the shadow storage 0 0 0 0 \ Shove four zeros on the stack to balance LEAVE \ Terminate the DO-LOOP now THEN LOOP OR OR OR 0<> ABORT" Illegal turret motor bit pattern encountered!" ; 0009 000A 0006 0005 4 CONTROL-TURRET-MOTOR FULL-STEP \ When the robot is powered up or reset, the rotational position of the turret \ is unknown. Knowledge of the rotational position is important for alignment \ of the ranging function. A Hall Effect sensor is installed on the robot and \ a small magnet is on the turret. When the turret is rotated so it is facing \ directly forward, the magnet is directly above the Hall Effect sensor. This \ causes a logic "1" value to be read on bit 4 of the RTXEB ASIC bus port 0x19. \ The value of the bit is logic '0' when the turret is rotated such that the \ magnet is no longer suspended directly over the sensor. \ This "turret index" signal is also used by the robot to determine the current \ phase of the stepper is when the turret is facing directly forward. This is \ important information that can be preserved between uses of the turret. It \ allows the robot to turn off power to the stepper motor between uses, thus \ saving power. When the stepper motor windings are re-energized for use, \ the same motor phase value can be applied that was present when the motor \ was turned off. This prevents the motor from jerking to a different phase \ orientation, which would induce turret miss-alignment on motor power-up. \ This function presumes that the motor has been activated. The parameter on \ the stack are 'dir' the rotation direction (ccw = 1, cw = -1) and the turret \ index flag value to test for ('TRUE' or 'FALSE'). Exits with TOS containing \ the number of steps (0 - 69) required to reach the desired turret index \ state. If the turret performs one full rotation (70 steps) and the index \ condition is not met, then something is wrong with the turret index sensing \ hardware or the turret motor. Return -1. : ROTATE-FOR-TURRET-INDEX ( dir iflag -- scount ) -1 -ROT \ Place failure code onto stack 46 0 DO \ 70 steps maximum; one full rotation 64 S>D WAIT-N-TICKS \ Wait for 100 ticks (500 ms) to transpire DUP TURRET-ON-INDEX? \ Duplicate index condition and get value ." Current index: " \ DEBUG STATEMENT DUP . CR \ DEBUG STATEMENT = IF \ If index condition matches the index value ROT DROP \ Remove the failure code I \ Loop index indicates number of steps taken -ROT \ Replace failure code with step count LEAVE \ Exit the loop now THEN OVER FULL-STEP \ Step motor one full step in stated direction LOOP \ Perform next iteration DROP DROP \ Discard iflag and dir DUP . ." STEPS COUNTED" CR \ DEBUG STATEMENT ; \ Results codes: \ -1 - A failure has occurred in finding the proper turret index state. \ -2 - Count failure on counter clockwise rotation of turret \ -3 - Count failure on clockwise rotation of turret \ -4 - Error on turret index detection after final positioning : ALIGN-TURRET ( -- code ) -1 FALSE ROTATE-FOR-TURRET-INDEX \ Rotate cw until turret index becomes 'FALSE' DUP 0< IF \ Test if failure has occurred EXIT \ Error if turret index never becomes 'FALSE' THEN DROP \ Don't need to retain count of steps taken ." Turret off index cw" CR \ DEBUG STATEMENT 1 FALSE ROTATE-FOR-TURRET-INDEX \ Rotate ccw until turret index becomes 'FALSE' DUP 0< IF \ Test if failure has occurred EXIT \ Error if turret index never becomes 'FALSE' THEN DROP \ Don't need to retain count of steps taken ." Turret off index ccw" CR \ DEBUG STATEMENT 1 TRUE ROTATE-FOR-TURRET-INDEX \ Rotate ccw until turret index becomes 'TRUE' DUP 0< IF \ Test if failure has occurred EXIT \ Error if turret index never becomes 'TRUE' THEN DROP \ Don't need to retain count of steps taken ." Turret on index ccw" CR \ DEBUG STATEMENT 1 FALSE ROTATE-FOR-TURRET-INDEX \ Rotate ccw until turret index becomes 'FALSE' DUP 0< IF \ Test if failure has occurred EXIT \ Error if turret index never becomes 'FALSE' THEN DUP 0= IF \ Test if no steps were taken DROP -1 1- EXIT \ Error code of -2 returned THEN \ Note that the ccw count of steps is retained ." Turret off index ccw" CR \ DEBUG STATEMENT -1 TRUE ROTATE-FOR-TURRET-INDEX \ Rotate cw until turret index is 'TRUE' DUP 0< IF \ Test if failure has occurred SWAP DROP \ Discard the ccw count of steps EXIT \ Error if turret index never becomes 'TRUE' THEN DROP \ Don't need to retain count of steps taken ." Turret on index cw" CR \ DEBUG STATEMENT -1 FALSE ROTATE-FOR-TURRET-INDEX \ Rotate cw until turret index becomes 'FALSE' DUP 0< IF \ Test if failure has occurred SWAP DROP \ Discard the ccw count of steps EXIT \ Error if turret index never becomes 'FALSE' THEN DUP 0= IF \ Test if no steps were taken DROP DROP \ Discard the ccw and cw count of steps -1 1- 1- EXIT \ Error code of -3 returned THEN \ Note that the ccw count of steps is retained ." Turret off index cw" CR \ DEBUG STATEMENT + 2/ \ Find average of ccw counts and cw counts DUP 0= IF \ Check for being forced to zero 1+ \ Increment so that there is one step to center THEN ." Final count: " DUP . CR \ DEBUG STATEMENT 0 DO \ Rotate ccw by average counts 64 S>D WAIT-N-TICKS \ Wait for 100 ticks (500 ms) to transpire 1 FULL-STEP \ Step motor one full step in stated direction LOOP \ Perform next iteration 64 S>D WAIT-N-TICKS \ Wait for 100 ticks (500 ms) to transpire TURRET-ON-INDEX? \ Monitor for turret index to be 'TRUE' now 0= IF \ If turret index is not 'TRUE' now -1 1- 1- 1- \ Error code of -4 is returned ." Turret not on index!" \ DEBUG STATEMENT ELSE 0 ." Turret on index!" \ DEBUG STATEMENT THEN CR \ DEBUG STATEMENT ; : TEST-ALIGN-TURRET ( phase -- ) RTC-INIT \ Initialize the real time clock TURRET-MOTOR-ON \ Turn on the turret motor ALIGN-TURRET \ Perform the turret alignment TURRET-MOTOR-OFF \ Deactivate the motor, saving the motor phase RTC-HALT \ Halt the real time clock ; \ \\\\\\\\\\\\\\\ END OF TURRET CONTROL FUNCTIONS \\\\\\\\\\\\\\\\\\\\\\\\