' {$STAMP BS2} ' {$PBASIC 2.5} ping PIN 15 'Ping))) is on PIN15 distance VAR Word 'distance we measure with the Ping))) lA CON 15 'length of humerus lB CON 15 'length of radius lC CON 5 'length of wrist lAC VAR Byte 'length from elbow to wrist (triangle ABC) Tx VAR Byte 'Target X position tx=15 Ty VAR Byte 'Target Y position Ty=10 Ta VAR Byte 'Angle we wish the wrist make with the origin ta=192 tri_AC VAR Byte 'Angle the ABC triangle makes with the origin tri_a VAR Byte 'Angle A in the ABC triangle tri_b VAR Byte 'Angle B in the ABC triangle tri_c VAR Byte 'Angle C in the ABC triangle p VAR Byte 'to store the semiperimeter of ABC tri_S VAR Word 'to store the Surface of ABC serv_A VAR Byte 'A joint angle (servo_angle) serv_B VAR Byte 'B joint angle (servo_angle) serv_C VAR Byte 'C joint angle (servo_angle) 'all the servo angles can be transformed in pulse length tmp1 VAR Word 'temp variables tmp3 VAR Word tmp4 VAR Word tmpbit VAR Bit circ VAR Byte circ=0 'tmpbit=1 debut: circ=circ+16 IF circ>=192 OR circ<=64 THEN Tx=15+(COS(circ)/10) ELSE Tx=15-(COS(circ-128)/10) ENDIF IF circ>=0 AND circ <=128 THEN Ty=15+(SIN(circ)/10) ELSE Ty=15-(SIN(circ-128)/10) ENDIF tA=192 'we want the 'hand' to be DOWN (if possible) tmp3=Tx HYP Ty 'distance between target and origin tmp4=Tx ATN Ty 'angle of the segment origin->target IF tmp3=lA+lB THEN 'if NOT possible to satisfy the hand angle requirement Ta=-tmp4 'angle hand going staight to the target IF Ty>lA THEN Ta=tmp4 'if target is high, take the opposite value lAC=(Tx-((lC*COS(Ta))/128)) HYP (Ty-((lC*SIN(Ta))/128)) 're-calculate the position of the wrist ENDIF tmp1 = Tx ATN (Ty+lC) 'calculation of the angle the ABC triangle tri_AC=tmp1 'makes with the origin p=lA+lB+lAC 'perimeter of triangle IF Tx>50 THEN 'original surface calculation p=p/2 'semi-perimeter of ABC triangle (half the perimeter) tri_S=SQR(p*(p-lA)*(p-lB)*(p-lAC)) 'surface of ABC triangle) ELSE 'try to solve near target problems p=p*2 'multiply everything by 4 tri_S=SQR(p*(p-(lA*4))) * SQR( (p-(lB*4)) * (p-(lAC*4)) ) / 16 ENDIF 'SQR(A*B)=SQR(A)*SQR(B) 'DEBUG CLS 'DEBUG "l_AC : ",DEC lAC,CR 'DEBUG "tri_Ac : ", DEC tri_ac*180/128 ,CR 'DEBUG "p : ",DEC p,CR 'DEBUG "S : ",DEC tri_S,CR tmp4=((2*tri_S*256)/(lAC*lA)) / 2 'angle A in tmp_triangle GOSUB arcsin tri_A=tmp3 'tmp4=((2*tri_S*256)/(lA*lB)) / 2 'angle B in tmp_triangle 'GOSUB arcsin 'tri_B=tmp3 tmp4=((2*tri_S*256)/(lB*lAC)) / 2 'angle A in tmp_triangle GOSUB arcsin tri_C=tmp3 tri_B=128 - tri_A - tri_C 'B is calculated to have a total of 128 brads '(sum of the angles in a triangle is 180°) 'just check that tri_A+tri_B+tri_C=128 brads (180°) 'DEBUG ? tri_A + tri_B + tri_C 'DEBUG "tri_A : ",DEC tri_A*180/128,CR 'DEBUG "tri_B : ",DEC tri_B*180/128,CR 'DEBUG "tri_C : ",DEC tri_C*180/128,CR '********* validate and adjust if needed *********************** IF Tx<10 THEN X_adj: tmp1=(lA*COS(tri_A))/128 tmp1=(tmp1+lB*COS(tri_A+tri_B))/128 tmp1=(tmp1+lC*COS(tri_A+tri_B+tri_C))/128 IF tmp1Tx THEN tri_B=tri_B+1 tri_C=tri_C-1 ENDIF Y_adj: tmp1=(lA*SIN(tri_A))/128 tmp1=(tmp1+lB*SIN(tri_A+tri_B))/128 tmp1=(tmp1+lC*SIN(tri_A+tri_B+tri_C))/128 IF tmp1>Ty THEN tri_B=tri_B-1 tri_A=tri_A+1 GOTO Y_adj ENDIF ENDIF '********* adjusted!!! ******************************************** serv_A=tri_A+tri_AC 'servo 1 angle = angleoftriangle + angle A serv_B=tri_B 'servo 2 angle = angle B serv_C=Ta-serv_A-serv_B 'servo 3 angle = hand angle - servo1 - servo2 ELSE 'if target is out of reach or just at the limit serv_A=tmp4 'direct angle to target serv_B=128 'straight arm serv_C=128 'hand pointing to target ENDIF 'in range.. or out.. it's done. FOR tmp1=1 TO 10 PULSOUT 12,(serv_A*6)+270 PULSOUT 13,(serv_B*6)+270 PULSOUT 14,(serv_C*6)+270 PAUSE 10 NEXT DEBUG "targ_X:",DEC tx,CR 'send the data on the serial port DEBUG "targ_Y:",DEC ty,CR 'to show the virtual arm in EXCEL DEBUG "targ_A:",DEC ta*180/128,CR DEBUG "serv_A:",DEC serv_A*180/128,CR DEBUG "serv_B:",DEC serv_B*180/128,CR DEBUG "serv_C:",DEC serv_C*180/128,CR DEBUG "-----",CR FOR tmp1=1 TO 10 PULSOUT 12,(serv_A*6)+270 PULSOUT 13,(serv_B*6)+270 PULSOUT 14,(serv_C*6)+270 PAUSE 10 NEXT GOTO debut 'loop forever ! DEBUG "END",CR END Ping_Out: ' PING))) LOW Ping ' Force PING))) Line Low PULSOUT Ping, 5 ' Activate PING))) Pulse PULSIN Ping, 1, distance ' Receive Return Pulse IF distance=0 THEN GOTO ping_out distance = distance ** 2257 ' Calculate Distance RETURN ' -----[ Subroutine - Arcsine ]----------------------------------------------- ' This subroutine calculates arcsine based on the y coordinate on a circle ' of radius 127. Set the side variable equal to your y coordinate before ' calling this subroutine. ARCSIN: ' Inverse sine subroutine GOSUB ARCOS ' Get inverse cosine tmp3 = 64 - tmp3 ' sin(angle) = cos(90 - angle) RETURN ' -----[ Subroutine - Arccosine ]--------------------------------------------- ' This subroutine calculates arccosine based on the x coordinate on a circle ' of radius 127. Set the side variable equal to your x coordinate before ' calling this subroutine. ARCOS: ' Inverse cosine subroutine tmp4 = ABS(tmp4) ' Evaluate positive side tmp3 = 63 - (tmp4/2) ' Initial angle approximation DO ' Successive approximation loop IF (COS tmp3 <= tmp4) THEN EXIT ' Done when COS angle <= side tmp3 = tmp3 + 1 ' Keep increasing angle LOOP RETURN