'----[RC_SG5ArmV1_0.bsp]------------------------------------------------ ' {$STAMP BS2p} ' {$PBASIC 2.5} ' ' File....... RC_SG5ArmV1_0.bsp ' Purpose.... Radio control SG5-UT arm ' Author..... CrustCrawler Inc. (Mike Gebhard) ' E-mail..... support@crustcrawler.com ' Started.... 5/27/2005 ' Updated.... 11/14/2005 ' ' Hardware: ' (1) SG5-UT Robotic Arm ' (1) Parallax Basic Stamp P Module ' (1) Parallax BOE ' (1) Parallax Servo Controller (PSC) ' (1) Tower Hobbies System 3000 6 channel FM Radio Control System ' (1) Tower Hobbies System 3000 7 channel receiver ' ' '========================================================================= ' Updates 11/14/2005 Includes support for SG5&6 Version 2 Robotic arms '========================================================================= ' Code updated for SG5 and SG6 version 2.0. SG version 2 arms are blue ' arm. If you have a SG version 2 change the line below to read ' #DEFINE sgVersion = 2 and save the file. If you have a gray arm or ' version 1 do nothing. ' #DEFINE sgVersion = 1 ' '========================================================================= ' Getting Started '========================================================================= ' This code uses PSC connections that are different from the assembly ' guide instructions. Use tables 1 & 2 below to verify your connections ' before running this code. Failure to do so could damage a servo. ' ' 1. Verify your PSC channel connections. ' 2. Verify that your arm is adjusted correctly physically and ' programmatically. ' 3. Enter the "RightBicepOffset Constant Declaration" you found ' running the Adjust_Bicept.bsp code. This is part of the assembly ' instructions as of 5/26/2005. This code will not run until you ' complete this step. If you already assembled the arm, this value ' is the difference between the bicep servos found in Chapter #5: ' Adjusting the Biceps Servos. ' '========================================================================= ' TABLE 1 ' Arm servo joints TO PSC channels connections '========================================================================= ' Base...........PSC Channel 0 ' Bicep..........PSC Channel 1 ' Elbow..........PSC Channel 2 ' Wrist..........PSC Channel 3 ' Empty..........PSC Channel 4 --> Place holder ' Gripper........PSC Channel 5 ' RightBicep.....PSC Channel 6 ' '========================================================================= ' TABLE 2 ' BS2p Pin to Tower Hobbies receiver connections '========================================================================= ' Pin 10.........ch6 (flaps) ' Pin 11.........ch2 (elevator) ' Pin 12.........ch3 (throttle) ' Pin 13.........ch4 (rudder) ' Pin 14.........ch1 (aileron) ' '========================================================================= ' Program Overview '========================================================================= ' RC_SG5ArmV1_0.bsp is a combination of two files ArmEngine_Basic.bsp and ' Five_Channel_RC.bsp. Both these files contain detailed programming ' information. If you want to know how the code works read the code ' comments in the two files. ' ' Five_Channel_RC.bsp contains the RC logic. This file can help ' troubleshoot RC interface issues. ' ' ArmEngine_Basic.bsp contols arm movement and can be used as a platform ' to help developers create SG5-UT programs. ' ' ArmEngine_Serial.bsp is another useful file. This file uses ' ArmEngine_Basic.bsp and an EEPROM table to control the SG5-UT. I'm ' mentioning this file because it can provide another perspective on ' controlling the SG5-UT. ' '========================================================================= ' TABLE 3 ' Transmitter Controls '========================================================================= ' Receiver | Joint | STAMP | PSC '----------------------------------------------------------- ' Throttle | Bicep and Elbow | Pin 12 | Channel 1 ,2 & 6 ' Rudder | Gripper | Pin 13 | Channel 5 ' Elevator | Wrist | Pin 11 | Channel 3 ' Aileron | Base | Pin 14 | Channel 0 ' Flaps | Elbow | Pin 10 | Channel 2 '----------------------------------------------------------- ' ' Take a few minutes to adjust yourself to the transmitter controls. ' Once you get the hang of it, you should find that the arm is very ' responsive. ' '========================================================================= ' Main Sub Routine '========================================================================= ' The main sub routine checks to see if the receiver is sending a signal ' to the STAMP (PULSIN). If no signal is detected Main loops forever. ' Get_Receiver_Channel_Values and Move_Arm_Joints sub routines are called ' if a signal is detected. ' '========================================================================= ' Adjusting the SG5-UT's responsiveness (speed and accuracy). '========================================================================= ' Find the code section below. '--------------------------- ' Initialize: ' ramp = $10 ' degrees = 5 '--------------------------- ' ' Ramp controls the servo rotation speed. The lower the ramp value ' the faster the servos will spin. The value $10 (base 16) is the ' hexadecimal representation of 16 (base 10). See your PSC manual ' for more information ' ' Degrees is the amount to move a joint or its accuracy. By default the ' arm joints move in 5 degree units. Degrees can not be a fraction ' or decimal number use only whole numbers. Therefore, with this ' program the SG5-UT is accurate to 1 degree. However, the PSC is ' accurate to 0.18 degrees. ' '========================================================================= ' Troubleshooting '========================================================================= ' Find the code section below. '--------------------------- ' #DEFINE debugMode = 0 '--------------------------- ' ' Set debugMode = 1 to display all program variable on the debug screen ' for troubleshooting. Default is debugMode = 0 ' ' BE CAREFULL - with debug mode on the program will pause 1/2 sec each ' time a joint moves. This is by design so that you can easily see the ' debug screen values before they update. Bicept servo always move ' together regardless of the debug mode. This protects the biceps ' servos from damage. If you forget to turn debug mode off, the SG5-UT ' will pause 1/2 sec after each joint moves. ' ' Nothing happens when transmitter controls are moved. ' Turn Debug mode on to verify that the STAMP is receiving a signal. ' The debug screen will indicate if a signal is detected by displaying ' "Receiver is off" or "Receiver is on". ' ' Unexpected arm movement or no arm movement. ' Check PSC connection against TABLE 1 above. ' ' If you can't get your arm working please post a detailed message ' on CrustCrawler's robotic forums at ' http://forums.crustcrawler.com/ ' '------------------------------------------------------------------------- '-----[ I/O ]------------------------------------------------------------- #IF ($stamp = BS2SX) OR ($stamp = BS2P) #THEN Baud CON 1021 ' BS2p 2400 baud #ELSE Baud CON 33164 ' BS2 2400 baud #ENDIF PSC CON 15 ' PSC Module Base CON 0 ' Rotating base Bicep CON 1 ' Left Bicep Elbow CON 2 ' Elbow Wrist CON 3 ' Wrist Gripper CON 5 ' Gripper RightBicep CON 6 ' Right Bicep '================================================================= ' Constant declaration statement found from ' running Adjust_Bicept.bsp during assembly. ' This code will not download until you declare ' the RightBicepOffset constant. ' Chapter #5: Adjusting the Biceps Servos. #IF sgVersion = 1 #THEN RightBicepOffset CON '-10 ' Right Bicep offset #ENDIF '================================================================= Constraints CON 0 ' Start of constraints RC_On CON 1 ' Receiver ON RC_Off CON 0 ' Receiver OFF '---- [ Variables ] ------------------------------------------------------l channelValue VAR Word ' Rx channel value servoPos VAR Word ' PSC servo position array VAR Byte(6) ' Servo positions (degrees) lowerConstr VAR Byte ' lower angle constraint upperConstr VAR Byte ' upper angle constraint ramp VAR Byte ' servo rotation speed ctrlByte VAR Byte ' Servo joint(s) updated baseBit VAR ctrlByte.BIT0 ' 0 = no change BicepBit VAR ctrlByte.BIT1 ' 1 = joint value changed elbowBit VAR ctrlByte.BIT2 wristBit VAR ctrlByte.BIT3 gripperBit VAR ctrlByte.BIT5 pscChannel VAR Nib ' PSC channel RxChannels VAR Nib ' Receiver channel degrees VAR Nib RC VAR Bit ' Rx On/Off '------------------------------------------------------------------------- ' base Bicep elbow wrist N/A gripper PUT Constraints, 30,150, 60,140, 0,150, 0,170, 0,0, 90,170 #DEFINE debugMode = 0 Initialize: ramp = $10 degrees = 5 array(Base) = 90 ' Initialize joints array(Bicep) = 90 array(Elbow) = 90 array(Wrist) = 90 array(4) = 0 array(Gripper) = 120 ctrlByte = %00101111 ' Move all joints GOSUB Move_Arm_Joints ' Arm starting position Main: ctrlByte = %00000000 ' Reset control byte GOSUB Check_Receiver_for_Signal ' Check for receiver signal IF RC THEN ' If there is a signal GOSUB Get_Receiver_Channel_Values ' Read pulse widths GOSUB Move_Arm_Joints ' Move the SG5-UT #IF debugMode #THEN ELSE DEBUG CLS GOSUB Display #ENDIF ENDIF GOTO Main ' Loop forever '----- [ Arm Code ] ------------------------------------------------------ 'sub(array(6) , ctrlByte ) Move_Arm_Joints: 'Move base if baseBit = 1 (update position) 'Skip if base bit = 0 (no change) IF baseBit THEN pscChannel = Base GOSUB Check_Joint_Constraint GOSUB Convert_Degrees_To_PSCUnits GOSUB Write_PSC #IF debugMode #THEN GOSUB Display #ENDIF ENDIF 'Move Bicep if BicepBit = 1 IF BicepBit THEN pscChannel = Bicep GOSUB Check_Joint_Constraint GOSUB Convert_Degrees_To_PSCUnits GOSUB Write_PSC #IF sgVersion = 1 #THEN pscChannel = RightBicep servoPos = servoPos + RightBicepOffset GOSUB Write_PSC #ENDIF #IF debugMode #THEN GOSUB Display #ENDIF ENDIF 'Move elbow if elbowBit = 1 IF elbowBit THEN pscChannel = Elbow GOSUB Check_Joint_Constraint GOSUB Convert_Degrees_To_PSCUnits GOSUB Write_PSC #IF debugMode #THEN GOSUB Display #ENDIF ENDIF 'Move wrist if wristBit = 1 IF wristBit THEN pscChannel = Wrist GOSUB Check_Joint_Constraint GOSUB Convert_Degrees_To_PSCUnits GOSUB Write_PSC #IF debugMode #THEN GOSUB Display #ENDIF ENDIF 'Move gripper if gripperBit = 1 IF gripperBit THEN pscChannel = Gripper GOSUB Check_Joint_Constraint GOSUB Convert_Degrees_To_PSCUnits GOSUB Write_PSC #IF debugMode #THEN GOSUB Display #ENDIF ENDIF RETURN 'function(pscChannel) return array(pscChannel) Check_Joint_Constraint: GET (Constraints + (pscChannel*2)), lowerConstr, upperConstr IF (array(pscChannel) <= lowerConstr) THEN array(pscChannel) = lowerConstr ENDIF IF (array(pscChannel) >= upperConstr) THEN array(pscChannel) = upperConstr ENDIF RETURN 'function(pscChannel) return servoPos Convert_Degrees_To_PSCUnits: servoPos = ((array(pscChannel) * 56) / 10) + 250 RETURN 'function(servoPos) return array(pscChannel) Convert_PSCUnits_To_Degrees: array(pscChannel) = ((servoPos - 250) * 10) / 56 RETURN 'sub(PSCChannel , ramp , servoPos ) Write_PSC: SEROUT PSC,Baud,["!SC",pscChannel, ramp, servoPos.LOWBYTE, servoPos.HIGHBYTE, CR] RETURN '----- [ RC Code ] ------------------------------------------------------- 'function() return ctrlByte Get_Receiver_Channel_Values: ' Loop through pins 10 to 14 (RxChannels) FOR RxChannels = 10 TO 14 ' Read Rx value PULSIN RxChannels, 1, channelValue ' Do nothing if stick positions are centered or near center IF (((channelValue > 2200) AND (channelValue < 2800)) OR ((channelValue > 400) AND (channelValue < 1800))) THEN SELECT RxChannels CASE 10 pscChannel = Elbow ' Array element to change elbowBit = 1 ' Joint affected CASE 11 pscChannel = Wrist wristBit = 1 CASE 12 pscChannel = Bicep BicepBit = 1 elbowBit = 1 CASE 13 pscChannel = Gripper gripperBit = 1 CASE 14 pscChannel = Base baseBit = 1 ENDSELECT GOSUB Incr_Decr_Servo_Position_Array ENDIF #IF debugMode #THEN DEBUG CLS GOSUB Display #ENDIF NEXT GOSUB Servo_Range_Constraint RETURN '------------------------------------------------------------------------- 'sub(pscChannel channelValue) return array(pscChannel) Incr_Decr_Servo_Position_Array: SELECT channelValue CASE > 2200 array(pscChannel) = array(pscChannel) - degrees CASE < 1800 array(pscChannel) = array(pscChannel) + degrees ENDSELECT '------------------------------------------------------------------------- ' This section updates elbow position if Bicep was just updated ' Elbow always moves in the opposite direction as the Bicep. ' The statements below can be removed if you do not want both ' the Bicep and elbow to move at the same time. The elbow is ' also controlled by the trim channel IF pscChannel = Bicep THEN pscChannel = Elbow SELECT channelValue CASE > 2200 array(pscChannel) = array(pscChannel) + degrees CASE < 1800 array(pscChannel) = array(pscChannel) - degrees ENDSELECT ENDIF '------------------------------------------------------------------------- RETURN 'function() return RC Check_Receiver_for_Signal: PULSIN 10, 1, channelValue IF channelValue = 0 THEN RC = RC_Off ELSE RC = RC_On ENDIF RETURN 'sub() array(pscChannel) Servo_Range_Constraint: ' This section enforces that the ' array(x) position values are between ' 0 and 180. FOR pscChannel = 0 TO 5 IF (array(pscChannel) > (180 + degrees)) THEN array(pscChannel) = 0 ENDIF IF (array(pscChannel) = (180 + degrees)) THEN array(pscChannel) = 180 ENDIF NEXT RETURN ' Display all variables #IF debugMode #THEN Display: DEBUG "-----------------------",CR IF RC THEN DEBUG "Receiver is ON!",CR ELSE DEBUG "Receiver is OFF!",CR ENDIF DEBUG DEC ?pscChannel, ?servoPos, ?lowerConstr, ?upperConstr, ?RxChannels, ?channelValue, ?array(Base), ?array(Bicep), ?array(Elbow), ?array(Wrist), '?array(4), Empty for 6 axis arm ?array(Gripper), BIN ?ctrlByte PAUSE 500 RETURN #ENDIF