|
' JOYSTIKB.bs2
' 01/22/99
'copyright by Carl A. Kollar
'
'This program shows the control of 2 motors with a BS2 and a standard
'joystick. An LCD is used to monitor certain variables (when enabled) for
'developement purposes. It's just like JOYSTIKA.bs2 except that I've
'included a Joytick center indicator routine using LEDs to show when the
'joystick's are centered and cleaned up & organized the code. Also, the
'turn on center routines have been refined.
'---------------------------Define Constants-------------------------------
I con 254 'instruction prefix
ClrLCD con 1 'Clear LCD instruction
N9600 con 84+$4000 '9600 baud,8 bit, inverted, no parity
N300 con 3313+$4000 '300 baud, 8 bit, inverted, no parity
N1200 con 813 '1200 baud
N2400 con 396 '2400 baud
N4800 con 188 '4800 baud
lr_center_led con 14 'put l-r center led on port 14
fr_center_led con 15 'put f-r center led on port 15
lcd con 4
'-----------------------------Define Variables-----------------------------
pot1 var word 'F-R pot
pot2 var word 'L-R pot
pwmpin1 con 1 'fwd/reverse or left wheel
pwmpin2 con 2 'left/right or right wheel
cycles var byte: cycles =100
neutral var byte: neutral = 132 'pwm value for 2.5V out
lspeed var byte: lspeed=neutral 'left speed variable
rspeed var byte: rspeed=neutral 'right speed variable
deadband var nib: deadband=3
turn_speed var byte
x var byte: x = 0 'counter variable
'-------------------------Preliminary Stuff--------------------------------
gosub init_lcd 'Initialize LCD readout if used. If not, comment out
for x=1 to 5
gosub no_go
next
high 0 'turn on motor controls
'-------------------------------main---------------------------------------
main:
gosub measure_joystick_pots
'gosub pot_readout 'use only if LCD being used
gosub light_center_leds
gosub scale_pot_readings
gosub pot_readout 'use only if LCD being used
gosub calc_lspeed_rspeed
gosub motor
goto main
'-----------------Preliminary LCD stuff. Use only if LCD used---------------
init_lcd:
low lcd 'make the serial output low
pause 1000 'let the LCD wake up
serout lcd,N9600,[I,ClrLCD] 'clear the LCD
pause 50
'serout lcd,N9600,["F-R Pot: "] 'print the fixed label for F-R pot
'serout lcd,N9600,[I,192,"L-R Pot: "] 'print the fixed label for L-R pot
return
'----------------------------Neutral--------------------------------------
no_go:
pwm pwmpin1,neutral,cycles 'bring left mtr control voltage to 2.5V
' (neutral)
pwm pwmpin2,neutral,cycles 'bring right mtr control voltage to 2.5V
' (neutral)
return
'---------------------Measure Joystick Pots-------------------------------
measure_joystick_pots:
high 8 'discharge F-R cap on BS2.13
pause 5 'allow time for discharge
rctime 8,1,pot1 'measure F-R cap charge time
high 9 'discharge L-R cap on BS2.14
pause 5 'allow time for discharge
rctime 9,1,pot2 'measure L-R cap charge time
if pot1 = 0 or pot2 = 0 then shut_down'Uh Oh, no joystick attached
high 0'Otherwise turn power back on to the motor controls
return
'-----------------------Scale the pot readings----------------------------
scale_pot_readings:
' Original (unprocessed) joystick reading is 350. Divide by 20 = 17.
' Add 115 = 132 which is neutral to the motor control when used in
' the pwm command.
pot1 = (pot1/20)+115
pot2 = (pot2/20)+115
return
'-----------------Calculate Left & Right Wheel Speeds---------------------
calc_lspeed_rspeed:
if pot1 > neutral-deadband and pot1 < neutral+deadband then turn_in_place
if pot2 => neutral then turn_right
if pot2 <= neutral then turn_left
return
turn_right:
'set both to neutral value for calculation purposes:
lspeed = neutral: rspeed = neutral
'keep left wheel at current speed:
lspeed = pot1
'Actually, find out how far the LR joystick is from neutral:
rspeed = pot2 - neutral
'divide this amount by 2 to make turning not as critical:
rspeed = pot1 - (rspeed/2)
return
turn_left:
'set both to neutral value for calculation purposes:
lspeed = neutral: rspeed = neutral
'keep right wheel at current speed:
rspeed = pot1
'Actually, find out how far the LR joystick is from neutral:
lspeed = neutral - pot2
'divide this amount by 2 to make turning not as critical:
lspeed = pot1 - (lspeed/2)
return
turn_in_place:
'Which way (left or right) is the joystick being moved?
if pot2 => neutral then turn_right_center
if pot2 <= neutral then turn_left_center
return
turn_right_center:
'Find out how far the joystick has moved to the right, divide that
'number by 2 to make the turns not as touchy, then add 1 to speed it
'up a little:
turn_speed = ((pot2 - neutral)/2)+1
'Add the resulting number to the neutral value plus another 1 to
'"fine tune" the turning speed to get the speed value for the left
'wheel:
lspeed = neutral + turn_speed+1
'Subtract the "turn speed" value (after it has been multiplied by 2)
'from the neutral value to make the right wheel go in reverse.
'Multiplication by 2 is necessary because of motor control idiosyncracies
'which make the motor go slower in reverse than it does in forward
'for the same amount of joystick movement:
rspeed = neutral - (turn_speed*2)
return
turn_left_center:
'Find out how far the joystick has moved to the left, divide that
'number by 2 to make the turns not as touchy, then add 1 to speed it
'up a little:
turn_speed = (neutral-pot2)/2
'Subtract the "turn speed" value (after it has been multiplied by 2)
'from the neutral value to make the left wheel go in reverse.
'Multiplication by 2 is necessary because of motor control idiosyncracies
'which make the motor go slower in reverse than it does in forward
'for the same amount of joystick movement:
lspeed = neutral - (turn_speed*2)
'Add the resulting number to the neutral value plus another 1 to
'"fine tune" the turning speed to get the speed value for the right
'wheel:
rspeed = neutral + turn_speed+1
return
'-----------------------Motor PWM------------------------------------------
motor:
pwm pwmpin2,rspeed,10
pwm pwmpin1,lspeed,10
return
'----------------------Pot Readout-----------------------------------------
pot_readout:
serout lcd,N9600,[254,128,"F-R Pot= ", DEC pot1," "]
serout lcd,N9600,[254,192,"L-R Pot= ", DEC pot2," "]
return
'-----------------------Motor Readout--------------------------------------
motor_readout:
serout lcd,N9600,[254,128,"Left Motor= ", dec lspeed," "]
serout lcd,N9600,[254,192,"Right Motor= ",dec rspeed," "]
return
'***************** Light Center Calibration LED's**********************
light_center_leds:
'Check if F-R pot is near center
if pot1 <355 and pot1 >345 then fr_center
'otherwise turn off the F-R center LED:
low fr_center_led
goto next_light
fr_center:
'turn on the F-R center LED
high fr_center_led
next_light:
'Check if left-right pot is near center:
if pot2 < 355 and pot2 > 345 then lr_center
'if not, turn off the L-R centered LED:
low lr_center_led
return
lr_center:
'turn on the left-right centered LED
high lr_center_led
return
'-------------------------Emergency Shut Down------------------------------
shut_down:
low 0
return
|
