'{$ '{$STAMP BS2} '*********************************************************************************************** '* Title: Teach and Play '* '* Filename: D:/ '* Date: 14 Mar 03 '* '****** I/O Assignments************************************************************************* clk con 0 ' PS2 pin connections att con 1 dat con 2 cmd con 3 scl con 4 ' EEPROM pin connections sda con 5 chm_pin con 6 ' buzzer pin RS con 0 ' LCD display PIN enable con 7 dirH = %11111111 '****** Variables ****************************************************************************** index var byte char var byte temp var byte PS2B1 var byte PS2B2 var byte base var byte shoulder var byte elbow var byte wrist var byte grip var byte mode var nib steps var word _24LC_addr var word '****** Equates ******************************************************************************** base_L var PS2B2.bit5 base_R var PS2B2.bit7 shoulder_B var PS2B2.bit6 shoulder_F var PS2B2.bit4 elbow_U var PS2B1.bit6 elbow_D var PS2B1.bit4 wrist_U var PS2B1.bit5 wrist_D var PS2B1.bit7 grip_O var PS2B2.bit3 grip_C var PS2B2.bit2 start var PS2B1.bit3 select var PS2B1.bit0 data_bus var OUTH address_write con %10100000 SSC12_speed con 48 '****** Data *********************************************************************************** select_mode_msg data "SELECT for next mode", 0 sel2exe_msg data "START to execute", 0 manual_msg data "Manual Mode", 0 record_msg data "Record Mode", 0 play_msg data "Playback Mode", 0 return_msg data "SELECT for main menu", 0 record_pos_msg data "START to record pos", 0 '****** Initialize ***************************************************************************** base = 128 ' Set all servos to center. This is a necessary shoulder = 128 ' ' step since the SSC-12 automatically elbow = 128 ' ' centers all servos on power up. wrist = 128 grip = 128 gosub out_5 gosub LCD_init mode = 0 MAIN gosub read_PS2 ' read PS2 or else auto enter to selected mode goto select_menus select_mode_state pause 500 gosub read_PS2 ' read input keys if start = 0 then select_menus if select = 1 then select_mode_state ' spin until start button active mode = mode + 1 if mode < 3 then select_menus ' 0 < mode < 3 mode = 0 select_menus gosub LCD_clear_all ' set the display for desired menu gosub LCD_line_1 index = select_mode_msg gosub LCD_char_loop gosub LCD_line_4 index = sel2exe_msg gosub LCD_char_loop if mode = 0 then display_man if mode = 1 then display_rec if mode = 2 then display_play display_man if start = 0 then man_state ' jump to desired mode if prompted by user gosub LCD_line_3 index = manual_msg gosub LCD_char_loop goto select_mode_state display_rec if start = 0 then rec_state gosub LCD_line_3 index = record_msg gosub LCD_char_loop goto select_mode_state display_play if start = 0 then play_state gosub LCD_line_3 index = play_msg gosub LCD_char_loop goto select_mode_state '******************************************************************************************** man_state gosub LCD_clear_all gosub LCD_line_1 index = manual_msg gosub LCD_char_loop gosub LCD_line_3 index = return_msg gosub LCD_char_loop man_state_loop gosub read_PS2 if select = 0 then main gosub update_pos goto man_state_loop '********** rec_state gosub LCD_clear_all gosub LCD_line_1 index = record_msg gosub LCD_char_loop gosub LCD_line_2 index = record_pos_msg gosub LCD_char_loop gosub LCD_line_4 index = return_msg gosub LCD_char_loop _24LC_addr = 10 ' prep to enter state loop steps = 0 rec_state_loop gosub read_PS2 ' read control if select = 0 then exit_rec_loop gosub update_pos if start = 1 then rec_state_loop ' record position prompt from user? steps = steps + 1 ' OK, next step gosub record_5 ' freqout chm_pin, 75, 1500 ' chime so user knows new position has been freqout chm_pin, 100, 1000 ' ' executed pause 700 ' delay for debounce goto rec_state_loop exit_rec_loop _24LC_addr = 0 ' save total number of steps recorded char = steps.lowbyte ' ' required for proper playback gosub transmit_byte _24LC_addr = 1 char = steps.highbyte gosub transmit_byte goto main record_5 ' store the position of each servo to EEPROM for index = 0 to 4 lookup index, [base, shoulder, elbow, wrist, grip], temp char = temp gosub transmit_byte ' call the store EEPROM sub _24LC_addr = _24LC_addr + 1 next return '********** play_state gosub LCD_clear_all gosub LCD_line_1 index = play_msg gosub LCD_char_loop gosub LCD_line_3 index = return_msg gosub LCD_char_loop _24LC_addr = 0 ' how many steps were resorded? gosub read_byte steps.lowbyte = char _24LC_addr = 1 gosub read_byte steps.highbyte = char _24LC_addr = 10 ' start playback at the first memory location play_state_loop gosub read_PS2 ' exit play mode if directed by user if select = 0 then select_mode_state gosub read_byte ' servo positions = position stored in EEPROM base = char _24LC_addr = _24LC_addr + 1 gosub read_byte shoulder = char _24LC_addr = _24LC_addr + 1 gosub read_byte elbow = char _24LC_addr = _24LC_addr + 1 gosub read_byte wrist = char _24LC_addr = _24LC_addr + 1 gosub read_byte grip = char _24LC_addr = _24LC_addr + 1 gosub out_5 pause 1500 ' pause between steps steps = steps - 1 if steps = 0 then main ' done with play goto play_state_loop ' playback not done out_5 ' Output the servo position, the servo speed is ' ' controlled by SSC12_speed var refer to SSC-12 ' ' user manual. Speed is a tradeoff between jerk ' ' and speed of movement. for index = 0 to 4 lookup index, [base, shoulder, elbow, wrist, grip], temp serout 15, $4054, [255, (SSC12_speed + index), temp] next return '****** Read PS2 controller ******************************************************************** read_PS2 ' A trade off was made between software and hardware. ' ' To simplify the software, the clock signal must be inverted. ' ' A transistor or other suitable inverter is required for ' ' this code. The problem arises with the SHIFTIN and SHIFTOUT ' ' commands. low att ' select the PSX controller shiftout cmd,clk, LSBFIRST,[$01,$42] ' send "get ready" and "send data" shiftin dat, clk, 3, [char] ' read then discard this byte, it ' ' it is just the PSX controller ' ' saying "I'm sending the data" shiftin dat, clk, 3, [char] ' 1st "digital" byte - left side switches PS2B1 = char shiftin dat, clk, 3, [char] ' 2nd "digital" byte - right side switches PS2B2 = char high att exit_read_PS2 return '****** position arm *************************************************************************** update_pos is_base_L if base_L = 1 then is_base_R ' ship if not base left if base = 254 then is_base_R ' skip if base at max base = base + 1 ' increment base position serout 15, 16468, [255, 128, base] ' update servo is_base_R if base_R = 1 then is_shoulder_B ' skip if not base right if base = 1 then is_shoulder_B ' skip if base at min base = base - 1 ' decrement base position serout 15, 16468, [255, 128, base] ' update servo is_shoulder_B if shoulder_B = 1 then is_shoulder_F ' skip if not shoulder back if shoulder = 200 then is_shoulder_F ' skip if shoulder at max shoulder = shoulder + 1 ' increment shoulder position serout 15, 16468, [255, 129, shoulder] ' update servo is_shoulder_F if shoulder_F = 1 then is_elbow_D ' skip if not shoulder forward if shoulder = 1 then is_elbow_D ' skip if shoulder at min shoulder = shoulder - 1 ' decrement shoulder position serout 15, 16468, [255, 129, shoulder] ' update servo is_elbow_D if elbow_D = 1 then is_elbow_U ' skip if not elbow down if elbow = 200 then is_elbow_U ' skip if elbow at max elbow = elbow + 1 ' decrement elbow position serout 15, 16468, [255, 130, elbow] ' update servo is_elbow_U if elbow_U = 1 then is_wrist_U ' skip if not elbow up if elbow = 1 then is_wrist_U ' skip if elbow at min elbow = elbow - 1 ' decrement elbow position serout 15, 16468, [255, 130, elbow] ' update servo is_wrist_U if wrist_U = 1 then is_wrist_D ' skip if not wrist up if wrist = 255 then is_wrist_D ' skip if wrist at max wrist = wrist + 1 ' increment wrist position serout 15, 16468, [255, 131, wrist] ' update servo is_wrist_D if wrist_D = 1 then is_grip_C ' skip if not wrist down if wrist = 1 then is_grip_C ' skip if wrist at min wrist = wrist - 1 ' decrement wrist position serout 15, 16468, [255, 131, wrist] ' update servo is_grip_C if grip_C = 1 then is_grip_O ' skip if grip open if grip = 254 then is_grip_O ' skip if grip at max grip = grip + 1 ' increment wrist position serout 15, 16468, [255, 132, grip] ' update servo is_grip_O if grip_O = 1 then exit_update_pos ' skip if grip closed if grip = 1 then exit_update_pos ' skip if grip at min grip = grip - 1 ' decrement grip position serout 15, 16468, [255, 132, grip] ' update servo exit_update_pos return ' exit subroutine '****** LCD Routine **************************************************************************** LCD_init: low RS low enable for index = 0 to 3 lookup index, [$38, $0C, $06, $01], temp data_bus = temp pulsout enable, 1 next return LCD_char_loop: read index, char ' read EEPROM data if char = 0 then exit_LCD ' exit if = 0 (add 0 to data string) gosub LCD_char_out ' send the char index = index + 1 ' next data goto LCD_char_loop ' again return LCD_line_1: data_bus = %10000000 ' point to line 1 pulsout enable, 1 return LCD_line_2: data_bus = %11000000 ' point to line 2 pulsout enable, 1 return LCD_line_3: data_bus = %10010100 ' point to line 3 pulsout enable, 1 return LCD_line_4: data_bus = %11010100 ' point to line 4 pulsout enable, 1 return LCD_char_out: ' send the character stored in char high RS ' ' to the LCD data bus data_bus = char pulsout enable, 1 low RS ' RS is returned low return LCD_clear_all data_bus = %00000001 pulsout enable, 1 return exit_LCD: return '****** 24LC515 interface routines ************************************************************* ' to transmit a byte to the 24LC515, ' set word length address _24LC_addr and place byte in char ' then "transmit_byte" ' to receive a byte from the 24LC515 ' set word length address _24LC_addr. Then "read_byte" ' Byte returned in char transmit_byte gosub start_condition temp = address_write ' select page (MSB address bit) if _24LC_addr.bit15 = 0 then con_transmit_byte temp.bit3 = 1 con_transmit_byte gosub tfr_byte ' send command temp = _24LC_addr.highbyte ' send high address byte gosub tfr_byte temp = _24LC_addr.lowbyte ' send low address byte gosub tfr_byte temp = char ' send data address byte gosub tfr_byte gosub stop_condition return read_byte gosub start_condition temp = address_write ' select page (MSB address bit) if _24LC_addr.bit15 = 0 then con_read_byte temp.bit3 = 1 con_read_byte gosub tfr_byte ' send command temp = _24LC_addr.highbyte ' send high address byte gosub tfr_byte temp = _24LC_addr.lowbyte ' send low address byte gosub tfr_byte gosub start_condition temp = address_write ' set read byte temp.bit0 = 1 ' select page (MSB address bit) if _24LC_addr.bit15 = 0 then con_read1_byte temp.bit3 = 1 con_read1_byte gosub tfr_byte gosub rcv_byte gosub stop_condition return start_condition low scl high sda high scl ' bus not busy, idle high low sda ' start data transfer pause 1 ' allow EEPROM setup time low scl return stop_condition ' scl is low low sda high scl high sda return tfr_byte shiftout sda, scl, MSBFIRST, [temp] goto acknowledge_byte rcv_byte shiftin sda, scl, MSBPRE, [char] goto acknowledge_byte acknowledge_byte ' ******** add a error detection routine if desired low scl input sda ' acknowledge high scl ' 9th clock pulse, slave controls sda low scl ' master controls sda low sda return '******************************************************************************************** end