View difference between Paste ID: bb2TbxAh and 8dtqxUXK

 # Ping Object Avoidance
 # Using Ultra Sonic Ping Sensor on servo for avoidance
 # Version 1.1.0
 
 
 # Configuration Settings
 
 # Adjust values below for port configuration
 $pingport = D0 # Change for Ping Port
 $echoport = D1 # Change for Echo Port
 $sweepservo = D2 # Change for sweep servo port
 
 # Adjust values below for movement control
 $reverseturn = 1 # Reverse before turn? 0 = no, 1 = yes
 $maxdistance = 30 # Change for maximum distance from object before avoiding in units
 $boxedindistance = 20 # Change for maximum distance for boxed in detection
 $turnamount = 500 # Change for how long to turn for in ms
 $reverseamount = 500 # Change for how long to reverse for in ms (if applicable)
 $movementspeed = 255 # Change for movement speed
 $slowturn = 127 # Change for slow turn speed
 
 # Adjust values below for sweep configuration
 $sweepmin = 10 # Change for min limit
 $sweepmax = 90 # Change for max limit
 $sweepservodelay = 500 # Change for delay between sweep servo movements and readings
 
 # Testing Options
 $testmode = 0 # Change to 1 if testing without PING sensor
 
 
 
 # Do not adjust these values
 $sweepcenter = $sweepmin+$sweepmax
 $sweepcenter = $sweepcenter/2
 $sweepcenter = Round($sweepcenter,0)
 $sweepprevious = $sweepmax # Do not change
 $sweepcurrent = $sweepcenter # Do not change
 $SweepErrorFlag = 0 # Do not change
 $BoxedInErrorFlag = 0 # Do not change
 $BoxedInRun = 0 # Do not change
 $SweepCenterRun = 0 # Do not change
 $EscapeRun = 0 # Do not change
 $ScriptErrorFlag = 0 # Do not change
 $isboxedin = 0 # Do not change
 $penultimatemove = "none"
 $lastmove = "none"
 
 
 
 # ------- The Script --------
 
 # Set the start point for any loops
 :begin
 
 # Center the sweep servo
 Servo($sweepservo, $sweepcenter)
 
 # Start moving forwards
 FORWARD()
 
 # Start the detection
 Goto(detect) # This line is redundant
 
 
 
 # Detection code
 
 # Set a label for loops and gotos
 :detect
 
 # Use random numbers if in test mode
 IF ($testmode=0)
   $currentdistance = GetPing($pingport, $echoport)
 ELSE
   $currentdistance = GetRandom(0,255)
 ENDIF
 
   # Check the current distance against the max allowed distance
 IF ($currentdistance <= $maxdistance)
   # If the current distance is below the max distance start avoiding
   GOTO(avoid)
 
   # Set label for avoid return to avoid return errors
   :avoidreturn
 ENDIF
 
   # Run the sweeping servo code
 GOTO(sweep)
 
 # Wait
 SLEEP ($sweepservodelay)
 
 # Loop back to the start of detection
 GOTO(detect)
 
 
 
 # Avoidance code
 
 # Set a label for loops and gotos
 :avoid
 
 # First check if boxed in
 Goto(boxedin)
 
 # If the robot is boxed in run the escape code
 IF ($isboxedin = 1)
   Goto(escape)
 
   # Avoid return error after escape loop by setting a label for a goto
   :escapereturn
 ENDIF
 
   # Check to see if to reverse before turning
 IF ($reverseturn = 1)
   # If the option of reverse before turning is set reverse
   Reverse($movementspeed,$reverseamount)
 ENDIF
 
   # Check the servo position
 
   # Check if it's to the left
 IF ($sweepcurrent = $sweepmin)
   # If the servo is in the lowst position (left) move right
   Goto(moveright)
   # Continue moving forwards
   FORWARD()
 
   # Else check if it's to the right
 ELSEIF ($sweepcurrent = $sweepmax)
   # If the servo is in the highest position (right) move left
   Goto(moveleft)
   # Continue moving forwards
   FORWARD()
 
   # Else assume it's in the middle
 ELSE
   # If the servo is in the center position check which side is closest to the object and move the other way
 
   # Move and check the left side
   Servo($sweepservo,$sweepmin)
   # Use random numbers if in test mode
   IF ($testmode=0)
     $pingmin = GetPing($pingport, $echoport)
   ELSE
     $pingmin = GetRandom(0,255)
   ENDIF
 
     # Wait
   Sleep(400)
 
   # Move and check the right side
   Servo($sweepservo,$sweepmax)
   # Use random numbers if in test mode
   IF ($testmode=0)
     $pingmax = GetPing($pingport, $echoport)
   ELSE
     $pingmax = GetRandom(0,255)
   ENDIF
 
     # Wait
   Sleep(400)
 
   # Move and check the center
   Servo($sweepservo,$sweepcenter)
   IF ($pingmin > $pingmax)
     Goto(moveright)
     FORWARD()
   ELSE
     Goto(moveleft)
     FORWARD()
   ENDIF
 ENDIF
 
   # Return to the main code
 Goto(avoidreturn)
 
 
 
 # The sweep code
 
 # Set a label for loops and gotos
 :sweep
 
 # Move in the correct direction and store previous position
 
 # Check what the current position is
 # Check if left
 IF ($sweepcurrent = $sweepmin)
 
   # Save the current position as the previous
   $sweepprevious = $sweepcurrent
 
   # Move to the next position
   Servo($sweepservo, $sweepcenter)
 
   # Save the current position
   $sweepcurrent = GetServo($sweepservo)
 
   # Else check if its center and where it was before
   # If it is center and was left before
 ELSEIF ($sweepcurrent = $sweepcenter and $sweepprevious = $sweepmin)
 
   # Save the current position as the previous
   $sweepprevious = $sweepcurrent
 
   # Move to the next position
   Servo($sweepservo, $sweepmax)
 
   # Save the current position
   $sweepcurrent = GetServo($sweepservo)
 
   # If it is center and was right before
 ELSEIF ($sweepcurrent = $sweepcenter and $sweepprevious = $sweepmax)
 
   # Save the current position as the previous
   $sweepprevious = $sweepcurrent
 
   # Move to the next position
   Servo($sweepservo, $sweepmin)
 
   # Save the current position
   $sweepcurrent = GetServo($sweepservo)
 
   # Else check if right
 ELSEIF ($sweepcurrent = $sweepmax)
 
   # Save the current position as the previous
   $sweepprevious = $sweepcurrent
 
   # Move to the next position
   Servo($sweepservo, $sweepcenter)
 
   # Save the current position
   $sweepcurrent = GetServo($sweepservo)
 
   # Else something has gone wrong
 ELSE
   # Set an error flag for debugging purposes
   $SweepErrorFlag = 1
 ENDIF
 
   # Return back to the main script
 Return()
 
 
 
 # The sweep center code
 
 # Set a label for loops and gotos
 :sweepcenter
 
 # Set a flag so we know it has run for debugging purposes
 $SweepCenterRun = 1
 
 # Move the servo to the left position
 Servo($sweepservo,$sweepmin)
 
 # Use random numbers if in test mode
 IF ($testmode=0)
   $pingmin = GetPing($pingport, $echoport)
 ELSE
   $pingmin = GetRandom(0,255)
 ENDIF
 
 # Wait
 Sleep(200)
 
 # Move the servo to the right
 Servo($sweepservo,$sweepmax)
 
 # Use random numbers if in test mode
 IF ($testmode=0)
   $pingmax = GetPing($pingport, $echoport)
 ELSE
   $pingmax = GetRandom(0,255)
 ENDIF
 
 # Wait
 Sleep(200)
 
 # Move the servo back to the center
 Servo($sweepservo,$sweepcenter)
 
 # Check which side has the closest object
 # If the object to the left is further away than the object to the right
 IF ($pingmin > $pingmax)
 
   # Move to the right
   RIGHT($movementspeed,$turnamount)
 
   # Move forwards again
   FORWARD()
 
   # Else if the object to the right is further away than the object to the left
 ELSEIF ($pingmin < $pingmax)
 
   # Move to the left
   LEFT($movementspeed,$turnamount)
 
   # Move forwards again
   FORWARD()
 
   # Else they are both the same
 ELSE
 
   # So move left - this can be customised
   LEFT($movementspeed,$turnamount)
 
   # And move forwards again
   FORWARD()
 ENDIF
 
