Other Projects

Marble Maze

Background:
In the summer of 2009 the Central Illinois Robotics Club was invited to demonstrate at the Illini Boy Scout Jamboree event. This event would be attended by over 2,000 scouts and provided an excellent opportunity to spark their interest in robotics. We planned several demonstrations to showcase autonomous hobby robotics, combat robotics, consumer robotics, and more. Based upon previous demonstrations our club had done in the past, I knew that a hands-on project would be very popular with visitors to our display. I began brainstorming ideas and decided upon an accelerometer-controlled marble maze.

I recently completed a project involving wireless control of a Parallax BOE-Bot using an accelerometer and decided to adapt this system to control a custom-built 2 axis maze assembly. Accelerometers can be demonstrated in many exciting ways and kids will likely be drawn to a demo considering the similarity to popular applications such as the Nintendo Wii. The goal of this project was to create an exciting game visitors can play while learning about sensors, microcontrollers, and hobby robotics. Looking ahead, I also have ambitions of one day incorporating vision software and equipment to allow the maze to solve itself.





Design:

The first step in my design process was to research existing applications or designs. No matter what you're building it's likely someone out there has already built something similar or perhaps even exactly the same. Rather than waste time and money going through several iterations yourself it's very beneficial to learn from others then adapt these ideas to your application. One example I referenced during my design process was a servo operated marble maze that can be controlled over the internet. This design featured two rotating assemblies that fit inside one another providing tilt about two perpendicular axis. The major benefit of this design was the simplicity of the servo actuation control and mechanical mounting. Either axis is mechanically isolated removing the need for complex multiple degree of freedom joints. Also because the servos do not move in relation to the shaft's axis, this removes the need for complex math since servo rotation and platform tilt remain a linear relationship.
 
 Existing Design
 

SolidWorks CAD Model of My Marble Maze Design
 

Mechanical Components:

I wanted to keep the design of the marble maze simple to allow for easy fabrication but I was also concerned with aesthetics. Since this would be used for demonstrations at events, it needed to look good and not like a hastily thrown together pile of parts. I considered using several materials but was constrained by my limited access to tools and the fact that I live in an apartment. Ultimately I choose Quick Frame from 80/20.


Quick Frame Extruded Aluminum

I had used extruded aluminum T-slot material on a previous project and was very pleased with the ease of assembly and aesthetics. While considering T-slot for this project I stumbled upon the Quick Frame system which is assembled by hammering plastic connectors into the extruded aluminum profiles. Some profiles include a flange which is perfect for mounting the marble maze to the assembly. I ordered the material needed along with connectors from 80/20's surplus eBay store at a significant savings but needed to cut the sections to length and miter edges myself.

The parts arrived a few days later and looked perfect. I'm still uncertain why 80/20 felt these parts should be considered surplus/scrap. In order to cut the sections to length I picked up a 6" cut-off saw from Harbor Freight and hoped my neighbors wouldn't mind the noise. After a few hours of cutting, filing, and deburring I had prepared every piece.


Quick Frame Parts Ready for Assembly

To operate the marble maze I selected two HS-645MG Hitec servos from Lynxmotion with a gear drive system from ServoCity. I chose gears rather than chain or direct drive in order to increase servo torque, limit maximum tilt, avoid backlash, and provide smoother operation. A  48 pitch 19 tooth gear is mounted to the servo as the pinion and a 98 tooth gear is mounted to the frame using a matching ServoCity hub.


ServoCity Gear with Hub


Gear Mounted to Frame

To mount the gears to the frame members I slid the hubs inside of the extruded aluminum members to provide a threaded "nut". I also drilled a hole in the bottom of the frame member so I can access the hub's set screw. This allows me to remove the shaft for assembly or disassembly. At this point I could begin to put the marble maze together starting with the inner ring followed by the outer ring and lastly the support base.


Inner Ring (Maze Platform)


Complete Assembly


Servo Drive for Inner Ring (X-Axis)


Servo Drive for Outer Ring (Y-Axis)

Initial Electronics:

