' {$STAMP BS2px}
' {$PBASIC 2.5}
' {$PORT COM1}
'Hydraulic S-Curve Swing Test reducing limit shock
'   Inputs: 10K pot for actuator position - 8/12 bit?
'   Outputs: D/A for Proportional Servo-Valve - 10/12 bit


'INPUTS
ArmPosIn  PIN 7

'OUTPUTS for D/A Chip
DAdat     PIN 10 'PORTC.0   'p17
DAclk     PIN 11 'PORTC.1   'p16
DAcs      PIN 12 'PORTC.2   'p15

'OUTPUTS for PWM
OxA      PIN 8
OxB      PIN 9

'Display LED's
LGl       PIN 0
LRl       PIN 1
LYl       PIN 2
LGr       PIN 13
LRr       PIN 14
LYr       PIN 15

'CONSTANTS
BitSW     CON 10     '10 or 12   switch for bit mode
UpState   CON 0
DnState   CON 1

'Junk
USBneg  VAR Bit

'VARIABLES
ArmPos VAR Word
CurrentState    VAR Bit
Charge          VAR Word
DnLimit   VAR Word  '100
DnTrans   VAR Word   '200
UpTrans   VAR Word  '500
UpLimit   VAR Word    '600  '4096 can go further with overhang
MxLimit    VAR Word

'Initialize
Charge = 0
MxLimit = 1<<BitSW
DnLimit  = MxLimit / 10
DnTrans = MxLimit / 5
UpTrans = MxLimit / 2
UpLimit = DnLimit + UpTrans

    'Main Loop
Begin: DO
LOW LGl
LOW LRl
LOW LYl
LOW LGr
LOW LRr
LOW LYr

  HIGH ArmPosIn                        ' charge the cap
  PAUSE 1                       '   for 1 ms
  RCTIME ArmPosIn, 1, ArmPos   ' measure RC discharge time
  USBneg = ArmPos    ' report ID and position (fix later)

DEBUG " ArmPos ", DEC ArmPos, CR
DEBUG " CurrentState ", DEC CurrentState, CR

IF ArmPos > UpLimit THEN CurrentState = DnState
IF ArmPos < DnLimit THEN CurrentState = UpState

 IF CurrentState = UpState AND ArmPos < DnTrans THEN
    GOSUB AccelUp
    GOTO Begin
 ENDIF
 IF CurrentState = UpState AND ArmPos < UpTrans THEN
    GOSUB FastUp
    GOTO Begin
 ENDIF
 IF CurrentState = UpState AND ArmPos < UpLimit THEN
    GOSUB DecelUp
    GOTO Begin
 ENDIF
 IF CurrentState = DnState AND ArmPos > UpTrans THEN
    GOSUB AccelDn
    GOTO Begin
 ENDIF
 IF CurrentState = DnState AND ArmPos > DnTrans THEN
    GOSUB FastDn
    GOTO Begin
 ENDIF
 IF CurrentState = DnState AND ArmPos > DnLimit THEN
    GOSUB DecelDn
 ENDIF
LOOP

AccelUp:        '0 -> max speed
Charge = ArmPos * 5   '0 - 100
HIGH LGl
GOSUB Coil_Output
RETURN

AccelDn:        '0 -> max speed
Charge = UpLimit + 20 - ArmPos * 10   '100 - 0
HIGH LGr
GOSUB Coil_Output
RETURN

FastUp:          'max speed
Charge = MxLimit
HIGH LRl
GOSUB Coil_Output
RETURN

FastDn:          'max speed
Charge = MxLimit
HIGH LRr
GOSUB Coil_Output
RETURN

DecelUp:        'max -> 0 speed
Charge = UpLimit + 20 - ArmPos * 10     '0 - 100
HIGH LYl
GOSUB Coil_Output
RETURN

DecelDn:        'max -> 0 speed
Charge = ArmPos * 5     '100 - 0
Charge = 1 + (Charge * MxLimit) / 100   '% max
HIGH LYr
GOSUB Coil_Output
RETURN

Coil_Output:
' PWM output code - 8 bit ' DAC output code - 12 bit
Charge = ABS Charge / 4 - 1  'Stop negative values when tweaking limits
DEBUG "Charge ", DEC Charge, REP 32\5,CR
IF CurrentState = UpState THEN
'Up: Coil A = Charge, B = 0
 IF BitSW = 10 THEN
   PWM OxA,Charge,250
   LOW  OxB
 ELSE
   LOW DAcs
     SHIFTOUT DAdat,DAclk,MSBFIRST,[1\4,0\12,0\1]        ' B to B buffer
   HIGH DAcs    'Chip select change is REQUIRED between B and A commands
   LOW DAcs
   SHIFTOUT DAdat,DAclk,MSBFIRST,[8\4,Charge\12,0\1]   ' A to A output and B buffer to B    output
 ENDIF
ELSE
'Dn: Coil A = 0, B = Charge
 IF BitSW = 10 THEN
   LOW OxA
   PWM OxB,Charge,250
 ELSE
   LOW DAcs
     SHIFTOUT DAdat,DAclk,MSBFIRST,[1\4,Charge\12,0\1]   ' B to B buffer
   HIGH DAcs    'Chip select change is REQUIRED between B and A commands
   LOW DAcs
   SHIFTOUT DAdat,DAclk,MSBFIRST,[8\4,0\12,0\1]        ' A to A output and B buffer to B    output
 ENDIF
ENDIF
 HIGH DAcs
RETURN
END