'----[ArmEngine_Serial.bsp]----------------------------------------- ' {$STAMP BS2p} ' {$PBASIC 2.5} ' ' File....... ArmEngine_Serial.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_Serial.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 functionality. ' ' Simply make sure that the ctrlByte variable always has a zero in the ' 4th position. ' ' ctrlByte = %XXX0 XXXX ' OR ' wristRotateBit = 0 ' ' SG5 Users: ' There is no harm in accidentally accessing the rotating wrist. 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 '========================================================================= ' When this program runs, the arm moves to the side and picks up an ' object. I used one of my kids' wood block about 1.5" wide (40cm). The ' arm picks up the block and places it directly in front of the base. ' Then the arm picks up the same block and moves it back to the starting ' position. ' The process repeats forever. ' '========================================================================= ' EEPROM Table DATA '========================================================================= ' Below is a code sample from an EEPROM table that holds arm positions. ' The table is made up of columns and rows. The columns define object ' types like a base servo or a delay value. Rows represent an arm ' movement. An arm movement can consist of 1 to 6 joint movements and ' a delay value. Joint positions are in degrees. ' ' bse bcp elbw wrst wrstR grppr dly ctrlByte ' Stream DATA 90, 100, 100, 70, 90, 150, 20, %00111111, ' 30, 100, 100, 70, 90, 150, 20, %00000001, ' ... ' $FF 'END ' ' Table Design: '---------------- ' Always follow the example above when creating custom position tables. ' All 8 columns are required in the order shown below. ' ' Order Column Type ' 1) bse........Base Servo ' 2) bcp........Bicep Servo ' 3) elbw.......Elbow Servo ' 4) wrst.......Wrist Servo ' 5) wrstR......Wrist Rotate Servo ' 6) grppr......Gripper ' 7) dly........Delay ' 8) ctrlByte...Control Byte ' ' Always end your table with an $FF. Not doing so will cause unexpected ' results. ' ' Delay: ' Delay is the amount of time to wait before executing the next table ' row (arm position). A delay of 20 = 20*100ms or 2 seconds. Delays ' are important, they give the joints time to reach their positions. ' ' Control Byte: ' The control byte activates arm joints. The control byte consists of ' 8 bits where bit positions 0 to 6 represent an arm joint. ' For example, %00000011 activates the base and bicep servos. ' ' Control byte Joint Activated ' ------------ --------------- ' %XX000001........Base Servo ' %XX000010........Bicep Servo ' %XX000100........Elbow Servo ' %XX001000........Wrist Servo ' %XX010000........Wrist Rotate Servo ' %XX100000........Gripper Servo ' %XX111111........All Servos ' ' A table row always contains 8 columns, 6 joint positions, delay, and the ' control byte. For most applications you don't need to send commands to ' all 6 joints. You might need to move the base, bicep, and elbow first ' then move the wrist and gripper. In this example, you would need two ' table rows; one to handle the base, bicep, and elbow and one for the ' wrist and gripper. ' ' Example: ' bse bcp elbw wrst wrstR grppr dly ctrlByte ' 90, 100, 100, 70, 90, 115, 30, %00000111, ' 90, 90, 90, 60, 90, 115, 10, %00011000, ' ' Using the control byte in this fashion eliminates excessive PSC I/O. ' It is also a creates a structure or interface. If you have an external ' device controlling the arm, you simply need to send an 8 byte position ' command. ' '========================================================================= ' Sub Routines. '========================================================================= ' This program uses sub routines from ArmEngine_Basic.bsp. For detailed ' information on ArmEngine_Basic.bsp please read the comments within the ' ArmEngine_Basic.bsp file. ' '========================================================================= ' IGet_Stream '========================================================================= ' IGet_Stream grabs 8 bytes from EEPROM, places the bytes into the ' array(Base), array(Bicep), array(Elbow), array(Wrist), ' array(WristRotate), array(Gripper), delay, and ctrlByte. The idea is to ' simulate a data stream coming from an external device. It's not a good ' idea to stream data into EEPROM tables. A better method is using ' scratch pad ram. Why, did I use a table? I ran out of Scratch pad RAM ' for this demonstration. Anyway, IGet_Stream starts at the top of the ' EEPROM table and continues to the end of the table. When it sees ' the end ($FF) the process repeats. ' ' You might be thinking, what about the scratch pad RAM how does that ' work? It works very much like the table example but the values are ' held in RAM not EEPROM. Therefore, you could create a routine that ' looks to the serial port or a pin for an incoming stream of 8 bytes. ' Place the 8 bytes into scratch pad RAM, verify that you received the ' data, signal the sending device that the data was received, if not ' resend. Read the ram data into the 8 variables. Request more data. ' Using scratch pad RAM can simulate cache. Or you could simply ' grab 8 bytes of serial data and place it in the appropriate variables. ' ' The only requirement is that the incoming data is placed in the correct ' object. A good way to accomplish this is to send the data in the order ' shown below. ' Order Column Type ' 1) bse........Base Servo ' 2) bcpt.......Bicept Servo ' 3) elbw.......Elbow Servo ' 4) wrst.......Wrist Servo ' 5) wrstR......Wrist Rotate Servo ' 6) grppr......Gripper ' 7) dly........Delay ' 8) ctrlByte...Control Byte ' ' ' It's up to the developer to come up with a communication protocol. ' '------------------------------------------------------------------------- '-----[ 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 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 ptrStream VAR Word ' Stream pointer 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 delay VAR Byte ' Pause 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. ' base bicep elbow wrist wristR gripper PUT Constraints, 30,150, 60,140, 0,150, 0,170, 0,180, 90,170 'Note: the rotating wrist is not accessed ' Bse Bcpt elbw wrst wrstR grppr dly ctrlByte Stream DATA 90, 100, 100, 70, 90, 150, 20, %00111111, 30, 100, 100, 70, 90, 150, 20, %00000001, 30, 90, 90, 60, 90, 150, 15, %00101110, 30, 85, 90, 60, 90, 115, 20, %00100010, 90, 100, 100, 70, 90, 115, 30, %00001111, 90, 90, 90, 60, 90, 115, 10, %00001110, 90, 85, 90, 60, 90, 150, 10, %00100010, 90, 100, 100, 70, 90, 150, 10, %00001110, 90, 85, 90, 60, 90, 150, 10, %00001110, 90, 85, 90, 60, 90, 115, 20, %00100000, 90, 100, 100, 70, 90, 115, 10, %00001110, 30, 100, 100, 70, 90, 115, 30, %00000001, 30, 85, 90, 60, 90, 115, 10, %00001110, 30, 85, 90, 60, 90, 150, 10, %00100000, 90, 100, 100, 70, 90, 150, 20, %00001111, $FF 'END #DEFINE debugMode = 0 ramp = $E Main: GOSUB IGet_Stream ' Get data PAUSE 2000 ' Pause before repeating GOTO Main ' Loop forever IGet_Stream: ptrStream = Stream ' Assign table pointer READ ptrStream, array(Base), array(Bicep), ' Read first EEPROM row array(Elbow), array(Wrist), array(WristRotate), array(Gripper), delay, ctrlByte DO WHILE array(Base) <> $FF ' End of EEPROM table? GOSUB Move_Arm_Joints ' Move joint(s) PAUSE (delay*100) ' pause ptrStream = ptrStream + 8 ' Point to next row READ ptrStream, array(Base), array(Bicep), ' Read row array(Elbow), array(Wrist), array(4), array(Gripper), delay, ctrlByte LOOP ' Loop until end of EEPROM RETURN '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 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 'sub(pscChannel) return servoPos Convert_Degrees_To_PSCUnits: servoPos = ((array(pscChannel) * 56) / 10) + 250 RETURN 'sub(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 ?ptrStream, 'DEC ?pscChannel, '?servoPos, '?lowerConstr, '?upperConstr, '?RxChannels, '?channelValue, ?array(Base), ?array(Bicep), ?array(Elbow), ?array(Wrist), ?array(WristRotate), 'SG6 Users ?array(Gripper), BIN ?ctrlByte RETURN #ENDIF ' bse bcp elbw wrst wrstR grppr dly ctrlByte 'Stream DATA 90, 100, 100, 70, 90, 150, 20, %00111111, '30, 100, 100, 70, 90, 150, 20, %00100000, '30, 90, 90, 60, 90, 150, 15, %00011101, '30, 85, 90, 60, 90, 115, 20, %00010001, '90, 100, 100, 70, 90, 115, 30, %00111100, ' 90, 90, 90, 60, 90, 115, 10, %00011100, ' 90, 85, 90, 60, 90, 150, 10, %00010000, ' 90, 100, 100, 70, 90, 150, 10, %00011100, ' 90, 85, 90, 60, 90, 150, 10, %00011100, ' 90, 85, 90, 60, 90, 115, 20, %00000001, ' 90, 100, 100, 70, 90, 115, 10, %00011100, ' 30, 100, 100, 70, 90, 115, 30, %00100000, ' 30, 85, 90, 60, 90, 115, 10, %00011100, ' 30, 85, 90, 60, 90, 150, 10, %00000001, ' 90, 100, 100, 70, 90, 150, 20, %00111100, ' $FF 'END