Once the basic assembly was completed I added servo cable extensions and zip ties to route the cables to the development board. This was done to prevent them from getting tangled or caught on the assembly during operation. To control the servos I used a BOE development board with a BS2 microcontroller from Parallax along with a Memsic 2125 dual-axis accelerometer as the controller.


Electronics Installed


Electronics

Marble Maze Test 8/9/09


Electronics Upgrade:

During testing I encountered a problem in which the servos and frame would "jitter". This can be seen in the video above. I was concerned that this may effect the final performance of the maze making it difficult to control. I asked for advice on the Central Illinois Robotics Club Yahoo Group and received tons of feedback immediately.

While the jitter was being caused by several factors both electrical and mechanical, the biggest problem was the update rate for the servos. Servos require a pulse command every 20 msecs and my program was not capable of this frequency. The speed in which the BS2 microcontroller could process my program resulted in a servo pulse command being sent approximately every 40 msecs after making several changes to decrease the subroutine duration. It seemed I was up against a wall limited by the processing power of the BS2. In my attempt to reach a 20 msec pulse update rate I also reached the limit of the BS2's variable space all while sacrificing the amount of averaging and smoothing I wanted to include.

One suggestion presented on the Yahoo group mailing list was upgrading to the Basic Atom Pro microcontroller. This microcontroller is much faster than the BS2, offers more variable space, includes several unique functions not offered in PBASIC, and has analog inputs as well as digital. This suggestion came from Jim Frye, the owner of Lynxmotion, who offered me a Basic Atom Pro 28, Bot Board II, and a Lynxmotion +/-2g Accelerometer for this project. I was very anxious to move beyond the BS2 to something more powerful and the Basic Atom seemed to be the perfect option. The Basic Atom uses a super set of PBASIC meaning I would not need to learn a completely new language. A few critical differences do exist between the languages but MBasic also offers several unique commands and functions not available in PBASIC such as HSERVO which automatically takes care of sending servo pulses at the necessary frequency.


New Basic Atom powered electronics

I stopped by Lynxmotion to pick up the new electronics and also bought a Hitec HS-5645MG Digital standard servo and Hitec servo programmer. Along with the microcontroller problems, the jitter also seemed to be caused by the frame itself. The mass and inertia of the system would cause the servos to jitter by attenuating small motions and whenever a sudden stop or directional change was performed. To solve this problem I decided to add mechanical dampening to the system and also planned to upgrade one of the servos to a higher torque digital option. I selected the HS-5645MG but also needed a servo programmer to modify the end limits for 180 degree rotation.

Hitec Servo Programmer next to newly installed HS-5645MG Digital Servo

The new electronics seemed to do the trick. The marble maze now operates much smoother with less jitter and I still plan to make a few adjustments to the code that will improve performance even further. The faster processing power and larger variable space of the Basic Atom Pro allows me to easily add even more smoothing and other enhancements if I choose. The video below shows the improved operation:

Marble Maze 8/24/09

Mechanical Damping:

To implement mechanical dampening I added springs and small rubber o-rings to each pivot joint in order to add frictional resistance. The springs were installed between the frame elements while the rubber o-rings were compressed between the shaft collars and frame. This added enough mechanical dampening to decrease inertial attenuation but without adding too much load on the servos.

Rubber o-rings compressed between shaft collars and frame

Along with the small rubber o-rings, compression springs and washers were added to the gap between frame components

The Maze:

While the mechanical design and electronics of the marble maze required a great deal of attention, we cannot forget the importance of the maze itself. The addition of the maze transforms this project from a electro-mechanical thing-a-ma-jig to an exciting hands-on game. For this project I choose to build a maze myself using balsa wood. This would allow me complete control of the maze difficulty and dimensions and also ensures the weight is kept to a minimum. I also found wooden balls to serve as the "marbles" for the maze. It just so happens that these balls and the balsa wood walls make a fun "thunk" noise whenever they collide.


Balsa Wood Maze


Wooden "Marble"

