'SERVO - Program to test the Micro Power SG90 servo motor using
' the original mono-colour Maximite and MMBasic V4.4
' ----------------------------------------------
'Normal analogue servos hold position dependant on the incoming pulse width on
'the control lead. The pulse stream needs to be maintained to hold servo
'position and the normal pulse repetition rate is approx 50Hz but this is not
'critical over quite a wide range.
'The centre (neutral) posn'n for most servos is at a pulse width of approx.
'1.5mS and the full pulse width range for most servos is approx 1mS to 2mS.
'The pulse width range needs calibrating for different servos so as to achieve
'their full movement range but not over-drive and thus possibly damage them.
'Pulse width Vs position is usually not fully linear, i.e. mid pulse width is
'not necessarily the centre servo position. Shorter pulses rotate the SG90 arm
'clockwise but this can be different for other servos.
'
'The SG90 has three leads:
' a) Gnd (usually brown)
' b) 5v supply (usually red - and usually the centre lead). This draws
' typically 50-80mA when moving and 5-10mA when holding or when
' there is no control pulse input. If over-driven the current can be
' above 200mA and damage may occur. Monitoring current on the servo
' lead is a good idea when testing!
' c) pulse control lead (usually yellow).
'
'Testing is conveniently done using a 10K linear pot connected across the
'Maximite's 3.3v supply and connecting the pot wiper to one of the Maximite
'analogue input pins via a 220 ohm resistor. Voltage at that pin can then be
'read by the program and scaled to provide the control pulses. The amplitude
'of the control pulses is not critical but should not exceed the 5v servo
'supply voltage. 3.3v pulses work well.
'
'The servo can be controlled with either the PULSE or PWM command. Control
'using PULSE requires the program to maintain the pulse stream. Control using PWM
'is simple and provides a continuous pulse stream in the program background till
'changed with another PWM command or stopped with a PWM STOP command. Another
'method is to connect the PWM output running at 50Hz to an input interrupt pin
'and issue PULSE commands within the interrupt routine.
'When using the PWM command the first argument is frequency and the second
'argument is a percentage of the period. At 50 Hz the period is 20mS, thus e.g.
'10% of that equates to a 2mS pulse.
'
'NOTE 1: versions of MMBasic prior to V4.4 had a bug which limited the accuracy
'of the pulse width (it's 'resolution') from the PWM command to about 1%, i.e.
'0.2mS at 50Hz which
'equates to around 10% of the full servo range. Thus using PWM control with
'MMBasic versions earlier than V4.4 creates very 'stepped' output. With V4.4 the
''resolution' is 0.1%, i.e. 0.02mS at 50hZ which gives fine control.
'
'NOTE 2: when using PWM control, the Maximite's PWM output circuit must have
'the voltage divider network removed to supply the full 3.3v pulse height.
'
' -----------------------------------------------------
'Rough SG90 pulse width Vs position calib'n using the PULSE command.
' Full range on the SG90 is: 2.4 mS (~90o) to 0.6mS (~270o) clockwise.
'
'Rough SG90 PWM command Vs position calib'n when running at 50Hz (20mS period).
' Full range: 12.4% (~90o) to 3.2% (~270o) clockwise.
' -----------------------------------------------------
'
Initialise:
SetTick 0,0: SetPin 11,0: SetPin 5,0: PWM STOP
Print "Input type of servo control to use:"
Print "1=Keybd_PULSE, 2=Keybd_PWM, 3=Pot_PULSE, 4=Pot_PWM"
Input controltype
On controltype GoTo Keybd_PULSE,Keybd_PWM,Pot_PULSE,Pot_PWM

Keybd_PULSE:
SetPin 11,8 'set pin 11 as digital out
Pin(11)=0 'set pin 11 to 0 (in case it's at 1)
Input "Input pulse width in mSecs: ",width 'range approx 0.6 to 2.4 mS
SetTick 20,Pulseout 'set timer interrupt for pulse out every 20mS (50Hz)
GOSUB Wait
goto Keybd_PULSE

Keybd_PWM:
Input "Input pulse width in mSecs: ",width 'range approx 0.6 to 2.4 mS
percent = width*5 'assuming frequency of 50Hz (i.e. 20mS period), thus %=width*100/20
PWM 50,percent 'start 50 Hz continuous pulse train
GOSUB Wait
goto Keybd_PWM

Pot_PULSE:
SetPin 11,8 'set pin 11 as digital out
Pin(11)=0 'set pin 11 to 0 (in case it's at 1)
SetPin 5,1 'set pin 5 as analog in
SetTick 20,Pulseout 'interrupt for pulse out every 20mS (50Hz)
Do
volts = Pin(5) 'read wiper voltage at the pot position
width = (0.5454*volts)+0.6 'from: slope=(2.4-0.6)/3.3 and y=Sx+c

key$ = Inkey$
If key$ <> "" GOTO Initialise
loop

Pot_PWM:
SetPin 5,1 'set pin 5 for analog input
do
volts = Pin(5) 'read wiper voltage at the pot position
width = (0.5454*volts)+0.6 'from: slope=(2.4-0.6)/3.3 and y=Sx+c
percent = width*5 'assuming frequency of 50Hz (i.e. 20mS period), thus %=width*100/20
PWM 50,percent 'start 50 Hz continuous pulse train
Pause 20 'delay needed for PWM at 50Hz otherwise no pulse output

key$ = Inkey$
If key$ <> "" GOTO Initialise
loop

Pulseout:
Pulse 11,width
IReturn

Wait:
key$ = Inkey$
If key$ = "" GOTO Wait 'wait for a keyboard press to input another pulse width
return