-  # Retyrn to the main code
+
 Return()
 
 
 
 # The boxed in code
 
 # Set a label for loops and gotos
 :boxedin
 
 # Set a flag so we know it has run for debugging
 $BoxedInRun = 1
 
 # Get distance to the side
 # Move the servo to the left
 Servo($sweepservo,$sweepmin)
 
 # Use random numbers if in test mode
 IF ($testmode=0)
   $side1scan = GetPing($pingport,$echoport)
 ELSE
   $side1scan = GetRandom(0,255)
 ENDIF
 
   # Get distance to the other side
   # Move the servo to the right
 Servo($sweepservo,$sweepmax)
 
 # Use random numbers if in test mode
 IF ($testmode=0)
   $side2scan = GetPing($pingport,$echoport)
 ELSE
   $side2scan = GetRandom(0,255)
 ENDIF
 
   # Get distance to the front
   # Move the servo to the center
 Servo($sweepservo,$sweepcenter)
 
 # Use random numbers if in test mode
 IF ($testmode=0)
   $centerscan = GetPing($pingport,$echoport)
 ELSE
   $centerscan = GetRandom(0,255)
 ENDIF
 
-  # Check if boxed in by compairing the results against a fixed boxed in distance
+  # Check if boxed in by comparing the results against a fixed boxed in distance
 IF ($side1scan < $boxedindistance and $side2scan < $boxedindistance and $centerscan < $boxedindistance)
 
   # If any are true set the boxed in flag
   $isboxedin = 1
 
 ENDIF
 
   # Return to the main script
 Return()
 
 
 
 # The escape code
 
 # Set a label for loops and gotos
 :escape
 
 # Set a flag so we know it has run for debugging
 $EscapeRun = 1
 
 # Reset the boxed in flag
 $isboxedin = 0
 
 # Center the sweep servo
 Servo($sweepservo,$sweepcenter)
 
 # Turn slowly
 Left($slowturn)
 
 # Set up a loop
 :escapeloop
 
 # Scan until clear
 # Use random numbers if in test mode
 IF ($testmode=0)
   $escapescan = GetPing($pingport,$echoport)
 ELSE
   $escapescan = GetRandom(0,255)
 ENDIF
 
   # If the scan result is below the boxed in distance loop
 IF ($escapescan < $BoxedInDistance)
 
   # Go back to the start of the escape loop
   Goto(escapeloop)
 
 ENDIF
 
   # Continue forwards
 FORWARD()
 
 # Return to the main script
 Goto(escapereturn)
 
 
 
 # Move Right code
 
 # Set a label for loops and gotos
 :moveright
 
 # Check the last 2 moves to avoid left right left right loops
 IF ($lastmove = "left" and $penultimatemove = "right")
 
   # If it has been right then left dont move right again but escape from a loop
   Goto(escape)
 
   # Reset the last move
   $lastmove = "none"
 
   # Else just move right
 ELSE
   RIGHT($movementspeed,$turnamount)
 
   # Save the penultimate move
   $penultimatemove = $lastmove
 
   # Save the last move
   $lastmove = "right"
 
 ENDIF
 
   # Go back to the main script
 Return()
 
 
 
 # Move left code
 
 # Set a label for loops and gotos
 :moveleft
 
 # Check the last 2 moves to avoid left right left right loops
 IF ($lastmove = "right" and $penultimatemove = "left")
 
-  # If it has been left then right dont move left afain but escape from a loop
+  # If it has been left then right dont move left again but escape from a loop
   Goto(escape)
 
   # Reset the last move
   $lastmove = "none"
 
   # Else just move left
 ELSE
   LEFT($movementspeed,$turnamount)
 
   # Save the penultimate move
   $penultimatemove = $lastmove
 
   # Save the last move
   $lastmove= "left"
 
 ENDIF
 
   # Go back to the main script
 Return()
 
 # End of scripts