/// <summary>
 /// Wait for drop command
 /// </summary>
 void WaitDrop()
 {
     if (TargetDropped)
     {
         PayloadDropCount++;
         DebugMessage.Clear();
         DebugMessage.Append(PayloadDropCount + "/" + PayloadDropTimeout);
         if (PayloadDropCount > PayloadDropTimeout)
         {
             State = DriveState.VerifyTarget;
         }
     }
     else
     {
         PayloadDropCount = 0;
     }
     Speed    = 0;
     Steering = 0;
 }
Exemple #2
0
 WayPoint obstacleAvoidance(WayPoint nextWaypoint, WayPoint currentLocation)
 {
     // Everything is in meters and radians
     if (Settings.UseVision)
     {
         if (AvoidanceVector.magnitude > 0)
         {
             SumVector             = new Vector2d(currentLocation, nextWaypoint);
             SumVector.magnitude  /= SumVector.magnitude;                           //Normalize(Check if normalize is needed)
             SumVector.magnitude  *= Alpha;                                         // Influence(Check if maxspeed is needed)
             AvoidanceVector.angle = (temp.angle + (45 * Math.PI / 180) + Heading); // add heading + 90Make Lidar data relative to robot position
             DebugMessage.Clear();
             DebugMessage.Append((AvoidanceVector.angle * 180 / Math.PI));
             SumVector          += AvoidanceVector;                                      //combine Avoidance and Attraction vector
             SumVector.magnitude = Math.Max(ReachWaypointZone + 1, SumVector.magnitude); //set the minimum vector length to 4 meters
             nextWaypoint        = WayPoint.Projection(currentLocation, SumVector.angle, SumVector.magnitude);
         }
     }
     return(nextWaypoint);
 }