For my original design I had planned to use Velcro or bolts to secure the maze to the frame but I was able to hold tight enough tolerances such that the maze actually fits snugly inside the frame without any fastening hardware needed. My goal is to build several mazes all 12" X 12" so that I can pop in a different maze if I want something more challenging or easier.


Maze Installed in Frame

Marble Maze Test 8/26/09


Controller:

During construction and testing of the marble maze I would hold the accelerometer sensor in my hand using two 24" servo extension cables. A better method would of course be needed for the final design. I needed something that would provide protection for the sensor and wire connections as well as provide a comfortable ergonomic way to operate the maze. I also wanted to do away with the separate servo wire extension cables and combine all wiring into something more compact and robust. My solution was to mount the sensor in a Wii steering wheel which is simply a plastic shell that the Wii controller snaps in to. To connect the controller to the Bot Board II I used a standard DB-9 serial cable.


Nintendo Wii Steering Wheel


DB-9 Serial Cable

I purchased the Wii steering wheel unaware that an additional button is mounted to the back for the "B" controller input. At first I considered removing this button to prevent users from getting confused but decided to utilize it instead. During testing I found it awkward and difficult to hand off the controller or put it down without the maze going crazy as it continued to follow the accelerometer's measurements. I decided that a "dead-mans" switch would be a useful addition so that the maze would only accept input when the button is held down. This allowed the controller to be handled without causing the maze to move unless desired.


"B" Button

Using a little super glue I mounted a push button switch scavenged from an old microwave (it always pays to horde scrap parts) directly on top of the "B" button. I used two of the serial cable wires to route the button leads back to the microcontroller board and added a bit of code that would check to see if the button was depressed before getting updated readings from the accelerometer. If the button is not held down, the maze will simply continue to hold the last known position effectively "pausing" the game.


Push Button Mounted to Controller "B" Button

I easily managed to fit all of the electronics inside of the Wii wheel and added a small wire clamp to hold the serial cable in place. The clamp made sure that the cable could not be pulled out yanking the connectors loose. I glued 4 threaded standoffs in each corner of the compartment and cut out a small 1/8" thick polycarbonate to cover everything. At first I planned to keep the panel clear to show the internal electronics but instead decided to use the space to add some logos for the Central Illinois Robotics Club and Lynxmotion who was kind enough to provide many of the components used. I think it helped make the project look more professional which was important since this project is primarily intended for use at events.

The 2-Axis Accelerometer and Push Button Switch Mounted in the Wii Wheel

Custom Polycarbonate Cover for the Electronics Compartment

The final step was to add connectors on the controller board enclosure for the serial cable, servo, and power connections. While not an absolute perfect fit, the standard SparkFun enclosure is sized close enough for the Bot Board II and I had a few available. I prefer to use the black enclosure base with the clear enclosure top. I added a power connector for use with a wall adapter or battery pack, a hole for the servo cable extensions, and a female DB-9 connector for the controller. I then mounted the enclosure to the frame using heavy duty hook and loop fastener to allow easy removal if needed.


SparkFun Enclosure

DB-9 Serial Connector for Controller, Servo Lead Hole, and Power Connector

Cables Attached

Enclosure Mounted to Marble Maze Frame

Gumball Dispenser:

Of course every game needs to have a prize and what better prize than candy! I wanted to add more mechanics and theatrics to the marble maze in the form of an automatic prize dispenser and decided to use a gumball machine that would automatically dispense a gumball when the player reaches the end of the maze. Rather than building my own dispenser from scratch, I decided to purchase a toy gumball machine through eBay for $20. This saved a great deal of time and also provided a classic-looking device. I easily replaced the hand crank with a plastic sprocket and mounted the gumball machine on top of the same project enclosure I used for my RFID door opener. This provided a place to hide the electronics for the tri-color LEDs lights which I'll discuss later and also featured a place to mount the drive servo. To operate the gumball dispenser I hacked a standard HITEC HS-5475HB servo for continuous rotation by removing the mechanical stop and gluing the potentiometer in the "center" position. Using a plastic chain drive I connected the servo to the gumball machine shaft.


