' pwm_to_gamoto.bs2
' Randy Gamage July 2004

' This program is a 'bridge' between a servo receiver and a Gamoto PID motor controller
' It takes a PWM input suitable for hobby R/C servos, and sends a corresponding
' serial command to the Gamoto Motor controller.
' This allows the [Stamp + Gamoto + motor + encoder] to act like a standard R/C servo

' There is no setup done to Gamoto - it must be pre-configured with desired
' Kp, Ki, Kd values, etc. but this will run as the Gamoto comes from the factory
' To custom-configure the Gamoto, use MotoView and then Save To Flash
' Also note that the serial response (ACK) from the Gamoto is ignored by this program

' This program uses PULSIN to measure a pulse generated by an RC Servo Receiver
' in the range of 1ms to 2ms in width, assuming 1.5ms center point.
' These values are adjustable in the Configuration Options section.
' PULSIN is read in units of 2 µs in the case of a Basic Stamp 2
' This code is written for the Basic Stamp 2, but can also be used with
' for the BS2e, BS2sx and BS2p also. Modify the STAMP directive before
' downloading to the BS2e, BS2sx or BS2p. Keep in mind that the unit of
' time may be different than what appears in the comments here.
'{$STAMP BS2} 'STAMP directive (specifies a BS2)

'Hardware Configuration
Ser_Pin CON 0       'Serial pin# to connect to Gamoto's RX pin
PWM_Pin CON 1       'PWM input pin#, to read pulse, connect to servo receiver output
BAUDMODE CON 32     '19200 baud (Set to 32 for 19200, 84 for 9600 Baud)

'Gamoto Constants
gxHEADER        CON $AA  'Serial Protocol header
gxSETPOSITION   CON 47   'Register location for setPosition register

'Configuration Options - Tweak as desired
POSITION_RANGE    CON 1000  'Range of motion, in encoder counts
'For example, if using 500 cnts/rev encoders, then a POSITION_RANGE
'of 1000 would give you a total motion range of 2 revolutions = 720 degrees

POSITION_CENTER CON 0 'Absolute encoder count value of the center position
'For example, if the center is 0 and POSITION_RANGE is 1000, then the
'Gamoto will be commanded to positions between -500 and +500.

PULSE_RANGE    CON 500  'Range of pulse width durations, in 2us units
'For example, 500 = 1000us = 1ms range, which is typical of a
'standard hobby servo, which min pulse = 1ms, max pulse = 2ms, centered at 1.5ms

PULSE_CENTER    CON 750 'Center pulse width, in 2us increments
'For example, 750 = 1500us = 1.5 ms, the standard center pulse width

'Scaling constants - Tweak Carefully
'These SCALERS are required due to the 16-bit signed size limitation of variables
'No result can exceed 32767, so care must be taken to prevent this
'Also, all division is integer, so try to multiply before dividing, as
'long as the multiplication doesn't result in > 32767
POS_SCALER1 CON 13 * POSITION_RANGE / 650  'Good for 100 < POSITION_RANGE < 2520
POS_SCALER2 CON 13
'For example, make sure the following cases is OK:
' 13 * POSITION_RANGE < 32767
'If POSITION_RANGE is smaller than 200, consider using these scalers:
'POS_SCALER1 CON 13 * POSITION_RANGE / 65
'POS_SCALER CON 130

'Calculated Constants - Don't Tweak
PULSE_MAX CON PULSE_RANGE/2 + PULSE_CENTER
PULSE_MIN CON PULSE_MAX - PULSE_RANGE
POSITION_MAX CON POSITION_RANGE/2 + POSITION_CENTER
POSITION_MIN CON POSITION_MAX - POSITION_RANGE

'RAM Variables
Time VAR Word     'Measured pulse duration
cs  VAR Byte      'Checksum for serial protocol
Len VAR Byte      'Length for serial protocol
setPosition VAR Word 'Desired position setpoint

MainLoop:
PULSIN PWM_Pin, 1, Time ' Measure positive pulse.
IF Time = 0 THEN MainLoop ' If no pulse, try again.
'Un-comment this line to debug:
'DEBUG CLS, DEC ? Time ' Display result.
'Boundary limit to specified ranges
IF Time >= PULSE_MIN THEN Not_Min
Time = PULSE_MIN
Not_Min:
IF Time <= PULSE_MAX THEN Not_Max
Time = PULSE_MAX
Not_Max:
'Calculate Position setpoint
setPosition = (Time-PULSE_MIN)*65/(PULSE_RANGE/10)*POS_SCALER1/POS_SCALER2 + POSITION_MIN
'Now tell the Gamoto to go to the setPosition
GOSUB SendPosition
GOTO MainLoop

SendPosition:
'Sends position command to Gamoto
'Note that due to 16-bit limitation of Stamp, we will sign-extend
'to the third byte, depending on pos or negative setting
Len = 4   'Length will always be 4 for this command / register combo
IF setPosition.HIGHBIT = 1 THEN NegPosition
'Positive Position Setpoint (sign-extend with a zero)
'Calculate checksum
cs = Len + gxSETPOSITION + setPosition.LOWBYTE + setPosition.HIGHBYTE
SEROUT Ser_Pin, BAUDMODE, [gxHEADER,Len,gxSETPOSITION,SetPosition.LOWBYTE,SetPosition.HIGHBYTE,0,cs]
GOTO Done
NegPosition:
'Negative Position Setpoint (sign-extend with a $FF)
'Calculate checksum
cs = Len + gxSETPOSITION + setPosition.LOWBYTE + setPosition.HIGHBYTE + $FF
SEROUT Ser_Pin, BAUDMODE, [gxHEADER,Len,gxSETPOSITION,SetPosition.LOWBYTE,SetPosition.HIGHBYTE,$FF,cs]
Done:
RETURN