/// <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; }
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); }