Gumball Dispenser mounted to Electronics Enclosure


HS-5475HB Servo hacked for Continuous Rotation


Plastic Chain Drive on Gumball Dispenser Shaft

When I placed an order through SparkFun Electronics for some of the maze components I also picked up a few of their Triple Output RGB LEDs. I had never used a tri-color LED before and was very intrigued. After playing around with a few of these LEDs I knew I had to incorporate then into the marble maze somehow. I decided to mount them in the bottom of the gumball dispenser so that they would shine through the transparent spiral ramps and reflect through the plastic.

Tri-Color LEDs Mounted in Base of Gumball Dispenser

LED Wiring Underneath Gumball Dispenser Base

The neat thing about tri-color LEDs is that you have the ability to create many more colors by pulsing the LEDs three main colors on and off at varying frequencies. This allows you to create not only red, green, and blue but also every combination in between these such as orange, purple, and more. I wrote a simple set of code that slowly turns each color on and off in different combinations to create a constantly changing light show inside the gumball dispenser. This code can be found at the bottom of the page in the "Program" section.


Unfortunately I ran into a problem when trying to implement these LEDs. The BASIC Atom Pro I was using for all of the marble maze functions was simply too busy to also handle the PULSOUT functions required for the LEDs. To solve this problem I decided to add a BS2 microcontroller whose sole purpose would be to handle operation of the LEDs. The BS2 and development board are mounted inside the project enclosure right beneath the gumball dispenser.

Sneak Peek of V2.0:


Sensor Upgrade:

One common problem I experienced with the first version of the marble maze was the maze completion photocell sensor. The sensor was mounted in a small cup attached to the underside of the maze in a hole. Normally the photocell would see high light levels indicating that the marble had not yet reached the end of the maze. When the marble rolled on top of this cup the light would be blocked and the photocell would signal that the maze had been completed. Unfortunately the photocell was too sensitive to changes in light level and would also occasionally be triggered by shadows passing over the sensor especially.

To avoid this problem for V2.0 of the marble maze I switched out the photocell with an infrared phototransistor. Specifically I chose to use the QRD1114 from Sparkfun. This sensor uses an infrared emitter and detector to indicate the presence of a nearby object rather than using light levels. The QRD also is very small which made it easy to implement.

QRD1114 Optical Detector / Phototransistor


Infrared Sensor at End of Maze

Impressed with the performance of this sensor, I also decided to use the QRD1114 for detecting successful gumball delivery. Another problem I encountered with V1.0 of the marble maze was that the gumball dispenser would occasionally jam causing no gumball to feed into the delivery mechanism. V1.0 simply ran the dispenser servo for a preset amount of time but had no way of knowing if a gumball actually came down the spiral chute. To fix this problem I mounted another QRD1114 inside of the gumball machine. This sensor was oriented directly above the beginning of the spiral chute. Once a gumball passed this sensor, the servo would stop. If a gumball was not detected after a set amount of time the anti-jam system would activate to try and dislodge any blockage.

Gumball Delivery Sensor (small black object near top of clear spiral chute)

Robot Arm:

In my quest to add even more mechanical excitement to the marble maze, I decided to add a robotic arm that would automatically return the marble to the beginning of the maze and deliver the gumball prize to each player. I choose the Lynxmotion AL5A robotic arm for this task. These kits offer the benefit of using the Lynxmotion Servo Erector System which makes it easy to modify the overall dimensions, functions, and other features.

Lynxmotion AL5A Robotic Arm

The addition of a robotic arm really adds to the excitement of the marble maze but I wanted to incorporate even more and make my implementation truly unique. To do this I turned to my experimentation with MicroRAX which led to a linear motion system. I decided to add linear motion capability to the robotic arm so that the arm could not only manipulate the marble and gumballs but also do so in different locations across the entire assembly.


Gear Rack Mounted to MicroRAX


Roller Wheels Installed on 4 Mounting Holes of Arm Rotation Base


Micro Servo Mount (spring provides constant gear engagement)

Micro Servo Installed in Spring Mount

