' {$STAMP BS2} ' -----[ Title ]------------------------------------------------------ ' ' File...... ROVER5.BS2 ' Purpose... TAR Rover Servo Control ' Authors... Steve Holmes (sholmes@mindspring.com) ' Ken Boone (kenboone@aol.com) ' Started... 15 August 1997 ' Updated... 28 September 1997 ' Updated... 1 March 2009 to add {$STAMP BS2} on the first line for the enviroment ' BS2-IC Connections: ' ' Pin 0 Control1 SteerCtl ' Pin 1 Control2 SpeedCtl ' Pin 2 NC ' Pin 3 NC ' Pin 4 NC ' Pin 5 NC ' Pin 6 NC ' Pin 7 NC ' Pin 8 NC ' Pin 9 NC ' Pin 10 Servo6 LDrive ' Pin 11 Servo5 RDrive ' Pin 12 Servo4 LRServo ' Pin 13 Servo3 LFServo ' Pin 14 Servo2 RRServo ' Pin 15 Servo1 RFServo ' -----[ Revision History ]------------------------------------------ ' ' 970815 : Initial Release ' 970827 : (krb) Calibrated turn servoes ' Inverted speed signal to right side during straight travel ' 970828 : (krb) Removed servo delays and loops ' Replaced gosubs with gotos ' Shortened input tests ' Added test for stop which stops servo signals (saves power) '970829 : (krb) Rewrote Adjust_Speed: section to be a very tight ' loop at the top of the program space for maximum cycle ' speed. ' Modified Turn: to remove the dead band in the turn speed '970915 : (krb) added traps for pulses out of range. '970928 (krb) adjusted traps. ' YYMMDD : ' -----[ Constants ]---------------------------------------- ' ----- Wheel Steering Parameters ---- ' Set these values to adjust the turn range of the ' servos. We only have the input range of the servos ' to work with though, about 90 degrees ' %% Right Front %% STRAIGHT_RF CON 621 TURN_RF CON 300 ' %% Right Rear % STRAIGHT_RR CON 690 TURN_RR CON 1010 ' %% Left Front %% STRAIGHT_LF CON 760 TURN_LF CON 1050 ' %% Left Rear %% STRAIGHT_LR CON 750 TURN_LR CON 450 ' ----- RC Receiver Input Control Parameters -------------------- ' For forward reverse contorl; ' 753 @ center, 976 @ forward limit, 536 @ back ' For turn control 753 @ center, 978 @ right limit, 522 @ left RIGHT_TURN CON 653 LEFT_TURN CON 853 FORWARD CON 763 _REVERSE CON 743 SteerTimer VAR Word 'Used to count program loops 'while servos are turning MAX_STEER_TIME CON 20 'Number ofprogram loops to turn 'steering servos '------[ Stamp2 Port Pins ]------------------------------------ SteerCtl CON 0 SpeedCtl CON 1 RFServo CON 15 RRServo CON 14 LFServo CON 13 LRServo CON 12 RDrive CON 11 LDrive CON 10 '-----[ Variables ]---------------------------------------------- Speed VAR Word 'Value read in from RC receiver Steer VAR Word 'Value read in from RC receiver RF_setpt VAR Word 'Position for Right Front servo RR_setpt VAR Word 'Position for Right Rear servo LF_setpt VAR Word 'Position for Left Front servo LR_setpt VAR Word 'Position for Left Rear servo LSpeed VAR Word 'Speed for Left side drive servos RSpeed VAR Word 'Speed for Right side drive servos Op_State VAR Byte 'Operation state (STRAIGHT, 'STEERING or TURNING STRAIGHT CON 0 'Op_State value for STEERING CON 1 'Op_State value for TURNING CON 2 'Op_State value for ' Initialize Servo Outputs to outputs and ' control inputs to inputs. Init: DIRL = %00000000 ' unused DIRH = %11111000 ' Servo Pins OUTS = $0000 ' all outputs off to start RF_setpt = STRAIGHT_RF RR_setpt = STRAIGHT_RR LF_setpt = STRAIGHT_LF LR_setpt = STRAIGHT_LR LSpeed = 0 RSpeed = 0 Op_State = STRAIGHT ' Main() is the control loop for the ' rover. It reads the control inputs positions, ' determines if we are turning, calls the appropriate ' function to handle the control law, and sets the ' servos. Main: ' Pulse out all the servos to the current position GOTO Pulse_Servos ' Adjust_Speed is the very tight loop for forward and reverse motion ' It is run after all the wheels are all straightened out. It ' inverts the pulses going to the right side drive servos. It sends ' the appropriate pulses to all of the servos. It reads the inputs ' and tests them for being in range and forward motion. Adjust_Speed: RSpeed = ((983-Speed)+523) 'Invert Speed for Right side ' LSpeed = Speed 'Left does not need invert 'Pulse_Servos: 'Sends one pulse to each servo PULSOUT RFServo,STRAIGHT_RF PULSOUT RRServo,STRAIGHT_RR PULSOUT LFServo,STRAIGHT_LF PULSOUT LRServo,STRAIGHT_LR PULSOUT LDrive,Speed PULSOUT RDrive,RSpeed Read_Input2: 'Reads inputs from RC Receiver PULSIN SpeedCtl,1,Speed 'Reads Speed input PULSIN SteerCtl,1,Steer 'Reads Steering input 'Traps test for out of range inputs IF Steer<450 OR Steer>1050 THEN Read_Input2 'stearing IF Speed<450 OR Speed>1050 THEN Read_Input2 'forward/back IF SteerLEFT_TURN THEN TURN 'Go to TURN: if Steer value 'not in dead band IF Speed<_REVERSE OR Speed>FORWARD THEN Adjust_Speed 'Go to Adjust_Speed if Speed 'value not in dead band GOTO Read_Input 'All inputs in dead band read 'again Pulse_Servos: 'Sends one pulse to each servo PULSOUT RFServo,RF_setpt PULSOUT RRServo,RR_setpt PULSOUT LFServo,LF_setpt PULSOUT LRServo,LR_setpt IF LSpeed = 0 THEN Read_input 'Does not update drive servos 'while turning wheels PULSOUT LDrive,LSpeed PULSOUT RDrive,RSpeed ' Collect commands from receiver Read_Input: 'From RC Receiver PULSIN SpeedCtl,1,Speed 'Reads Speed input PULSIN SteerCtl,1,Steer 'Reads Steer input 'Debug Output commands ' debug ? Steer ' debug ? Speed ' goto Read_Input: ' uncomment the lines ABOVE to check the input ranges ' uncomment the lines BELOW to check the state machine ' debug ? SteerTimer ' debug ? Op_State 'Traps test for out of range inputs IF Steer<450 OR Steer>1050 THEN Read_Input 'stearing IF Speed<450 OR Speed>1050 THEN Read_Input 'forward/back IF SteerLEFT_TURN THEN TURN 'Go to TURN: if Steer value 'not in dead band IF Speed<_REVERSE OR Speed>FORWARD THEN GO_STRAIGHT 'Go to GO_STRAIGHT: if Steer 'value not in dead band GOTO Read_Input 'Read inputs again. Inputs in 'dead band ' TURN ' ' This function handles turning of the rover, if the ' wheels are in the right position it turns the rover, ' if the wheels are STRAIGHT it goes into STEERING mode, ' stopping the rover and steering the wheels, once the ' wheels are steered it adjusts the "throttle" according ' to the Steer variable. TURN: IF Op_State = TURNING THEN Turn_Speed IF Op_State = STEERING THEN Wait_Turn Set_Turn: ' We need to set the wheels for a turn Op_State = STEERING 'Set Op_State SteerTimer = 0 'Reset SteerTimer RF_setpt = TURN_RF 'Set servo command values RR_setpt = TURN_RR LF_setpt = TURN_LF LR_setpt = TURN_LR LSpeed = 0 'Set Drive servos to stop RSpeed = 0 ' The steering control is just bang-bang for now ' we will wait a while for the servos to catch up ' if the servos need to be turned slowly in can be ' done here Wait_Turn: IF SteerTimer > MAX_STEER_TIME THEN Turn_Speed SteerTimer = SteerTimer + 1 GOTO Pulse_Servos Turn_Speed: 'Finished turning steering 'servos. Ready to start 'turning rover Op_State = TURNING 'Set Op_State to TURNING IF Steer<=RIGHT_TURN THEN Turn_Right 'Test for turning right RSpeed = Steer - 100 'Subtracts out 100 count dead LSpeed = Steer - 100 'band in left Steer control 'speed GOTO Pulse_Servos Turn_Right: RSpeed = Steer + 100 'Adds 100 count dead band to LSpeed = Steer + 100 'right Steer control speed GOTO Pulse_Servos ' GO_STRAIGHT ' ' This function handles moving the rover staight. ' if the rover was in STEERING or TURNING the wheels ' are straightened. once the wheels are in the right ' position, the throttle is adjusted according to the ' Speed input. GO_STRAIGHT: IF Op_State = STRAIGHT THEN Wait_Straight ' We need to straighted out Op_State = STRAIGHT SteerTimer = 0 RF_setpt = STRAIGHT_RF RR_setpt = STRAIGHT_RR LF_setpt = STRAIGHT_LF LR_setpt = STRAIGHT_LR LSpeed = 0 RSpeed = 0 ' The steering control is just bang-bang for now ' we will wait a while for the servos to catch up ' if the servos need to be turned slowly in can be ' done here Wait_Straight: IF SteerTimer > MAX_STEER_TIME THEN Adjust_Speed 'Go to Adjust_Speed 'when wheels are 'straight SteerTimer = SteerTimer + 1 GOTO Pulse_Servos