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