<<SECTION UNDER CONSTRUCTION>>


Code:

Basic Atom Pro 28 Program:
Version 2.0 (11/6/10)

' -----[ Title ]--------------------------------------------------------------
' MarbleMaze.bas
' 2-axis marblemaze is controlled using an handheld controller with accelerometer

' -----[ Constants ]----------------------------------------------------------

deadband           CON     50              ' Tilt threshhold
sample             CON     40              ' Define sample size for accelerometer averaging

' -----[ Bot Board I/O Pin Definitions ]---------------------------------------

' Sensors
AccY            CON     18              ' Accelerometer y-axis
AccX            CON     17              ' Accelerometer x-axis
;MazeComp       CON        11
;GumballSense   CON        12

' Switches
;LinrMaze       CON        4                ' Limit switch on linear track (maze end)
;LinrDisp       CON        5                ' Limit switch on linear track (gumball dispenser end)
;Trigger        CON        6                ' Trigger button on back of controller
;ButtonReset    CON        13                ' Game start/reset button
;ButtonDisp     CON        14                ' Manual gumball dispensing

' Servos
YServo          CON     2               ' Y-axis servo
Xservo          CON     3               ' X-axis servo

' MISC
LCD             CON        10                ' LCD       

' -----[ SSC-32 I/O Pin Definitions ]------------------------------------------

linr            var        word            ' P0 - Arm Linear Movement Servo
base            var        word            ' P1 - Arm Base Rotation Servo
shld            var        word            ' P2 - Arm Shoulder Servo
elbo            var        word            ' P3 - Arm Elbow Servo
wrst1           var        word            ' P4 - Arm Wrist Servo
vac             var        word            ' P13 - Vacuum Command
gumball         var        word            ' P15 - Gumball Dispensing Servo
shake           var        word            ' P12 - Gumball Dispenser Shaker
ttm             var        word            ' Duration of move

' -----[ Variables ]----------------------------------------------------------

x                  VAR     Word                ' Left/right (inner ring) tilt
y                  VAR     Word                ' Uphill/downhill (outter ring) tilt

xMeas              VAR     Word(sample + 1)    ' Accelerometer x-tilt measurements
yMeas              VAR     Word(sample + 1)    ' Accelerometer y-tilt measurements

xSum               VAR     Word                ' Summation variable for x-tilt
ySum               VAR     Word                ' Summation variable for y-tilt

rdy                var        Word                ' LCD Screen check for "ready" subroutine

pulseX          VAR     SWord               ' left/right (inner ring) pulse value
pulseY          VAR     SWord               ' Uphill/downhill (outter ring) pulse value

pulseVac        VAR     SWord            ' Vacuum relay command

counter         VAR     Byte                ' Loop counter variable used for accelerometer averaging

MazeComp        VAR        Byte

ButtonB         VAR        Byte          ' Controller trigger state

PhotoSense      VAR        Byte          ' Maze completion light sensor

Button_Reset    VAR        Byte          ' Game start/reset button

Button_Disp     VAR        Byte          ' Gumball dispense button

GumballSense    VAR        Byte          ' Confirmation of gumball release

Linr_Maze       VAR        Byte         ' Maze end linear motion limit switch

Linr_Disp       VAR        Byte         ' Gumball dispenser linear motion limit

' -----[ Initialization ]-----------------------------------------------------

sound 9, [100\880, 100\988, 100\1046, 100\1175]    'Play tune to indicate program start has begun

' Clear LCD Screen
serout LCD, i9600, [12]
serout LCD, i9600, ["Initialize"]

enablehserial
sethserial h38400,h8databits,hnoparity,h1stopbits

' Define initial SSC-32 position commands
linr=1519:    base=1460:    shld=1725:    elbo=1570:
wrst1=1260:    gumball=1503: vac=1000: shake=1630: ttm=2000:
gosub send_data

' Set servo pins as outputs
low    YServo
low    XServo

' Set LCD "ready" check to 0
rdy = 0

' -----[ Game Ready Routine ]--------------------------------------------------

