public RobotTwoWheelState GetCurrentState(RobotTwoWheelCommand currentCommand) { RobotWheelModel rightW = new RobotWheelModel(rWheelPos, rWheelVel, rWheelAccel); RobotWheelModel leftW = new RobotWheelModel(lWheelPos, lWheelVel, lWheelAccel); RobotTwoWheelState state = new RobotTwoWheelState(curPose, currentCommand, rightW, leftW); return new RobotTwoWheelState(state, currentCommand); }
public RobotTwoWheelCommand GetCommand() { Random rand = new Random(); double lowestCost = double.MaxValue; double tempCost; RobotTwoWheelCommand lowestCommand, tempCommand; lowestCommand = new RobotTwoWheelCommand(0, 0); for (int i = 0; i < 1000; i++) { tempCommand = new RobotTwoWheelCommand(rand.NextDouble() * maxVelocity, rand.NextDouble() * 2 * maxTurn - maxTurn); tempCost = GetCost(tempCommand); if (tempCost < lowestCost) { lowestCommand = tempCommand; lowestCost = tempCost; } } Console.WriteLine("Command: " + lowestCommand.velocity + ", " + lowestCommand.turn + " Cost: " + lowestCost); return lowestCommand; }
public RobotTwoWheelState(RobotTwoWheelState toCopy, RobotTwoWheelCommand command) { //deep copy the class this.pose = new RobotPose(toCopy.pose); this.rightWheel = new RobotWheelModel(toCopy.rightWheel); this.leftWheel = new RobotWheelModel(toCopy.leftWheel); this.command = command; }
public RobotTwoWheelState(RobotTwoWheelState toCopy, RobotTwoWheelCommand command) { //deep copy the class this.pose = new RobotPose(toCopy.pose); this.rightWheel = new RobotWheelModel(toCopy.rightWheel); this.leftWheel = new RobotWheelModel(toCopy.leftWheel); this.command = command; }
public override bool Equals(object obj) { if (obj is RobotTwoWheelCommand == false) { return(false); } RobotTwoWheelCommand other = obj as RobotTwoWheelCommand; return(other.turn == this.turn && other.velocity == this.velocity); }
public RobotCommandMessage(int robotID, RobotCommandType type, List<Vector2> waypoints, RobotTwoWheelCommand directCmd, double angleWhenDone, RobotState.RobotMode mode) { this.robotID = robotID; Type = type; Waypoints = waypoints; DirectCmd = directCmd; AngleWhenDone = angleWhenDone; Mode = mode; }
public static RobotTwoWheelState Simulate(RobotTwoWheelCommand cmd, RobotTwoWheelState inititalState, double timestep) { //copy the initial state first! RobotTwoWheelState final = new RobotTwoWheelState(inititalState,cmd); double prevIntegratedLeft = inititalState.leftWheel.pos; double prevIntegratedRight = inititalState.rightWheel.pos; if (cmd.velocity > 2) cmd.velocity = 2; else if (cmd.velocity < -2) cmd.velocity = -2; if (cmd.turn > 360) cmd.turn = 360; else if (cmd.turn < -360) cmd.turn = -360; cmd.turn = cmd.turn * 0.5; double cmdLeftWheelVel = cmd.velocity - 0.5 * trackWidth * (cmd.turn * Math.PI / 180); double cmdRightWheelVel = cmd.velocity + 0.5 * trackWidth * (cmd.turn * Math.PI / 180); final.leftWheel.ApplyCommand(cmdLeftWheelVel, timestep); final.rightWheel.ApplyCommand(cmdRightWheelVel, timestep); //now figure out the updated pose given the new state of our robot.. double diffL = final.leftWheel.pos - prevIntegratedLeft; double diffR = final.rightWheel.pos - prevIntegratedRight; prevIntegratedLeft = final.leftWheel.pos; prevIntegratedRight = final.rightWheel.pos; //from lavalle's book ; http://planning.cs.uiuc.edu/node659.html //calculate the derivatives in measurements we have double xdot = ((diffL + diffR) / 2.0) * Math.Cos(inititalState.Pose.yaw); double ydot = ((diffL + diffR) / 2.0) * Math.Sin(inititalState.Pose.yaw); double headingDot = (diffR - diffL) / trackWidth; //update the state vector final.Pose.x = final.Pose.x + xdot; final.Pose.y = final.Pose.y + ydot; final.Pose.yaw = final.Pose.yaw + headingDot; while (final.Pose.yaw > Math.PI) final.Pose.yaw = final.Pose.yaw - 2 * Math.PI; while (final.Pose.yaw < -Math.PI) final.Pose.yaw = final.Pose.yaw + 2 * Math.PI; final.Pose.timestamp = final.Pose.timestamp + timestep; return final; }
public RobotState() { RobotID = 0; Mode = RobotMode.Starting; SubMode = RobotSubMode.None; Pose = new RobotPose(); rPose = new RobotPose(); PoseWatchdogFail = true; PlannerFail = false; InObstacle = false; WpInObstacle = false; IsLockedOn = false; NeutralizeStatus = RobotNeutralizeStatus.None; Waypoints = null; AngleWhenDone = 0; TrackedPath = null; SparsePath = null; Cmd = new RobotTwoWheelCommand(0, 0); CmdTime = DateTime.Now; WaypointsCompleted = 0; }
public RobotState(int robotID, RobotMode mode, RobotSubMode subMode, RobotPose pose, RobotPose rpose, bool poseWatchdogFail, bool plannerFail, bool inObstacle, bool wpInObstacle, bool isLockedOn, RobotNeutralizeStatus neutralizeStatus, List<Vector2> waypoints, double angleWhenDone, IPath path, IPath sparsePath, RobotTwoWheelCommand cmd, DateTime cmdTime, int waypointsCompleted) { RobotID = robotID; Mode = mode; SubMode = subMode; Pose = pose.DeepCopy(); rPose = rpose.DeepCopy(); PoseWatchdogFail = poseWatchdogFail; PlannerFail = plannerFail; InObstacle = inObstacle; WpInObstacle = wpInObstacle; IsLockedOn = isLockedOn; NeutralizeStatus = neutralizeStatus; Waypoints = new List<Vector2>(waypoints); AngleWhenDone = angleWhenDone; TrackedPath = path.Clone(); SparsePath = sparsePath.Clone(); Cmd = new RobotTwoWheelCommand(cmd.velocity, cmd.turn); CmdTime = new DateTime(cmdTime.Ticks); WaypointsCompleted = waypointsCompleted; }
public void SetVelocityTurn(RobotTwoWheelCommand command) { SetVelocityTurn(command, false); }
public RobotCommandMessage(int robotID, RobotCommandType type, RobotTwoWheelCommand cmd) { this.robotID = robotID; Type = type; DirectCmd = cmd; }
public RobotTwoWheelCommand GetCommand() { double g = 3; double zeta = .7; //parameters for STC double dt = .4; // adjust to change speed double deltadist = .10; //incremental distance for looking ahead // deltadist/dt = nominal speed along path //get trajectory // this step is performed outside of this function by BehavioralDirector //discretize trajectory into [xr, yr] //generate reference velocities at each trajectory point [vr, wr, thetar] if new path if (!pathsame) { this.GenrefVWHuniformdt(pathCurrentlyTracked, deltadist, dt); } //use current timestamp to find corresponding index in reference vectors //double currenttime = TimeSync.CurrentTime; double currenttime = (DateTime.Now.Ticks - 621355968000000000) / 10000000.0; int indextotrack = (int) (Math.Ceiling((currenttime - timestamps[0]) / dt)); if (indextotrack >= timestamps.Length) indextotrack = timestamps.Length - 1; Console.WriteLine("indextotrack = {0}", indextotrack); Console.WriteLine("time = {0}", currenttime); //unwrap thetar against thetarold, ignore if null (1337) if (!endpointsame) { thetarold = 1337; } if (thetarold != 1337) { while (hr[indextotrack] - thetarold >= Math.PI) { hr[indextotrack] -= 2 * Math.PI; ; } while (hr[indextotrack] - thetarold <= -Math.PI) { hr[indextotrack] += 2 * Math.PI; } } //set thetarold to thetar thetarold = hr[indextotrack]; double[] qrefdata = { xr[indextotrack], yr[indextotrack], hr[indextotrack] }; UMatrix qref = new UMatrix(3, 1, qrefdata); //print to qref to screen // use only for debugging //string qrefstring = qref.ToString(); //Console.WriteLine("QREF\n {0}", qrefstring); //establish pose vector q = [x, y, theta]; double x, y, theta; x = this.currentPoint.x; y = this.currentPoint.y; theta = this.currentPoint.yaw; // in radians double[] qdata = { x, y, theta }; UMatrix q = new UMatrix(3, 1, qdata); //unwrap pose against qref while (q[2, 0] - qref[2, 0] >= Math.PI) { q[2, 0] -= 2 * Math.PI; ; } while (q[2, 0] - qref[2, 0] <= -Math.PI) { q[2, 0] += 2 * Math.PI; } //establish kinematic state error UMatrix delq = new UMatrix(3, 1); delq = qref - q; //if (!endpointsame) { delqthetarold = 1337; } //if (delqthetarold != 1337) //{ // while (delq[2,0] - delqthetarold >= Math.PI) // { // delq[2,0] -= 2 * Math.PI; ; // } // while (delq[2,0] - delqthetarold <= -Math.PI) // { // delq[2,0] += 2 * Math.PI; // } //} ////set thetarold to thetar //delqthetarold = delq[2,0]; //print delq to screen // use only for debugging string delqstring = delq.ToString(); //Console.WriteLine("ERROR\n {0}", delqstring); //Console.ReadLine(); //create rotation matrix double[] Rotdata = { Math.Cos(theta), Math.Sin(theta), 0, -Math.Sin(theta), Math.Cos(theta), 0, 0, 0, 1 }; UMatrix Rot = new UMatrix(3, 3, Rotdata); //convert to robot frame UMatrix e = new UMatrix(3, 1); e = Rot * delq; //error in the robot reference frame: first element is along track error, second element is cross track error, last element is angle error string estring = e.ToString(); Console.WriteLine("ERROR\n {0}", estring); // calculate Uf = [vf, wf] double vf = vr[indextotrack] * Math.Cos(e[2, 0]); // in meters/sec double wf = wr[indextotrack] * 180 / Math.PI; // in degrees/sec Console.WriteLine("vf = {0}, wf = {1}",vf,wf); //calculate wn = sqrt(wr^2 + g*vr^2) double wn = Math.Sqrt(Math.Pow(wr[indextotrack], 2) + g * (Math.Pow(vr[indextotrack], 2))); //calculte STC gain matrix double k1, k2, k3; k1 = 2 * zeta * wn; k3 = k1; k2 = g * vr[indextotrack]; double[] Kdata = { k1, 0, 0, 0, k2, k3 }; UMatrix K = new UMatrix(2, 3, Kdata); //find control values Ub = K*e UMatrix Ub = new UMatrix(2, 1); Ub = K * e; Console.WriteLine("vb = {0}, wb = {1}", Ub[0, 0], Ub[1, 0]); //find total control u = Uf + Ub double vcommand = (Ub[0, 0] + vf) * 3.6509; // multiply by 3.6509 to get "segway units" double wcommand = (Ub[1, 0] * 180 / Math.PI + wf) * 3.9; // convert to deg/sec and multiply by 3.9 for "segway units" (counts/deg/sec) RobotTwoWheelCommand command = new RobotTwoWheelCommand(vcommand * 2.23693629, wcommand); if (Math.Abs(wcommand) >= 600) Console.WriteLine("W saturation"); //create text file of information if (indextotrack < timestamps.Length && counter < 15) { if (indextotrack == timestamps.Length - 1) { counter = counter + 1; } tw2.WriteLine("{0} {1} {2} {3} {4} {5} {6} {7} {8} {9} {10} {11} ", indextotrack, timestamps[indextotrack], x, y, e[0, 0], e[1, 0], e[2, 0], vf, wf, Ub[0, 0], Ub[1, 0]); } else { tw2.Close(); } return command; }
public RobotTwoWheelCommand GetCommand(ILidarScan<ILidar2DPoint> LidarScan, int angleTolerance, double threshold, double vmax, double wmax, out List<Polygon> polys) { // convert laser data into polar direction and magnitude //ScanToTextFile(LidarScan); //export scan data to analyze in matlab hysteresismin = threshold * .75; double[] magnitudes, betas, gammas; int n = LidarScan.Points.Count; magnitudes = new double[n]; betas = new double[n]; gammas = new double[n]; for (int ii = 0; ii < n; ii++) { if (LidarScan.Points[ii].RThetaPoint.R < dmax) { magnitudes[ii] = (a - Math.Pow(LidarScan.Points[ii].RThetaPoint.R, 2) * b); if (LidarScan.Points[ii].RThetaPoint.R < Rbloat) gammas[ii] = Math.PI / 2; else gammas[ii] = Math.Asin(Rbloat / LidarScan.Points[ii].RThetaPoint.R); } else { magnitudes[ii] = 0; gammas[ii] = Math.Asin(Rbloat / dmax); } double betaii = LidarScan.Points[ii].RThetaPoint.thetaDeg; while (betaii < 0) { betaii = betaii + 360; } while (betaii > 360) { betaii = betaii - 360; } betas[ii] = betaii; } // create polar histogram double[] hk = new double[numk]; double[] angles = new double[numk]; //corresponding angle for (int z = 0; z < numk; z++) { hk[z] = 0; angles[z] = z * alpha; } for (int j = 0; j < magnitudes.Count(); j++) { //int kmin = (int)(((betas[j] - gammas[j] + 360) % 360) / alpha); //int kmax = (int)(((betas[j] + gammas[j] + 360) % 360) / alpha); int k = (int)(betas[j] / alpha); //for (int k = kmin; k <= kmax; k++) hk[k] = hk[k] + magnitudes[j]; } for (int k = 0; k < numk; k++) { if (hkold[k] >= threshold && (hk[k] > hysteresismin && hk[k] < threshold)) hk[k] = hkold[k]; } hkold = hk; // smooth the histogram double[] hkprime = Smoother(hk, 5); /*for (int i = 0; i < numk; i++) Console.WriteLine(angles[i] + "," + hk[i] + "," + hkprime[i]); Console.WriteLine("END +++++++++++++++++++++++");*/ // Temporary building of polygons //List<Vector2> points = new List<Vector2>(); polys = new List<Polygon>(); /*for (int i = 0; i < numk; i++) { points.Add(new Vector2(hkprime[i] * Math.Cos(angles[i] * Math.PI / 180), hkprime[i] * Math.Sin(angles[i] * Math.PI / 180))); } polys.Add(new Polygon(points));*/ // find candidate valleys double goalangle = 180 * (goalpoint.ArcTan) / Math.PI; ktarget = (int)(Math.Round(goalangle / alpha)); List<double[]> valleys = FindValleys(hkprime); if (!trapped) { // pick out best candidate valley double[] valley = PickBestValley(valleys); // calculate steering angle double steerangle = FindSteerAngle(valley); //degrees // calculate speed commands double vmin = 0; // (m/s) //double vmax = .2; // (m/s) //double wmax = 25; // (deg/s) double w = 0; double v = 0; double steerwindow = angleTolerance; if (Math.Abs(steerangle) > steerwindow) { w = wmax * Math.Sign(steerangle); } else { w = wmax * (steerangle / steerwindow); } /*points = new List<Vector2>(); points.Add(new Vector2(0, 0)); points.Add(new Vector2(threshold * 1.5 * Math.Cos(steerangle * Math.PI / 180), threshold * 1.5 * Math.Sin(steerangle * Math.PI / 180))); polys.Add(new Polygon(points)); points = new List<Vector2>(); points.Add(new Vector2(0, 0)); points.Add(new Vector2(threshold * 1 * Math.Cos(goalangle * Math.PI / 180), threshold * 1 * Math.Sin(goalangle * Math.PI / 180))); polys.Add(new Polygon(points)); points = new List<Vector2>(); for (int i = 0; i < 180; i++) { points.Add(new Vector2(threshold * Math.Cos(i * 2 * Math.PI / 180), threshold * Math.Sin(i * 2 * Math.PI / 180))); } polys.Add(new Polygon(points));*/ double hc = Math.Min(hk[0], threshold); double vp = vmax * (1 - hc / threshold); v = vp * (1 - Math.Abs(w) / wmax) + vmin; double vcommand = v * 3.6509; // multiply by 3.6509 to get "segway units" double wcommand = w * 3.9; // convert to deg/sec and multiply by 3.9 for "segway units" (counts/deg/sec) //RobotTwoWheelCommand command = new RobotTwoWheelCommand(0, 0); RobotTwoWheelCommand command = new RobotTwoWheelCommand(vcommand * 2.23693629, wcommand); return command; } else // trapped (no valleys were found), so stop, or lift threshold { RobotTwoWheelCommand command = new RobotTwoWheelCommand(0, 0); return command; } }
public static RobotTwoWheelState Simulate(RobotTwoWheelCommand cmd, RobotTwoWheelState inititalState, double timestep) { //copy the initial state first! RobotTwoWheelState final = new RobotTwoWheelState(inititalState, cmd); double prevIntegratedLeft = inititalState.leftWheel.pos; double prevIntegratedRight = inititalState.rightWheel.pos; if (cmd.velocity > 2) { cmd.velocity = 2; } else if (cmd.velocity < -2) { cmd.velocity = -2; } if (cmd.turn > 360) { cmd.turn = 360; } else if (cmd.turn < -360) { cmd.turn = -360; } cmd.turn = cmd.turn * 0.5; double cmdLeftWheelVel = cmd.velocity - 0.5 * trackWidth * (cmd.turn * Math.PI / 180); double cmdRightWheelVel = cmd.velocity + 0.5 * trackWidth * (cmd.turn * Math.PI / 180); final.leftWheel.ApplyCommand(cmdLeftWheelVel, timestep); final.rightWheel.ApplyCommand(cmdRightWheelVel, timestep); //now figure out the updated pose given the new state of our robot.. double diffL = final.leftWheel.pos - prevIntegratedLeft; double diffR = final.rightWheel.pos - prevIntegratedRight; prevIntegratedLeft = final.leftWheel.pos; prevIntegratedRight = final.rightWheel.pos; //from lavalle's book ; http://planning.cs.uiuc.edu/node659.html //calculate the derivatives in measurements we have double xdot = ((diffL + diffR) / 2.0) * Math.Cos(inititalState.Pose.yaw); double ydot = ((diffL + diffR) / 2.0) * Math.Sin(inititalState.Pose.yaw); double headingDot = (diffR - diffL) / trackWidth; //update the state vector final.Pose.x = final.Pose.x + xdot; final.Pose.y = final.Pose.y + ydot; final.Pose.yaw = final.Pose.yaw + headingDot; while (final.Pose.yaw > Math.PI) { final.Pose.yaw = final.Pose.yaw - 2 * Math.PI; } while (final.Pose.yaw < -Math.PI) { final.Pose.yaw = final.Pose.yaw + 2 * Math.PI; } final.Pose.timestamp = final.Pose.timestamp + timestep; return(final); }
public RobotTwoWheelState(RobotPose initialPose, RobotTwoWheelCommand initialCommand, RobotWheelModel right, RobotWheelModel left) { this.pose = initialPose; this.command = initialCommand; rightWheel = new RobotWheelModel(right); leftWheel = new RobotWheelModel(left); }
private void PlanPurePursuit() { if (pathCurrentlyTracked == null) return; //we are really far off the path, just get on the stupid path //mark sucks and wants this behavior if (pathCurrentlyTracked.Length == 0) { goalPoint = pathCurrentlyTracked.EndPoint.pt; } else if (segmentCurrentlyTracked.DistanceOffPath(currentPoint.ToVector2()) > lookAheadDistParam / 2) { //Console.WriteLine("2"); double lookaheadRef = 0; PointOnPath pTemp = segmentCurrentlyTracked.StartPoint; goalPoint = pathCurrentlyTracked.AdvancePoint(pTemp, ref lookaheadRef).pt; } else { //Console.WriteLine("1"); //see if we can track the next segment and if so, update that... PointOnPath currentPointOnPath = segmentCurrentlyTracked.ClosestPoint(currentPoint.ToVector2()); double lookaheadRef = lookAheadDistParam; PointOnPath lookaheadPointOnPath = pathCurrentlyTracked.AdvancePoint(currentPointOnPath, ref lookaheadRef); //the path lookahead point is at the end, and we cant do antyhing segmentCurrentlyTracked = lookaheadPointOnPath.segment; goalPoint = lookaheadPointOnPath.pt; } double errorX = (goalPoint.X - currentPoint.x); double errorY = (goalPoint.Y - currentPoint.y); Vector2 verr = new Vector2(errorX, errorY); //Console.WriteLine(errorX + " | " + errorY); double errorDistance = verr.Length; double currentYaw = currentPoint.yaw; double errorYaw = UnwrapAndCalculateYawError(errorX, errorY, ref currentYaw); //Console.Write("neg Hyst: " + hysterisisNegative + " pos Hyst: " + hysterisisPositive + " yawError" + errorYaw * 180.0/ Math.PI+ " ||"); //the reason for this is that our angles are defined from 0 to 360 //but the PID controller expects angle to be -180 to 180 //calculate the control outputs //the idea here is we want to make the velocity (which is a derivative) be proportional to the error in our actual //position double unsignedYawErrorNormalizeToOne = Math.Abs(errorYaw) / Math.PI; double commandedVelocity = (kPPosition * errorDistance); double commandedHeading = kPHeading * errorYaw; if (Math.Abs(commandedVelocity) > capVelocity) commandedVelocity = Math.Sign(commandedVelocity) * capVelocity; if (Math.Abs(commandedHeading) > capHeadingDot) commandedHeading = Math.Sign(commandedHeading) * capHeadingDot; //if (unsignedYawErrorNormalizeToOne > .1) // find which input to use RobotTwoWheelCommand currentInput = inputList[pathCurrentlyTracked.IndexOf(segmentCurrentlyTracked)]; //commandedVelocity *= (1 - Math.Pow(unsignedYawErrorNormalizeToOne, velocityTurningDamingFactor)); //command = new RobotTwoWheelCommand(commandedVelocity, commandedHeading); if (pathCurrentlyTracked.EndPoint.pt.DistanceTo(currentPoint.ToVector2()) < .2) command = new RobotTwoWheelCommand(0, 0); else { if (currentInput.velocity < 0.3) command = new RobotTwoWheelCommand(0.4, commandedHeading); else command = new RobotTwoWheelCommand(currentInput.velocity, commandedHeading); } //if (reachedGoal) //{ // command.velocity = 0; // command.turn = 0; //} //Console.WriteLine("Current: " + currentPoint.x + " " + currentPoint.y + " " + currentPoint.yaw + " | " + errorDistance + " | " + errorYaw + " | " + commandedVelocity + " " + commandedHeading); //Console.WriteLine(); }
private double GetCost(RobotTwoWheelCommand command) { double lookAheadRemaining = lookAhead; double cost = 0; PointOnPath closestPoint = path.GetClosest(state.Pose.ToVector2()); PointOnPath lookAheadPoint = path.AdvancePoint(closestPoint, ref lookAheadRemaining); RobotTwoWheelState newState = RobotTwoWheelModel.Simulate(command, state, (lookAhead - lookAheadRemaining) / command.velocity); return newState.Pose.ToVector2().DistanceTo(lookAheadPoint.pt) * DISTANCE_WEIGHT;// - command.velocity * VELOCITY_WEIGHT - command.turn * TURN_WEIGHT; }
public TeleopMessage(int robotID, RobotTwoWheelCommand cmd) { this.robotID = robotID; this.cmd = cmd; }
public void SetVelocityTurn(RobotTwoWheelCommand command, bool inclined) { double velocity = command.velocity; double turn = command.turn; double cVelocityKp = .75; if (inclined) velocity = velocity + cVelocityKp * (velocity - ForwardVelocity*8.3333); //Console.WriteLine("V: " + velocity + " FV: " + ForwardVelocity*8.3333 + " E: " + (velocity - ForwardVelocity*8.3333)); /////////////////////////////////////////////////////////////////////////////////// //DO NOT CHANGE THESE VALUES WITHOUT ASKING MARK FIRST, THIS IS FOR SAFETY PURPOSES /////////////////////////////////////////////////////////////////////////////////// if (turn > 600) turn = 600; if (turn < -600) turn = -600; if (velocity > 3) velocity = 3; if (velocity < -1) velocity = -1; /////////////////////////////////////////////////////////////////////////////////// if (isBackwards) { velocity *= -1; } RMPControlMsg ctrl = new RMPControlMsg(velocity, turn); byte[] send = new byte[14]; send[0] = 0x00; //send to bridge send[1] = 0x17; //command send[2] = 0x00; //len (hi) send[3] = 0x0a; //len (lo) send[4] = 0x04; // can msg hi send[5] = 0x13; // can msg lo send[6] = (byte)(ctrl.velocityCommand >> 8); send[7] = (byte)(ctrl.velocityCommand & 0xFF); send[8] = (byte)(ctrl.turningCommand >> 8); send[9] = (byte)(ctrl.turningCommand & 0xFF); send[10] = (byte)((short)ctrl.configurationCommand >> 8); send[11] = (byte)((short)ctrl.configurationCommand & 0xFF); send[12] = (byte)(ctrl.configurationParameter >> 8); send[13] = (byte)(ctrl.configurationParameter & 0xFF); if (simMode) simRobotCommandServer.SendUnreliably(new SimMessage<RobotTwoWheelCommand>(NetworkAddress.GetSimRobotID("behavioral"), command, wheelPosTs)); else udpClient.Send(send, 14, new IPEndPoint(ipUnicast, controlPort)); }
public RobotTwoWheelCommand GetCommand(double transMax, double turnMax) { lock (followerLock) { if (pathCurrentlyTracked == null) { Console.WriteLine("Null path tracked!"); return new RobotTwoWheelCommand(0, 0); } //we are really far off the path, just get on the stupid path //mark sucks and wants this behavior if (pathCurrentlyTracked.Length == 0) //single waypoint case { goalPoint = pathCurrentlyTracked.EndPoint.pt; startPoint = pathCurrentlyTracked.EndPoint.pt; } //else if (segmentCurrentlyTracked.DistanceOffPath(currentPoint.ToVector2()) > lookAheadDistParam / 2) //{ // //Console.WriteLine("2"); // double lookaheadRef = 0; // PointOnPath pTemp = segmentCurrentlyTracked.StartPoint; // goalPoint = pathCurrentlyTracked.AdvancePoint(pTemp, ref lookaheadRef).pt; //} else { //Console.WriteLine("1"); //see if we can track the next segment and if so, update that... PointOnPath currentPointOnPath = segmentCurrentlyTracked.ClosestPoint(currentPoint.ToVector2()); double lookaheadRef = lookAheadDistParam; PointOnPath lookaheadPointOnPath = pathCurrentlyTracked.AdvancePoint(currentPointOnPath, ref lookaheadRef); if (segmentCurrentlyTracked != lookaheadPointOnPath.segment) segmentCurrentlyTracked = lookaheadPointOnPath.segment; goalPoint = lookaheadPointOnPath.pt; startPoint = currentPointOnPath.pt; } // Calculate errors double errorX = (goalPoint.X - currentPoint.x); double errorY = (goalPoint.Y - currentPoint.y); Vector2 verr = new Vector2(errorX, errorY); double tangentX = (goalPoint.X - startPoint.X); double tangentY = (goalPoint.Y - startPoint.Y); Vector2 tangent = new Vector2(tangentX, tangentY); double errorDistance = currentPoint.ToVector2().DistanceTo(startPoint); double currentYaw = currentPoint.yaw; double tempCurrentYaw = currentPoint.yaw; errorYawTangent = UnwrapAndCalculateYawError(tangentX, tangentY, ref tempCurrentYaw); errorYaw = UnwrapAndCalculateYawError(errorX, errorY, ref currentYaw); double tangentEquation = (goalPoint.Y - startPoint.Y) * currentPoint.x / (goalPoint.X - startPoint.Y) - (goalPoint.Y - startPoint.Y) * goalPoint.X / (goalPoint.X - startPoint.Y) + startPoint.Y; //Console.Clear(); //Console.WriteLine("Current yaw is: " + currentYaw); if (tangentEquation < currentPoint.y) { if (Math.PI/2 <= Math.Abs(currentYaw) && Math.Abs(currentYaw) <= Math.PI) { //Console.WriteLine("Above line, pointing left"); errorDistance *= -1; } //else Console.WriteLine("Above line, pointing right"); } else { if (0 <= Math.Abs(currentYaw) && Math.Abs(currentYaw) <= Math.PI/2) { //Console.WriteLine("Below line, pointing right"); errorDistance *= -1; } //else Console.Write("Below line, pointing left"); } //if we are really close to last waypoint, make the robot just stop if ((segmentCurrentlyTracked == pathCurrentlyTracked[pathCurrentlyTracked.Count - 1]) && (PathCurrentlyTracked.EndPoint.pt - currentPoint.ToVector2()).Length < lookAheadDistParam) { command = new RobotTwoWheelCommand(0, 0); //Console.WriteLine("Acheived!"); } else { //the idea here is we want to make the velocity (which is a derivative) be proportional to the error in our actual //position double unsignedYawErrorNormalizeToOne = Math.Abs(errorYaw) / Math.PI; double velocityPercentage = Math.Abs((Math.PI - Math.Abs(errorYawTangent)) / Math.PI); double commandedVelocity = (velocityPercentage < 0.5) ? 0.0 : transMax * velocityPercentage; //double commandedHeading = 200 * errorYawTangent + 100 * errorYaw + 300 * errorDistance; double commandedHeading = 150 * errorYawTangent + 100 * errorYaw + 200 * errorDistance; if (Math.Abs(commandedVelocity) > transMax) commandedVelocity = Math.Sign(commandedVelocity) * transMax; if (Math.Abs(commandedHeading) > turnMax) { commandedHeading = Math.Sign(commandedHeading) * turnMax; commandedVelocity = 0.0; } /*if (unsignedYawErrorNormalizeToOne > .1) commandedVelocity *= (1 - Math.Pow(unsignedYawErrorNormalizeToOne, velocityTurningDamingFactor));*/ command = new RobotTwoWheelCommand(commandedVelocity, commandedHeading); //Console.WriteLine(errorYaw + ", " + errorYawTangent + ", " + errorDistance); } return command; } }
public RobotTwoWheelState(RobotPose initialPose, RobotTwoWheelCommand initialCommand, RobotWheelModel right, RobotWheelModel left) { this.pose = initialPose; this.command = initialCommand; rightWheel = new RobotWheelModel(right); leftWheel = new RobotWheelModel(left); }
public void SetVelocityTurn(RobotTwoWheelCommand command) { double velocity = command.velocity; double turn = command.turn; byte direction = 0x3b;//moving forward /////////////////////////////////////////////////////////////////////////////////// //DO NOT CHANGE THESE VALUES WITHOUT ASKING MARK FIRST, THIS IS FOR SAFETY PURPOSES /////////////////////////////////////////////////////////////////////////////////// if (turn > 600) turn = 600; if (turn < -600) turn = -600; if (velocity > 3) velocity = 3; if (velocity < -1) velocity = -1; /////////////////////////////////////////////////////////////////////////////////// if (isBackwards) { velocity *= -1; direction = 0x1b; } double rotational_term = Math.PI/180*turn/7.8; double leftvel = (command.velocity - rotational_term); double rightvel = (command.velocity + rotational_term); if (leftvel > 0x50) leftvel = 0x50; if (rightvel > 0x50) rightvel = 0x50; int pioneerVelocity = (int)(command.velocity*3.04108844*15);//convert counts to mm/sec RMPControlMsg ctrl = new RMPControlMsg(velocity, turn); byte[] send = new byte[14]; send[0] = 0x01; //send to pioneer send[1] = 0x00; //blank send[2] = 0x00; //len (hi) send[3] = 0x3; //len (lo) for bridge send[4] = 0xfa; // Pioneer start byte 1 send[5] = 0xfb; // Pioneer start byte 2 send[6] = 0x6; // pioneer packet length send[7] = 32; // pioneer command send[8] = 0x3b; // args type send[9] = (byte)rightvel; // high byte of vel send[10] = (byte)leftvel; send[11] = (byte)((checkSum(send)) >> 8); send[12] = (byte)((checkSum(send)) & 0xff);// checksum (excluding startbytes+command) SendData(send, 13); System.Threading.Thread.Sleep(1000); /* send[0] = 0x00; //send to bridge send[1] = 0x17; //command send[2] = 0x00; //len (hi) send[3] = 0x0a; //len (lo) send[4] = 0x04; // can msg hi send[5] = 0x13; // can msg lo send[6] = (byte)(ctrl.velocityCommand >> 8); send[7] = (byte)(ctrl.velocityCommand & 0xFF); send[8] = (byte)(ctrl.turningCommand >> 8); send[9] = (byte)(ctrl.turningCommand & 0xFF); send[10] = (byte)((short)ctrl.configurationCommand >> 8); send[11] = (byte)((short)ctrl.configurationCommand & 0xFF); send[12] = (byte)(ctrl.configurationParameter >> 8); send[13] = (byte)(ctrl.configurationParameter & 0xFF); send[0] = 0x01; //send to pioneer send[1] = 0x00; //command send[2] = 0x00; //len (hi) send[3] = 0x3; //len (lo) send[4] = 0xfa; // can msg hi send[5] = 0xfb; // can msg lo send[6] = 0x5; // can msg lo send[7] = 0xb; // can msg lo send[8] = 0xa; // can msg lo send[9] = 0xa; // can msg lo send[10] = (byte)((checkSum(send)) >> 8); send[11] = (byte)((checkSum(send)) & 0xff);// checksum (excluding startbytes+command)*/ /* MoveMotors(send); if (simMode) simRobotCommandServer.SendUnreliably(new SimMessage<RobotTwoWheelCommand>(NetworkAddress.GetSimRobotID("behavioral"), command, wheelPosTs)); else udpClient.Send(send, 12, new IPEndPoint(ipUnicast, controlPort));*/ }