Third Servo Motor
I'm putting together a sumobot for a competition, and my strategy is based on using a third servo motor as a winch to draw up a forklift-type tool in the front of the robot. I've connected the motor, and have used it to successfully operate the tool, but when I try to add winching capabilities to something simple like the basic competition code that comes with the sumo kit, the robot acts weird -- it's moved more slowly than usual, driven off the edge of the mat where before it wouldn't have, and done other strange stuff.
In addition to that, I'm having some difficulty with writing my own code that doesn't even involve the winch motor at all. My code is pretty ugly, mostly Frankensteined together out of sample code (This competition allows only a very very short prep time, and I'm not at all familiar with BASIC), but I'll reply to this in a little while with some code and problems that I've observed trying to run it.
I'm looking for any help I can get: general tips about things to watch out for while trying to use a third servo, specific problems with my setup, anything.
In addition to that, I'm having some difficulty with writing my own code that doesn't even involve the winch motor at all. My code is pretty ugly, mostly Frankensteined together out of sample code (This competition allows only a very very short prep time, and I'm not at all familiar with BASIC), but I'll reply to this in a little while with some code and problems that I've observed trying to run it.
I'm looking for any help I can get: general tips about things to watch out for while trying to use a third servo, specific problems with my setup, anything.
Comments
If you're not a code wizard, consider using a DC motor instead of a servo. You can turn that on and off with a simple high or low command that closes a reed relay. It needn't interfere with your drive servo loop.
Attach your code and maybe someone can help.
' -----[ Title ]---------------------------------------------------------------- ' WALL-E ' {$STAMP BS2} ' Target = BASIC Stamp 2 ' {$PBASIC 2.5} ' Language = PBASIC 2.5 ' -----[ I/O Definitions ]------------------------------------------------------ LMotor PIN 13 ' left servo motor RMotor PIN 12 ' right servo motor WMotor PIN 5 ' winch motor LLineSnsPwr PIN 10 ' left line sensor power LLineSnsIn PIN 9 ' left line sensor input RLineSnsPwr PIN 7 ' right line sensor power RLineSnsIn PIN 8 ' right line sensor input LFIrOut PIN 4 ' left IR LED output LFIrIn PIN 11 ' left IR sensor input RFIrOut PIN 15 ' right IR LED output RFIrIn PIN 14 ' right IR sensor input LSIrOut PIN 2 ' left IR LED output LSIrIn PIN 1 ' left IR sensor input RSIrOut PIN 3 ' right IR LED output RSIrIn PIN 0 ' right IR sensor input ' -----[ Constants ]------------------------------------------------------------ LFwdFast CON 1000 ' left motor forward; fast LFwdSlow CON 800 ' left motor forward; slow LStop CON 750 ' left motor stop LRevSlow CON 700 ' left motor reverse; slow LRevFast CON 500 ' left motor reverse; fast RFwdFast CON 500 ' right motor forward; fast RFwdSlow CON 700 ' right motor forward; slow RStop CON 750 ' right motor stop RRevSlow CON 800 ' right motor reverse; slow RRevFast CON 1000 ' right motor reverse; fast WinchUp CON 1000 ' winch motor reeling in line WinchDown CON 500 ' winch motor letting out line WinchStop CON 750 ' winch motor stop IRNearFreq CON 42000 ' nearsighted IR IRRegFreq CON 38500 ' regular IR ' -----[ Variables ]------------------------------------------------------------ leftSense VAR Word ' left sensor raw reading rightSense VAR Word ' right sensor raw reading blackThresh VAR Word ' QTI black threshold setting lineBits VAR Nib ' decoded sensors value lineLeft VAR lineBits.BIT1 lineRight VAR lineBits.BIT0 irBits VAR Byte ' storage for IR target data irRS VAR irBits.BIT3 ' State of Right Side IR irLS VAR irBits.BIT2 ' State of Left Side IR irRF VAR irBits.BIT1 ' State of Right Front IR irLF VAR irBits.BIT0 ' State of Left Front IR pulses VAR Byte ' counter for motor control counter VAR Byte temp VAR Byte '-------[EEPROM]-------- RunStatus DATA $00 ' run status ' -----[ Initialization ]------------------------------------------------------- Run_Check: ' user Reset button as On-Off READ RunStatus, temp ' read current status temp = ~temp ' invert status WRITE RunStatus, temp ' save status for next reset IF (temp = 0) THEN Set_Threshold ' run now? END ' -- no ... next time ' Sets black threshold to 1/4 the average of the two sensor readings. ' SumoBot must be placed over black playing surface before this code runs. Set_Threshold: ' set QTI black threshold GOSUB Read_Line_Sensors blackThresh = (leftSense / 8) + (rightSense / 8) Start_Delay: ' mandatory five second delay FOR temp = 1 TO 5 PAUSE 900 'INPUT StartLED ' blink each second 'FREQOUT Speaker, 100, 2500, 3000 ' beep each second NEXT ' -----[ Main Code ]------------------------------------------------------------ Main: DO GOSUB Read_Line_Sensors IF lineLeft = 1 AND lineRight = 1 THEN GOTO About_Face ELSEIF lineLeft = 1 THEN GOTO Spin_Left ELSEIF lineRight = 1 THEN GOTO Spin_Right ENDIF '================[ STRATEGY ]================== Label_1: GOSUB Read_IR_Sensors IF (irLF = 1 AND irRF = 1) THEN GOTO Lunge ELSEIF irLF = 1 THEN counter = 0 DO UNTIL (irLF = 1 AND irRF = 1) OR counter > 30 GOSUB Spin_Left2 GOSUB Read_IR_Sensors counter = counter + 1 LOOP ELSEIF irRF = 1 THEN counter = 0 DO UNTIL (irLF = 1 AND irRF = 1) OR counter > 30 GOSUB Spin_Right2 GOSUB Read_IR_Sensors counter = counter + 1 LOOP ELSEIF irLS = 1 THEN counter = 0 DO UNTIL (irLF = 1 AND irRF = 1) OR counter > 30 GOSUB Spin_Left2 GOSUB Read_IR_Sensors counter = counter + 1 LOOP ELSEIF irRS = 1 THEN ' Object right side? counter = 0 DO UNTIL (irLF = 1 AND irRF = 1) OR counter > 30 GOSUB Spin_Right2 GOSUB Read_IR_Sensors counter = counter + 1 LOOP ELSE DO UNTIL (irLF = 1 OR irRF = 1 OR irLS = 1 OR irRS = 1) GOSUB Spin_Right2 GOSUB Read_IR_Sensors LOOP ENDIF LOOP '============================================== ' --[ Movement and Sensors]-- Spin_Left: ' right sensor was active FOR pulses = 1 TO 20 PULSOUT LMotor, LRevFast PULSOUT RMotor, RFwdFast PAUSE 20 NEXT GOTO Main Spin_Right: ' left sensor was active FOR pulses = 1 TO 20 PULSOUT LMotor, LFwdFast PULSOUT RMotor, RRevFast PAUSE 20 NEXT GOTO Main Spin_Right2: ' this one returns instead of going to main FOR pulses = 1 TO 5 PULSOUT LMotor, LFwdFast PULSOUT RMotor, RRevFast PAUSE 20 NEXT RETURN Spin_Left2: ' this one returns instead of going to main FOR pulses = 1 TO 5 PULSOUT LMotor, LRevFast PULSOUT RMotor, RFwdFast PAUSE 20 NEXT RETURN About_Face: ' both sensors on Shikiri line FOR pulses = 1 TO 10 ' back up from edge PULSOUT LMotor, LRevFast PULSOUT RMotor, RRevFast PAUSE 20 NEXT FOR pulses = 1 TO 30 ' turn around PULSOUT LMotor, LFwdFast PULSOUT RMotor, RRevFast PAUSE 20 NEXT GOTO Main Move_Fwd: GOSUB Creep_Forward GOTO Main Creep_Forward: FOR pulses = 1 TO 10 PULSOUT LMotor, LFwdSlow PULSOUT RMotor, RFwdSlow PAUSE 20 NEXT RETURN Lunge: ' locked on -- go get him! FOR pulses = 1 TO 15 PULSOUT LMotor, LFwdFast PULSOUT RMotor, RFwdFast GOSUB Read_Line_Sensors IF (lineBits = %11) THEN Match_Over ' in sight and we're on the line NEXT GOTO Main ' If SumoBot can see the opponent with both "eyes" and both QTIs are ' detecting the border, we must have pushed the opponent out. Match_Over: FOR pulses = 1 TO 10 ' stop motors PULSOUT LMotor, LStop PULSOUT RMotor, RStop PULSOUT WMotor, WinchStop PAUSE 20 NEXT INPUT LMotor INPUT RMotor DIRS = $0000 ' disable all outputs GOTO Run_Check ' reset for next round Read_Line_Sensors: HIGH LLineSnsPwr ' activate sensors HIGH RLineSnsPwr HIGH LLineSnsIn ' discharge QTI caps HIGH RLineSnsIn PAUSE 1 RCTIME LLineSnsIn, 1, leftSense ' read left sensor RCTIME RLineSnsIn, 1, rightSense ' read right sensor LOW LLineSnsPwr ' deactivate sensors LOW RLineSnsPwr ' convert readings to bits lineBits = %00 LOOKDOWN leftSense, >=[blackThresh, 0], lineLeft LOOKDOWN rightSense, >=[blackThresh, 0], lineRight RETURN Read_IR_Sensors: FREQOUT LFIrOut, 1, IRNearFreq ' modulate left front IR LED irLF = ~LFIrIn ' read input (1 = target) FREQOUT RFIrOut, 1, IRNearFreq ' modulate right front IR LED irRF = ~RFIrIn ' read input (1 = target) FREQOUT LSIrOut, 1, IRRegFreq ' modulate left side IR LED irLS = ~LSIrIn ' read input (1 = target) FREQOUT RSIrOut, 1, IRRegFreq ' modulate right side IR LED irRS = ~RSIrIn ' read input (1 = target) RETURNRegarding my initial question about the winch, here's a pseudocode example of something that I tried to add to the "Lunge" section of "Mini Sumo 5.1 : Basic Competition Program"
Lunge: ' locked on -- go get him! FOR pulses = 1 TO 15 PULSOUT LMotor, LFwdFast PULSOUT RMotor, RFwdFast GOSUB Read_Line_Sensors IF (lineBits = %11) THEN Match_Over ' in sight and we're on the line NEXT ' for- next loop using pulsout command to raise tool using winch motor ' for- next loop using pulsout command to lower tool using winch motor GOTO Mainthis appeared to cause the robot to move significantly slower than it had been, and it also tended to drive off the edge of the mat, even though the border avoidance should have stopped it.
-Does PAUSE mean that the robot doesn't do anything at all for x number of milleseconds?
-Does the modulus function work in BASIC? If it does, would using it to split my QTI sensor readings (read left sensor on odd counts, right on evens) help me have a more efficient loop?
Correct, it's dead time; nothing happens during a pause. Better to be doing something useful, like checking sensors umpteen or hundreds of times to while away the lonely (20) milliseconds...
Look at driving all 3 servos simultaneously like this:
z=750
main:pulsout leftmotor,x
pulsout rightmotor,y
pulsout winch,z
blah testsensor
blah testsensor
if blah calculate new x
if blah calculate new y
if blah calculate new z
pause 5
goto main
Here you're driving your winch servo at zero speed (pulsout 750) in that fast loop until you decide to change z, which starts to rotate your servo. Notice that I used a pause 5 instead of 20. All your sensor tests, calculations and if checks takes some time, so if you have a lot of those, they will burn up some time and effectively reduce your pause time. In practice, many servos don't need a full 20 ms pause between pulsouts; they can use a shorter interval, but not longer. Test yours and see what works best for your servos.