ready:
     
    ' Display current location in program
    if rdy = 0 then
        serout LCD, i9600, [12]
        serout LCD, i9600, ["Press the RED", 148, "Button to Start"]
    endif

    ' Reset rdy variable
    rdy = 0
   
    ' Start with all pulses centered (maze level)
    pulseX = 0                                        
    pulseY = 0

    enablehservo                            ' Activate HSERVO function

    HSERVO [XServo\pulseX, YServo\pulseY]   ' Send control pulses to servos
    HSERVOWAIT [XServo, YServo]

      Linr_Maze = in4                            ' Check to see if the arm is in position next to the maze
      if (Linr_Maze = 1) then
          goto Arm_Position_Maze
      endif
     
      Button_Reset = in13                        ' Store input reading from start/reset button
      if (Button_Reset = 0) then                ' If start/reset button is pressed, start game
          goto start
      endif
     
      ' Overide button is used to dispense gumball in case of malfunction
      Button_Disp = in14                    ' Store input reading from gumball overide button
      if (Button_Disp = 0) then            ' If overide button is pressed, go to dispense subroutine
          goto dispense_gumball
      endif
    
     ' Set rdy variable to 1 so that on next sub routine execution, LCD will not clear
     rdy = 1
    
goto ready

' -----[ Game Start Routine ]--------------------------------------------------

start:

    ' Display current location in program
    serout LCD, i9600, [12]
    serout LCD, i9600, ["Resetting Maze..."]

    ' Position arm to pick up marble from finish position
    base=2225:    shld=1405: elbo=1420:
    wrst1=1595:    ttm=2000
    gosub send_data
   
    ' Lower suction cup onto marble
    elbo=1620: ttm=2000:
    gosub send_data
   
    ' Turn on vacuum pump
    vac=2000: ttm=10:
    gosub send_data
    pause 500
   
    ' Raise marble from maze
    elbo=1360: wrst1=1400: ttm=2000:
    gosub send_data
   
    ' Check to see if suction failed to pick up marble
    MazeComp = in11                                ' Store input reading from photosensor
    if (MazeComp = 0) then                        ' If sensor detects ball at end of maze, go to victory routine
         vac=1000: ttm=10:                        ' Turn off vacuum
         gosub send_data
        ' Move maze then return to flat position
        pulseX=1000: pulseY=1000:
        HSERVO [XServo\pulseX, YServo\pulseY]   ' Send control pulses to servos
        HSERVOWAIT [XServo, YServo]
        pause 500
        pulseX=0: pulseY=0:
        HSERVO [XServo\pulseX, YServo\pulseY]   ' Send control pulses to servos
        HSERVOWAIT [XServo, YServo]
        goto start
     endif
       
    ' Position marble over beginning of maze
    base=2350: shld=1560: elbo=1560: wrst1=1420: ttm=2000:
    gosub send_data
   
    ' Turn off vacuum pump
    vac = 1000: ttm=10:
    gosub send_data
    pause 500

    ' Move Arm out of the way
    base=1460:    shld=1730:    elbo=1570:
    wrst1=1260: ttm=2000:
    gosub send_data
   
    serout LCD, i9600, [12]
    serout LCD, i9600, ["Get Ready!"]
   
    ;for counter = 1 to 3                        ' Play start count down
    ;    sound 9, [150\2000]
    ;    pause 850
    ;next

    serout LCD, i9600, [12]
    serout LCD, i9600, ["GO!!!"]
   
    sound 9, [500\3136]

goto main

' -----[ Main Routine ]-------------------------------------------------------

main:                                             ' Begin main routine.

      Button_Reset = in13                            ' Store input reading from start/reset button
      if (Button_Reset = 0) then                    ' If start/reset button is pressed, reset game
          pause 500
         goto ready
      endif

    MazeComp = in11                            ' Store input reading from photosensor
    if (MazeComp = 0) then                    ' If sensor detects ball at end of maze, go to victory routine
         goto Confirm
     endif
 
    goto get_pulses                                ' Read accelerometers                           
   
goto main       

