'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''' ''' ''' READING_ENCODERS.BS2 - Program to read the encoder ''' ''' AUTHORS: Lior Elazary (lelazary@yahoo.com) ''' ''' LOG: 7/3/01 Created (Lior Elazary) ''' ''' '''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''' ' Wheels Vars Right_Wheel Con 0 ' The right wheel pin connection Left_Wheel Con 1 ' The left wheel pin connection Right_Encoder Var IN2 ' The pin the right encoder is attached Left_Encoder Var IN3 ' The pin the left encoder is attached Right_Stop Con 650 ' The right stop value determined by simple.bs2 Left_Stop Con 642 Right_Speed Var Word Left_Speed Var Word Right_Encode_Speed Var byte Left_Encode_Speed Var word Last_Right_Encoder Var Bit Last_Left_Encoder Var Bit Odom Var word X Var byte T Var byte Input 2 Input 3 Speed Con 30 Move_Forward: Right_Speed = Right_Stop - Speed 'Set the right and left speed Left_Speed = Left_Stop + Speed Odom = 0 ' Reset odometer Move_Forward_Loop: Right_Encode_Speed = 0 'reset encode speed values Left_Encode_Speed = 0 For x = 1 to 46 'loop 46 times to make about 1s pulsout Right_Wheel, Right_Speed 'set the servos speed Pulsout Left_Wheel, Left_Speed For t = 1 to 10 ' Loop 10 times to make ~21ms of loop If Last_Right_Encoder = Right_Encoder THEN Cont_Right 'count changes from 1 to 0 and 0 to 1 Last_Right_Encoder = Right_Encoder Right_Encode_Speed = Right_Encode_Speed + 1 Cont_Right: If Last_Left_Encoder = Left_Encoder THEN Cont_Left Last_Left_Encoder = Left_Encoder Left_Encode_Speed = Left_Encode_Speed + 1 Cont_Left: Next Next ' ~1 sec total time for loop (21ms+1ms) * 46 = 1.012s pulsout Right_Wheel, Right_Speed ' Set the servos speed again to give about 21ms for calculations Pulsout Left_Wheel, Left_Speed Odom = Odom + ((Right_Encode_Speed + Left_Encode_Speed) / 2) ' Set the odometer to the average values of the two speeds ''' ''' Speed is at encode_speed/32 RPS ''' debug "ODOM:", dec Odom, " RS:", dec Right_Encode_Speed, " ", dec (Right_Encode_Speed/32), " LS:", dec Left_Encode_Speed, CR 'Display results Goto Move_Forward_Loop
Make your own free website on Tripod.com