'----[ArmEngine_Basic.bsp]----------------------------------------- ' {$STAMP BS2p} ' {$PBASIC 2.5} ' ' File....... ArmEngine_Basic.bsp ' Purpose.... Basic arm movement ' Author..... CrustCrawler Inc. (Mike Gebhard) ' E-mail..... support@crustcrawler.com ' Created.... 5/26/2005 ' Updated.... 11/14/2005 ' ' Hardware: ' (1) SG5/6-UT Robotic Arm ' (1) Parallax Basic Stamp P Module ' (1) Parallas BOE ' (1) Parallax Servo Controller (PSC) ' ' '========================================================================= ' 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 have a ' single large bicep servo. If you have a SG version 2 the line below ' should read #DEFINE sgVersion = 2, save the file. If you have a ' gray arm or an arm with 2 servo bicep servos set #DEFINE sgVersion = 1. ' #DEFINE sgVersion = 2 ' '========================================================================= ' Updates 9/24/2005 Includes support for SG6 (6 axis arm) '========================================================================= ' ArmEngine_Basic.bsp will function for both the SG6 and SG5, 6 and 5 ' axis robotic arms. If you purchase a 5 axis you should consider turning ' off the rotating wrist by editing one line of code. Find the code ' section below. ' ' Main: ' ... ' baseBit = 1 ' biceptBit = 1 ' elbowBit = 1 ' wristBit = 1 ' wristRotateBit = 1 'SG5 users change to 0 ' gripperBit = 1 ' ' Change wristRotateBit = 1 assignment to ' wristRotateBit = 0 ' ' SG5 Users: ' There is no harm in leaving the statement as is. Doing so causes ' unnecessary lines of code to run. The result might look like a ' slight delay in program execution while the PSC sends data to a ' nonexistent joint. ' '========================================================================= ' Getting Started '========================================================================= ' 1. Verify your PSC channel connections. ' 2. Verify that your arm is adjusted physically and ' programmatically. ' '======== SG Version 1 Only ================================================ ' 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. ' '========================================================================= ' Arm servo joints to PSC channels connections '========================================================================= ' PLEASE verify the PSC connection below. They might be different from your ' assembly guide instructions. ' Base...........0 ' Bicep..........1 ' Elbow..........2 ' Wrist..........3 ' WristRotate....4 --> (SG6 only) ' Gripper........5 ' RightBicep.....6 --> SG version 1 only (Gray Arm) ' '========================================================================= ' Program Overview '========================================================================= ' This is the base code used to move the SG arm joints. It takes a ' 7 element array containing servo positions (in degrees) and one control ' byte (ctrlByte) as parameters. The array provides instructions to ' position the arm joints. The ctrlByte controls which joints move. ' ' All joints move within adjustable contraints. Constraining the joints ' stops the arm from moving into an unrecoverable position, allows the ' user to control where the arm does its work, and/or stops a joint from ' moving to a position that could potentially harm a servo or servo ' assembly. ' ' This code is designed to give developers a starting point. ' Sub routine details below ' '========================================================================= ' Walk-Through: '========================================================================= ' Move_Arm_Joints checks the baseBit, biceptBit, elbowBit, wristBit, ' and gripperBit variables respectively to determine which joint to move. ' A 1 means move the related joint to the position in array(x) ' 0 means skip this joint. ' ' For example, lets say you want to move the base to 30 degress ' You, the programmer, would assign baseBit = 1 and array(Base) = 30. ' The sub routine sees the baseBit equals 1 and knows you want to ' rotate the base. ' ' Next, joint constraints are checked by the Check_Joint_Constraint ' helper sub. If the value in array(pscChannel), array(0) in this example, ' falls within the base servo contraints, nothing happens (more later). ' ' Since array(pscChannel) is in degrees and the PSC does not understand ' degrees the array(pscChannel) value is converted to PSC units by the ' Convert_Degrees_To_PSCUnits helper sub. ' ' All values are sent to Write_PSC which is responsible for ' sending the information to the PSC and moving the base servo. ' ' Move_Arm_Joints continues by checking each of the remaining "jointBits" ' ' Order: ' 1. baseBit ' 2. biceptBit ' 3. elbowBit ' 4. wristBit ' 5. wristRotateBit ' 6. gripperBit ' '========================================================================= ' Sub Routines. '========================================================================= ' Each sub routine can take input parameters and return or update ' variables. ' ' Example: ' sub(input parameter) return variable. ' ' The next several sections describe parameters, return values, and ' general functionality. ' '========================================================================= ' Move_Arm_Joints ' sub(array(6), ctrlByte ) '========================================================================= ' Move_Arm_Joints is the main sub routine for controlling arm movement. ' It takes the following required parameters: ' ' - ctrlByte This byte controls which joint to move. It is made up of the ' alieses below. ' baseBit VAR ctrlByte.BIT0 ' 0 = no change ' biceptBit VAR ctrlByte.BIT1 ' 1 = joint value changed ' elbowBit VAR ctrlByte.BIT2 ' wristBit VAR ctrlByte.BIT3 ' wristRotateBit VAR ctrlByte.BIT4 --> SG6 only ' gripperBit VAR ctrlByte.BIT5 ' ' - array(6) contains degree positions for all (5 or 6) arm joints values. ' These values range from 0 to 180 degrees or maximun servo ' rotation. It is up to the programmer to verify that thees ' values do not exceed 180 else you will get unexpected ' results. ' ' array(Base) ' array(Bicep) ' array(Elbow) ' array(Wrist) ' array(WristRotate) --> SG6 only ' array(Gripper) ' '========================================================================= ' Check_Joint_Constraint '========================================================================= ' sub(pscChannel) return array(pscChannel) ' ' - pscChannel This variable holds the PSC channel related to the current ' joint to move and the array element that holds the joint ' position. ' ' Check_Joint_Constraint is more like a property of the arm. By suppling ' contraints to the arm you define where the arm does its work or lack of. ' Constraint information is stored in Scratch Pad RAM (PUT Command). Each ' arm joint has a related lower and upper constraint. These values can ' easily be changed programmatically during program execution so the work ' area can change dynamically during run time. ' ' Let's look at the base servo constraints. First find the line of code ' below. The first two numbers 30 and 150 are the base servo contraints. ' ' base bicept elbow wrist wristR gripper ' PUT Constraints, 30,150, 60,140, 0,150, 0,170, 0,180, 90,170 ' ' When the Check_Joint_Constraint is called it graps the lower and ' upper base contraints (Constraints + (pscChannel*2)) and places them ' in lowerConstr and upperConstr variables. ' GET (Constraints + (pscChannel*2)), lowerConstr, upperConstr ' ' The value in array(pscChannel) is checked against ' the contraints. If array(pscChannel) is less than the lower constraint ' then array(pscChannel) = lowerContr. If array(pscChannel) is greater ' than upperConstr then array(pscChannel) = upperConstr. If neither are ' true then array(pscChannel) is not modified at this time. ' '========================================================================= ' Convert_Degrees_To_PSCUnits '========================================================================= ' sub(pscChannel) return servoPos ' ' - servoPos This variable holds the servo postion in PSC terms ' ' This function converts degrees to PSC units ' servoPos = ((angle * 56) / 10) + 250 ' '========================================================================= ' Convert_PSCUnits_To_Degrees '========================================================================= ' sub(servoPos) return array(pscChannel) ' ' This function converts PSC units to degrees. This function is not used. ' It is included for use in later applications. ' array(pscChannel) = ((servoPos - 250) * 10) / 56 ' '========================================================================= ' Write_PSC '========================================================================= ' sub(PSCChannel , ramp , servoPos ) ' ' Write_PSC is responsible for sending joint information to the PSC. The ' result is a joint moves. ' '========================================================================= ' Display '========================================================================= ' sub() returns ' ' This sub displays all the variables and values used in this code when ' ' #DEFINE debugMode = 1 ' ' This is a handy troubleshooting technique if you decide to change the ' base code and need to know what the variables are doing. ' ' For more information look up "compiler directives" in the PBASIC help. ' '========================================================================= ' Main '========================================================================= ' This sub is designed to give a programmer hands on experience with ' the variables and constants used in this program. Like the move base ' example above, if you want to move the base and only the base servo ' to 30 degrees do the following. ' ' Main: ' array(Base) = 30 ' array(Bicep) = 90 ' array(Elbow) = 90 ' array(Wrist) = 90 ' array(WristRotate) = 90 --> SG6 only ' array(Gripper) = 120 ' ctrlByte = %00000000 ' Reset control byte ' baseBit = 1 ' biceptBit = 0 ' elbowBit = 0 ' wristBit = 0 ' wristRotateBit = 0 ' gripperBit = 0 ' ' Note: Only the baseBit = 1. Therefore only the base servo rotates. ' ' Run the program. '------------------------------------------------------------------------- '-----[ 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 degrees CON 2 ' Amount to move a joint Base CON 0 ' Rotating base Bicep CON 1 ' Left bicept Elbow CON 2 ' Elbow Wrist CON 3 ' Wrist WristRotate CON 4 ' Rotating Wrist Gripper CON 5 ' Gripper RightBicep CON 6 ' Right bicept '================================================================= ' 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 you have an SG version two (a single large servo in the bicep ' position) the go t the top of this file and set ' #DEFINE sgVersion = 2 ' #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 ] ------------------------------------------------------ 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 pscChannel VAR Nib ' PSC channel RxChannels VAR Nib ' Receiver channel RC VAR Bit ' Rx On/Off 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 wristRotateBit VAR ctrlByte.BIT4 gripperBit VAR ctrlByte.BIT5 '------------------------------------------------------------------------- ' Joint constraints in angles ' The values below limit where the arm joints can be positioned. ' The constraints stop the arm from getting itself into ' a unrecoverable position or a position that could damage ' a servo. For example the base servo can move from 30 ' to 150 degrees no more and no less. ' ' base bicep elbow wrist wristR gripper PUT Constraints, 30,150, 60,140, 0,150, 0,170, 0,180, 90,170 #DEFINE debugMode = 0 ramp = $B Main: array(Base) = 90 array(Bicep) = 100 array(Elbow) = 90 array(Wrist) = 90 array(WristRotate) = 90 array(Gripper) = 120 ctrlByte = %00000000 ' Reset control byte baseBit = 1 bicepBit = 1 elbowBit = 1 wristBit = 1 wristRotateBit = 1 'SG5 users change to 0 gripperBit = 1 GOTO Move_Arm_Joints END '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 bicept if biceptBit = 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 wristRotate if wristRotateBit = 1 IF wristRotateBit THEN pscChannel = WristRotate 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 END '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 ' 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(WristRotate), '--> SG6 ?array(Gripper), BIN ?ctrlByte RETURN #ENDIF