' -----[ Servo Position Output Control ]--------------------------------------

SERVO_PULSES:

      HSERVO [XServo\pulseX, YServo\pulseY]       ' Send control pulses to servos
      HSERVOWAIT [XServo, YServo]
 
goto main
  
;' -----[ Accelerometer Control Sub Routine ]----------------------------------

GET_PULSES:

      ADIN AccX, xMeas(sample)                         ' Get controller X-axis tilt
      ADIN AccY, yMeas(sample)                         ' Get controller Y-axis tilt
 
      ' Define maximum and minimum values to avoid anomalies
      x = (x MIN 365 MAX 650)
      y = (y MIN 365 MAX 650)
 
      ' Define summation variables as previous calculated average
      xSum = x
      ySum = y

      FOR counter = 0 TO sample
        xSum = xSum + xMeas(counter)                ' Sum together last few X measurements
        xMeas(counter) = xMeas(counter + 1)         ' Index the value to lower position
        ySum = ySum + yMeas(counter)                ' Sum together last few Y measurements
        yMeas(counter) = yMeas(counter + 1)            ' Index the value to lower position
      NEXT

      ' Divide summations by number of included variables to calculate average
      x = xSum / (sample + 2)
      y = ySum / (sample + 2)

      ' Maze tilt calculations
      pulseX  = 40000 - (x * 80)                      ' X & Y tilt servo pulses
      pulseY = 40000 - (y * 80)

      ' Define maximum and minimum servo pulse values to specify servo position limits
      pulseX  = pulseX  MIN -10000 MAX 10000
      pulseY = pulseY MIN -10000 MAX 10000
 
      goto SERVO_PULSES
 
' -----[ Maze Completion Confirmation ]-------------------------------------

Confirm:

    pause 400

    MazeComp = in11                ' Store input reading from photosensor
    if (MazeComp = 0) then        ' If sensor detects ball at end of maze, go to victory routine
         goto Victory
     endif

goto main

' -----[ Maze Completion Sub Routine ]--------------------------------------
 
 Victory:
 
    ' Display current location in program
    serout LCD, i9600, [12]
    serout LCD, i9600, ["Congratulations!"]
   
     ' Play victory song upon maze completion (Roomba Victory Song)
     sound 9, [200\523, 200\659, 200\784, 300\1046, 100\0, 175\1046, 450\1397]

    ' Start with all pulses centered (maze level)
    pulseX = 0                                        
    pulseY = 0

    HSERVO [XServo\pulseX, YServo\pulseY]   ' Send control pulses to servos
    HSERVOWAIT [XServo, YServo]
    
 goto dispense_gumball
    
' -----[ Dispense Gumball Sub Routine ]-------------------------------------
 
 dispense_gumball:

    ' Display current location in program
    serout LCD, i9600, [12]
    serout LCD, i9600, ["Here Comes Your Prize!"]
 
    ' Activate servo to dispense gumball
    gumball=1480: ttm=250:                   
    gosub send_data                   

    i    var    word

    i = 0
    while (i <= 35000)                        ' Wait for gumball to be dispensed but only for limited time
   
        GumballSense = in8                    ' Store input reading from IR sensor
        if (GumballSense = 0) then
                gumball=1503: ttm=1:        ' Stop Servo           
                gosub send_data               
             goto deliver_gumball
         else
             i = i + 1
         endif

    wend   

    gumball=1503: ttm=1:                    ' Stop Servo                   
       gosub send_data               
    goto dislodge_gumball

goto dispense_gumball

' -----[ Deliver Gumball Sub Routine ]-------------------------------------

