' {$STAMP BS2px} ' {$PBASIC 2.5} ' {$PORT COM1} 'Hydraulic S-Curve - reduce limit shock ' Inputs: 10K pot for actuator position - 10 bit ' Outputs: D/A for Proportional Servo-Valve - 12 bit 'INPUTS ArmPosIn PIN 7 'OUTPUTS for D/A Chip DAdat PIN 10 DAclk PIN 11 DAcs PIN 12 'DISPLAY LED's LGl PIN 0 LRl PIN 1 LYl PIN 2 LGr PIN 13 LRr PIN 14 LYr PIN 15 'CONSTANTS UpState CON 0 DnState CON 1 'VARIABLES ArmPos VAR Word CurrentState VAR Bit Charge VAR Word ChgLim VAR Word DnLimit VAR Word DnTrans VAR Word UpTrans VAR Word UpLimit VAR Word 'Initialize Charge = 0 DnLimit = 50 DnTrans = 200 UpTrans = 800 UpLimit = 950 '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 DEBUG " ArmPos ", DEC ArmPos, CR ChgLim = 1<<12 - 1 DEBUG " ChgLim ", DEC ChgLim, CR IF ArmPos >= UpLimit THEN CurrentState = DnState ChgLim = 0 ENDIF IF ArmPos <= DnLimit THEN CurrentState = UpState ChgLim = 0 ENDIF DEBUG " CurrentState ", DEC CurrentState, CR 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: '100 - 200 -> 0 - ChgLim Charge = (ArmPos - 100) * (ChgLim / 100) HIGH LGl GOSUB Coil_Output RETURN AccelDn: '900 - 800 -> 0 - ChgLim Charge = ABS (900 - ArmPos) * (ChgLim / 100) HIGH LGr GOSUB Coil_Output RETURN FastUp: 'max speed Charge = ChgLim HIGH LRl GOSUB Coil_Output RETURN FastDn: 'max speed Charge = ChgLim HIGH LRr GOSUB Coil_Output RETURN DecelUp: '800 - 900 -> ChgLim - 0 Charge = (ArmPos - 800) * (ChgLim / 100) HIGH LYl GOSUB Coil_Output RETURN DecelDn: '200 - 100 -> ChgLim - 0 Charge = ABS (100 - ArmPos) * (ChgLim / 100) HIGH LYr GOSUB Coil_Output RETURN Coil_Output: Charge = ABS Charge MAX ChgLim 'Stop negatives when tweaking limits DEBUG "Charge ", DEC Charge, REP 32\5,CR IF CurrentState = UpState THEN 'Up: Coil A = Charge, B = 0 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 ELSE 'Dn: Coil A = 0, B = Charge 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 HIGH DAcs RETURN END