In SX Microcontrollers, SX/B Compiler and SX-Key Tool, uptonryan wrote: '========================================================================== ' ' File: servoworking.sxb ' Purpose : To control the Pololu 8 servo control and corresponding servos ' Author : Ryan Upton ' Company : Upton Robotics pty ltd ' E-mail : adslpuay@tpg.com.au ' Started : 22 / 03 / 09 ' Updated : 22 / 03 / 09 ' ' ------------------------------------------------------------------------- ' Program Description ' ------------------------------------------------------------------------- ' ' This program talks to pololu 8 servo controller via pin 3 of the SX28 chip ' ------------------------------------------------------------------------- ' Device Settings ' ------------------------------------------------------------------------- 'OSCXT2 externl 4 'OSCHS2, exteernal 50 'OSC4MHZ DEVICE SX28, OSCHS1, TURBO, STACKX, OPTIONX FREQ 50_000_000 ID "Servo" ' ------------------------------------------------------------------------- ' IO Pins ' ------------------------------------------------------------------------- SOut PIN RA.3 OUTPUT ' output to pololu ' ------------------------------------------------------------------------- ' Constants ' ------------------------------------------------------------------------- Baud CON "T57600" '"T57600" Sync CON $80 'Pololu Synchronisation value PDEv CON $01 'Pololu Device Cmd0 CON $00 'Pololu Command 0 Set Range and direction Cmd1 CON $01 'Pololu Command 1 Set Speed Cmd2 CON $02 'Pololu Command 2 Set Position 1 data byte Cmd3 CON $03 'Pololu Command 3 Set Position 2 data byte Cmd4 CON $04 'Pololu Command 4 Set Position Absolute Cmd5 CON $05 'Pololu Command 5 Set Neutral Ser0 CON $00 'Servo 0 Ser1 CON $01 'Servo 1 Ser2 CON $02 'Servo 2 Ser3 CON $03 'Servo 3 Ser4 CON $04 'Servo 4 Ser5 CON $05 'Servo 5 Ser6 CON $06 'Servo 6 Ser7 CON $07 'Servo 7 ' ------------------------------------------------------------------------- ' Variables ' ------------------------------------------------------------------------- idx VAR Byte ' loop counter tmpB1 VAR Byte ' temporay byte 1 tmpB2 VAR Byte ' temporay byte 2 tmpTX VAR Byte ' transmit byte tmpW1 VAR Word ' ========================================================================= PROGRAM Start ' ========================================================================= ' ------------------------------------------------------------------------- ' Subroutine Declarations ' ------------------------------------------------------------------------- SET_SERVO_SPEED_TO_FAST SUB 0 RESET_POLOLU SUB 0 SET_SERVO_RANGE_DIRECTION SUB 0 MOVE_SERVO SUB 2 DELAY_MS SUB 1, 2 ' delay in milliseconds TX_BYTE SUB 1 ' transmit a byte ' ------------------------------------------------------------------------- ' Program Code ' ------------------------------------------------------------------------- Start: 'Reset 'RESET_POLOLU 'set servo speed to fast SET_SERVO_SPEED_TO_FAST 'set range and direction SET_SERVO_RANGE_DIRECTION DO FOR idx = 0 To 1 READ Move_Servo_Byte1 + idx , tmpB1 READ Move_Servo_Byte2 + idx , tmpB2 MOVE_SERVO tmpB1, tmpB2 NEXT LOOP ' ------------------------------------------------------------------------- ' Subroutines ' ------------------------------------------------------------------------- 'reset SUB RESET_POLOLU TX_BYTE Sync 'set pos servo 0 TX_BYTE Cmd2 'set pos servo 0 TX_BYTE $00 'set pos servo 0 ENDSUB 'set servo speed to fast SUB SET_SERVO_SPEED_TO_FAST TX_BYTE Sync 'synchronization value TX_BYTE PDEv 'pololu device TX_BYTE Cmd1 'cmd to set speed TX_BYTE Ser0 'servo 0 TX_BYTE $00 'set pos servo 0 DELAY_MS 2000 ENDSUB 'set range and direction SUB SET_SERVO_RANGE_DIRECTION TX_BYTE Sync 'synchronization value TX_BYTE PDEv 'pololu device TX_BYTE Cmd0 'cmd 0 set range and direction TX_BYTE Ser0 'servo 0 TX_BYTE %01011110 'set range 30 DELAY_MS 2000 ENDSUB 'move servo SUB MOVE_SERVO TX_BYTE Sync 'synchronization value TX_BYTE PDEv 'pololu device TX_BYTE Cmd3 'cmd 3 set Position 2 Data bytes TX_BYTE Ser0 'servo 0 TX_BYTE tmpB1 'data1 TX_BYTE tmpB2 'data2 DELAY_MS 2000 ENDSUB 'Delay SUB DELAY_MS IF __PARAMCNT = 1 THEN tmpW1 = __PARAM1 ' save byte value ELSE tmpW1 = __WPARAM12 ' save word value ENDIF PAUSE tmpW1 ENDSUB ' Transmit a byte SUB TX_BYTE tmpTX = __PARAM1 SEROUT SOut, Baud, tmpTX ENDSUB 'pg101 'set neutral 'serout SOut, Baud,Sync 'synchronization value 'serout SOut, Baud,PDEv 'pololu device 'serout SOut, Baud,Cmd5 'cmd 'serout SOut, Baud,Ser0 'servo 'serout SOut, Baud,$1E 'data1 neutral 3000 'serout SOut, Baud,$00 'data2 neutral 'PAUSE 2000 'set absolute position 'serout SOut, Baud,Sync 'synchronization value 'serout SOut, Baud,PDEv 'pololu device 'serout SOut, Baud,Cmd4 'cmd 4 set postion absolute 'serout SOut, Baud,Ser0 'servo 0 'serout SOut, Baud,$00 'data1 'serout SOut, Baud,$00 'data2 'PAUSE 2000 'mini SSC II Mode 'serout SOut, Baud,$FF 'set pos servo 0 'serout SOut, Baud,$00 'set pos servo 0 'serout SOut, Baud,$FE 'set pos servo 0 '------------------------------------------------------------------------------ ' User Data '------------------------------------------------------------------------------ '360 degrees 'Move_Servo_Byte1: 'DATA $00 '0 'DATA $7F '2 'Move_Servo_Byte2: 'DATA $01 '1 'DATA $70 '3 '180 degrees Move_Servo_Byte1: DATA $00 '0 DATA $00 '2 Move_Servo_Byte2: DATA $01 '1 DATA $70 '3 ---------- End of Message ---------- You can view the post on-line at: http://forums.parallax.com/forums/default.aspx?f=7&p=1&m=342774#m343017 Need assistance? Send an email to the Forum Administrator at forumadmin@parallax.com The Parallax Forums are powered by dotNetBB Forums, copyright 2002-2009 (http://www.dotNetBB.com)