programming the sumobot with ping

joy1

Hello everyone...I've been trying to blend the ping program with the simple sumobot program. I thought my goals were modest but I am a bit overwhelmed at this point after hours of trying to get the program to work. Generally speaking, the code works for everything except for the following: when the sumobot goes forward, I want it to go slow (which it does). When it identifies an opponent, I want it to charge ahead at full speed (which it doesn't do) If the PING senses something it runs a bit smoother than when it doesn't detect anything (a bit chattery) Any help would be appreciated. thanks in advance

Here's the code

' Modified SumoBot-3.3-Simple-Mini-Sumo with Ping.BS2
' {$STAMP BS2}
' {$PBASIC 2.5}

'

---

[ I/O Definitions ]

---

LMotor PIN 13 ' left servo motor
RMotor PIN 12 ' right servo motor

LED PIN 14 ' Red LED active high
Ping PIN 15 ' Parallax Ping))) sensor

LLinePwr PIN 10 ' left line sensor power
LLineIn PIN 9 ' left line sensor input
RLinePwr PIN 7 ' right line sensor power
RLineIn PIN 8 ' right line sensor input

StartLED PIN 0 ' display start delay

'

---

[ Constants ]

---

LFwdFast CON 1000 ' left motor fwd; fast
LFwdSlow CON 770 ' left motor fwd; slow
LStop CON 750 ' left motor stop
LRevSlow CON 730 ' left motor rev; slow
LRevFast CON 500 ' left motor rev; fast

CmConstant CON 2260 ' Calc roundtrip time of sound
MaxDistance CON 20 ' Maximum can measure (empirical)

RFwdFast CON 500 ' right motor fwd; fast
RFwdSlow CON 730 ' right motor fwd; slow
RStop CON 750 ' right motor stop
RRevSlow CON 770 ' right motor rev; slow
RRevFast CON 1000 ' right motor rev; fast

'

---

[ Variables ]

---

lLine VAR Word ' left sensor raw reading
rLine VAR Word ' right sensor raw reading
lineBits VAR Nib ' decoded sensors value
lbLeft VAR lineBits.BIT1
lbRight VAR lineBits.BIT0

cmDistance VAR Word ' Distance in centimeters
time VAR Word ' Round trip echo time

pulses VAR Byte ' counter for motor control
temp VAR Byte

'

---

[ EEPROM Data ]

---

RunStatus DATA $00 ' run status

'

---

[ Initialization ]

---

Reset:
READ RunStatus, temp ' read current status
temp = ~temp ' invert status
WRITE RunStatus, temp ' save for next reset
IF (temp > 0) THEN END ' run now?

Start_Delay:
HIGH StartLED ' show active
PAUSE 5000 ' start delay
LOW StartLED ' LED off

'

---

[ Program Code ]

---

Main:
GOSUB Read_Line_Sensors

BRANCH lineBits, [Go_Fwd, Spin_Left, Spin_Right, About_Face]

Go_Fwd:
PULSOUT LMotor, LFwdSlow
PULSOUT RMotor, RFwdSlow
LOW LED ' LED off before each measurement
PULSOUT 15, 5 ' Start Ping)))
PULSIN 15, 1, time ' Read echo time
cmDistance = cmConstant ** time ' Calculate distance from time

IF cmDistance >= MaxDistance THEN HIGH LED ' Toggle LED if out
' range
PULSOUT LMotor, LFwdFast
PULSOUT RMotor, RFwdFast


GOTO Main

Spin_Left:
FOR pulses = 1 TO 10 ' back up from edge
PULSOUT LMotor, LRevFast
PULSOUT RMotor, RRevFast
PAUSE 20
NEXT
FOR pulses = 1 TO 20
PULSOUT LMotor, LRevFast
PULSOUT RMotor, RFwdFast
PAUSE 20
NEXT
GOTO Main

Spin_Right:
FOR pulses = 1 TO 10 ' back up from edge
PULSOUT LMotor, LRevFast
PULSOUT RMotor, RRevFast
PAUSE 20
NEXT
FOR pulses = 1 TO 20
PULSOUT LMotor, LFwdFast
PULSOUT RMotor, RRevFast
PAUSE 20
NEXT
GOTO Main

About_Face:
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

END

'

---

[ Subroutines ]

---

Read_Line_Sensors:
HIGH LLinePwr ' activate sensors
HIGH RLinePwr
HIGH LLineIn ' discharge caps
HIGH RLineIn
PAUSE 1
RCTIME LLineIn, 1, lLine ' read left sensor
RCTIME RLineIn, 1, rLine ' read right sensor
LOW LLinePwr ' deactivate sensors
LOW RLinePwr

' convert readings to bits
LOOKDOWN lLine, >=[300, 0], lbLeft ' 0 = black, 1 = line
LOOKDOWN rLine, >=[300, 0], lbRight
RETURN

Ttailspin

I would hate to see a future SumoBot champion fall thru the cracks, but I don't have a Sumobot to test your code..

However, I might could help by asking exactly what you have powered by the sumo's voltage regulator?
You might be running into some power issues, if you are trying to draw more then 50ma from the sumo's regulator.


-Tommy

joy1

I'm running the board plus two QTI line sensors and the ping.

Andy Lindsay (Parallax)

When you are stuck like this, it helps to either write or recite a narrative of what the code is doing. For example, the narrative starting at Go_Fwd would be:

- Deliver a slow motor pulse
- Turn LED off
- Copy ping distance to cmDistance
- If cmDistance is too large, then deliver fast motor pulse
- Go back to main loop

Once you have the logic on paper, it become apparent that your code is delivering a slow pulse no matter what, and then only going fast if the object is out of range. Next, you have to decide what you want to do in the Go_Fwd routine. For example:

- Check ping
- Turn off LED
- If distance > 15, deliver slow pulse & leave LED off
- if distance <=15, LED on and fast pulse
- Go back to main loop

That would translate to a Go_Fwd routine that looks like this. Notice that I put my actual list of actions as comments right above the code. That helps keep your code on the right track.

Go_Fwd:
 
  ' Check ping
  PULSOUT 15, 5 ' Start Ping))) 
  PULSIN 15, 1, time ' Read echo time
  cmDistance = cmConstant ** time ' Calculate distance from time
 
  ' Turn off LED
  LOW LED ' LED off before each measurement
 
  ' if distance > 15, deliver slow pulse & leave LED off
  ' if distance <= 15, LED on and fast pulse
  IF cmDistance > 15 THEN
    PULSOUT LMotor, LFwdSlow
    PULSOUT RMotor, RFwdSlow
  ELSE
    IF cmDistance >= MaxDistance THEN HIGH LED ' light if in range
    PULSOUT LMotor, LFwdFast
    PULSOUT RMotor, RFwdFast
  ENDIF
 
' Go back to main loop
GOTO Main

Now, I'm not sure if that's the only bug, but I can tell you that this is a necessary step toward getting the program to work right. One thing the code doesn't show is that I changed maxDistance to from 20 to 15. We don't want the "maxiumum it can measure", because then you are comparing it to a condition it cannot get to, so it'll never happen. You should probably reduce it to 10 for the sake of testing. Once the SumoBot charges at your hand when it's less than 10 cm, then, start testing for a practical distance that works.

Please let me know what happened with the modified Go_Fwd routine.