deliver_gumball:

    ' Move arm into position to pick up gumball
    linr=1435: ttm=1000:
    gosub send_data
   
    ' Wait for arm to reach limit switch at gumball dispenser end of track
    Linr_Disp = in5
    while (Linr_Disp = 1)
        Linr_Disp = in5
    wend

    ' Stop linear movement
    linr=1519: ttm=1:
    gosub send_data
   
    ' Prepare arm to grab gumball
    base=1420: shld=1845: elbo=2100: wrst1=1950: ttm=2000:
    gosub send_data
   
    ' Position arm to grab gumball
    shld=1200: elbo=1670: ttm=2000
    gosub send_data
   
    ' Lower Suction Cup to Gumball
    shld=1125: elbo=1700: ttm=2000
    gosub send_data
   
    ' Turn on vacuum pump
    vac=2000: ttm=10:
    gosub send_data
    pause 500
   
    ' Lift gumball from dispenser
    shld=1390: elbo=1750: wrst1=1775: ttm=2000:
    gosub send_data
   
    ' Position gumball over gumball delivery chute
    base=1170: shld=1570: elbo=1860: wrst1=1820: ttm=2000:
    gosub send_data

    ' Turn off vacuum pump
    vac=1000: ttm=10:
    gosub send_data
    pause 500
   
    ' Return arm to default position
    linr=1519:    base=1460:    shld=1725:    elbo=1570:
    wrst1=1260: ttm=2000:
    gosub send_data
   
goto Arm_Position_Maze

' -----[ Dislodge Gumball Sub Routine ]-------------------------------------

dislodge_gumball:

    serout LCD, i9600, [12]
    serout LCD, i9600, ["Honestly Baby, I Swear This Never Happens"]

    ' Tilt gumball dispenser up
    shake=1000: ttm=250
    gosub send_data
   
    ' Tilt gumball dispenser down
    shake=2250: ttm=500
    gosub send_data
   
    ' Return gumball dispenser to level
    shake=1630: ttm=250
    gosub send_data
   
goto dispense_gumball

;' -----[ Update SSC-32 Servo Positions ]-----------------------------------

Arm_Position_Maze

    ' Display current location in program
    serout LCD, i9600, [12]

    ' Move arm into position next to maze
    linr=1590: ttm=500:
    gosub send_data
   
    ' Wait for arm to reach limit switch at gumball dispenser end of track
    Linr_Maze = in4
    while (Linr_Maze = 1)
        Linr_Maze = in4
    wend

    ' Stop linear movement
    linr=1519: ttm=1:
    gosub send_data
   
goto ready

' -----[ Update SSC-32 Servo Positions ]-----------------------------------

send_data:

    HSEROUT ["#0P", DEC linr, "#1P", DEC base, "#2P", DEC shld, "#3P", DEC elbo, "#4P", DEC wrst1, "#12P", DEC shake, "#13P", DEC vac, "#15P", DEC gumball, "T", DEC ttm, 13]
    pause ttm

return


Basic Stamp 2 Program for Tri-Color LEDs

' {$STAMP BS2}
' {$PBASIC 2.5}

' Author: Dan Toborowski

' Last Updated 10/20/2010

stp         CON   10
Limit       CON   2000

Light       CON   0
Green       CON   13
Red         CON   14
Blue        CON   15

pulsecount  VAR   Word

'-------------[ Initialize ]--------------------------------------------------

LOW Light    ' Turn on Sidewinder Light


'-------------[ RGB LED Light Show ]-------------------------------------------

main:

FOR pulsecount = 1 TO Limit STEP stp

  PULSOUT Green, Limit
  PULSOUT Red, 0 + pulsecount

NEXT

FOR pulsecount = 1 TO Limit STEP stp

  PULSOUT Green, Limit - pulsecount
  PULSOUT Red, Limit

NEXT

FOR pulsecount = 1 TO Limit STEP stp

  PULSOUT Red, Limit
  PULSOUT Blue, 0 + pulsecount

NEXT

FOR pulsecount = 1 TO Limit STEP stp

  PULSOUT Red, Limit - pulsecount
  PULSOUT Blue, Limit

NEXT

FOR pulsecount = 1 TO Limit STEP stp

  PULSOUT Blue, Limit
  PULSOUT Green, 0 + pulsecount

NEXT

FOR pulsecount = 1 TO Limit STEP stp

  PULSOUT Blue, Limit - pulsecount
  PULSOUT Green, Limit

NEXT

GOTO main

<\font>
Comments