'----[VR_ARM.bsp]----------------------------------------- ' {$STAMP BS2p} ' {$PBASIC 2.5} ' ' File....... VR_ARM.bsp ' Purpose.... Voice Recognition control of CrustCrawler SG6-UT ' Author..... Bryan Bergeron ' ' Hardware: ' SG6-UT Robotic Arm ' Parallax Basic Stamp P Module ' Parallax Servo Controller ' SR-07 Speech Recognition Board ' ' 6 DOF Naming Convention ' Base->Shoulder->Elbow->Wrist->Wrist Rotate->Grip ' SR-07 Vocabulary Mapping ' 01 - Base Clockwise ' 02 - Base Counterclockwise ' 03 - Shoulder Flex ' 04 - Shoulder Extend ' 05 - Elbow Flex ' 06 - Elbow Extend ' 07 - Wrist Flex ' 08 - Wrist Extend ' 09 - Wrist Clockwise ' 10 - Wrist Counterclockwise ' 11 - Grip Close ' 12 - Grip Open ' 13 - Stop ' 14 - Park Arm ' ' Error Codes: 55, 66, 77 ' '-----[ Constants ]----------------------------------------------------- Baud CON 1021 ' BS2p 2400 baud PSC CON 15 ' PSC Module Base CON 0 ' Rotating base Shoulder CON 1 ' First Joint Elbow CON 2 ' Second Joint Wrist CON 3 ' Wrist WristRotate CON 4 ' Rotating Wrist Gripper CON 5 ' Gripper BaseC CON 160 ' Base Clockwise BaseCC CON 20 ' Base CounterClockwise ShoulderF CON 60 ' Shoulder Flex ShoulderE CON 140 ' Shoulder Extend ElbowF CON 0 ' Elbow Flex ElbowE CON 150 ' Elbow Extend WristF CON 0 ' Wrist Flex WristE CON 130 ' Wrist Extend WristRC CON 20 ' Wrist Rotate Clockwise WristRCC CON 160 ' Wrist Rotate CounterClockwise GripC CON 80 ' Griper Closed GripO CON 170 ' Gripper Open BaseP CON 90 ' Base Park ShoulderP CON 120 ' Shoulder Park ElbowP CON 60 ' Elbow Park WristP CON 0 ' Wrist Park WristRP CON 95 ' Wrist Rotate Park GripP CON 120 ' Gripper Park ramp = $15 ' Nice and slow INPUT 0 ' Make P0 - P7 Inputs INPUT 1 INPUT 2 INPUT 3 INPUT 4 INPUT 5 INPUT 6 INPUT 7 '---- [ Variables ] ------------------------------------------------------ SRINPUT VAR Byte ' SR-07 input servoPos VAR Word ' PSC servo position target VAR Byte ' Target servo angle ramp VAR Byte ' Servo rotation speed pscChannel VAR Nib ' PSC channel changeByte VAR Byte ' Servo joint change Buff VAR Byte(3) '------------------------------------------------------------------------- Init: ' for servo position read GOSUB ParkArm ' initialized position Main: GOSUB GetVRData IF SRINPUT.HIGHNIB > 4 THEN Main ' 55,66,77 error messages IF SRINPUT <> changeByte THEN GOSUB StopAll changeByte = SRINPUT GOSUB Move_Joints ENDIF GOTO Main END GetVRData: SRINPUT.LOWNIB.BIT0 = IN0 SRINPUT.LOWNIB.BIT1 = IN1 SRINPUT.LOWNIB.BIT2 = IN2 SRINPUT.LOWNIB.BIT3 = IN3 SRINPUT.HIGHNIB.BIT0 = IN4 SRINPUT.HIGHNIB.BIT1 = IN5 SRINPUT.HIGHNIB.BIT2 = IN6 SRINPUT.HIGHNIB.BIT3 = IN7 RETURN Move_Joints: 'Base clockwise IF SRINPUT = $1 THEN pscChannel = Base: target=BaseC GOSUB Convert_Degrees_To_PSCUnits GOSUB Write_PSC ENDIF 'Base counterclockwise IF SRINPUT = $2 THEN pscChannel = Base: target=BaseCC GOSUB Convert_Degrees_To_PSCUnits GOSUB Write_PSC ENDIF 'Shoulder Flex IF SRINPUT = $3 THEN pscChannel = Shoulder: target=ShoulderF GOSUB Convert_Degrees_To_PSCUnits GOSUB Write_PSC ENDIF 'Shoulder Extend IF SRINPUT = $4 THEN pscChannel = Shoulder: target=ShoulderE GOSUB Convert_Degrees_To_PSCUnits GOSUB Write_PSC ENDIF 'Elbow Flex IF SRINPUT = $5 THEN pscChannel = Elbow: target=ElbowF GOSUB Convert_Degrees_To_PSCUnits GOSUB Write_PSC ENDIF 'Elbow Extend IF SRINPUT = $6 THEN pscChannel = Elbow: target=ElbowE GOSUB Convert_Degrees_To_PSCUnits GOSUB Write_PSC ENDIF 'Wrist Flex IF SRINPUT = $7 THEN pscChannel = Wrist: target=WristF GOSUB Convert_Degrees_To_PSCUnits GOSUB Write_PSC ENDIF 'Wrist Extend IF SRINPUT = $8 THEN pscChannel = Wrist: target=WristE GOSUB Convert_Degrees_To_PSCUnits GOSUB Write_PSC ENDIF 'Wrist Clockwise IF SRINPUT = $9 THEN pscChannel = WristRotate: target=WristRC GOSUB Convert_Degrees_To_PSCUnits GOSUB Write_PSC ENDIF 'Wrist counterClockwise IF SRINPUT = $10 THEN pscChannel = WristRotate:target=WristRCC GOSUB Convert_Degrees_To_PSCUnits GOSUB Write_PSC PAUSE 16000 ENDIF 'Grip Close IF SRINPUT = $11 THEN pscChannel = Gripper:target=GripC GOSUB Convert_Degrees_To_PSCUnits GOSUB Write_PSC ENDIF 'Grip Open IF SRINPUT = $12 THEN pscChannel = Gripper:target=GripO GOSUB Convert_Degrees_To_PSCUnits GOSUB Write_PSC ENDIF 'Stop All IF SRINPUT = $13 THEN GOSUB StopAll ENDIF 'Park Arm IF SRINPUT = $14 THEN GOSUB ParkArm ENDIF RETURN ParkArm: ' Park arm and clear change memory GOSUB stopAll target = BaseP: pscChannel = Base GOSUB Convert_Degrees_To_PSCUnits GOSUB Write_PSC target = ShoulderP: pscChannel = shoulder GOSUB Convert_Degrees_To_PSCUnits GOSUB Write_PSC target = ElbowP: pscChannel = Elbow GOSUB Convert_Degrees_To_PSCUnits GOSUB Write_PSC target = WristP: pscChannel = Wrist GOSUB Convert_Degrees_To_PSCUnits GOSUB Write_PSC target = WristRP: pscChannel = WristRotate GOSUB Convert_Degrees_To_PSCUnits GOSUB Write_PSC target = GripP: pscChannel = Gripper GOSUB Convert_Degrees_To_PSCUnits GOSUB Write_PSC changeByte = 0 ' Reset control byte SRINPUT = 0 PAUSE 6000 ' Allow arm to park fully RETURN Convert_Degrees_To_PSCUnits: servoPos = ((target * 56) / 10) + 250 RETURN Write_PSC: SEROUT PSC,Baud,["!SC",pscChannel, ramp, servoPos.LOWBYTE, servoPos.HIGHBYTE, CR] RETURN StopAll: pscChannel = 0: GOSUB StopServo pscChannel = 1: GOSUB StopServo pscChannel = 2: GOSUB StopServo pscChannel = 3: GOSUB StopServo pscChannel = 4: GOSUB StopServo pscChannel = 5: GOSUB StopServo RETURN StopServo: SEROUT PSC, Baud, ["!SCRSP", pscChannel, CR] SERIN PSC, Baud, 1000, Init,[STR Buff\3] servoPos.LOWBYTE = buff(2) servoPos.HIGHBYTE = buff(1) SEROUT PSC, Baud,["!SC", pscChannel, ramp, servoPos.LOWBYTE, servoPos.HIGHBYTE, CR] RETURN