public void closeConnection()
{
instance = null;
port.Close();
}
/**
* start - Begins taking in CommandLine arguments and parses them
* into valid or invalid commands
*
* @return - none
**/
public void start()
{
// Start the Kinect
startKinect();
// Get the Arduino Port Object
port = COMConnection.COMConnection.Instance;
/*
// Get the current relative angle from the IMU
int startingDegree = getAverageCurrentLocationInDegrees();
int degreeDifference = getDegreeAddition(startingDegree, initialDirectionDegree);
Console.WriteLine("Degree IMU Needs to get to: " + degreeDifference);
// Perform the initial Swivel to get to the appropriate starting direction
// If less than 180 swivel right
if (initialDirectionDegree <= 180)
{
port.sendString("N");
port.sendString("30707");
System.Threading.Thread.Sleep(500); // Let the wheels get going
}
else
{
port.sendString("N");
port.sendString("40707");
System.Threading.Thread.Sleep(500); // Let the wheels get going
}
// Until we get to the right direction keep swiveling
while (!(withinDegrees(degreeDifference, startingDegree)))
{
startingDegree = getAverageCurrentLocationInDegrees();
Console.WriteLine("New Degree: " + startingDegree);
}
*/
// Stop, wait 0.5 seconds
port.sendString("N");
port.sendString("51010");
System.Threading.Thread.Sleep(500);
while (true)
{
//System.Threading.Thread.Sleep(5000);
// send mode
//port.sendString(Console.ReadLine());
// Make sure we are in Navigation Mode
port.sendString("N");
//System.Threading.Thread.Sleep(500);
// string myString = Console.ReadLine();
// send right ticks
// port.sendString(Console.ReadLine());
// send left ticks
//port.sendString(Console.ReadLine());
// Check for first threshhold
if (walkerKinect.isEmergency())
{
int turnResult = walkerKinect.isRightTurnBetter();
switch (turnResult)
{
case 1:
Console.WriteLine("Go Straight Slowly...");
goStraightSlowly();
break;
case 2:
Console.WriteLine("Stop, start turning LEFT!");
pivotLeft();
break;
case 3:
Console.WriteLine("Stop, start turning RIGHT!");
pivotRight();
break;
default:
Console.WriteLine("SWIVEL!");
//walkerKinect.setYPos(((leftEncoderTick + rightEncoderTick / 2) * mmPerEncoder) / 20);
//walkerKinect.printMap();
swivelRight();
break;
}
}
// Check for Second Threshold
else if (walkerKinect.isBlocked())
{
int turnResult = walkerKinect.isRightTurnBetter();
switch (turnResult)
{
case 1:
Console.WriteLine("Go Straight Slowly...");
goStraightSlowly();
break;
case 2:
Console.WriteLine("Start turning slowly LEFT!");
glideLeft();
break;
case 3:
Console.WriteLine("Start turning slowly RIGHT!");
glideRight();
break;
default:
Console.WriteLine("SWIVEL!");
//walkerKinect.setYPos(((leftEncoderTick + rightEncoderTick / 2) * mmPerEncoder) / 20);
//walkerKinect.printMap();
swivelRight();
break;
}
}
else
{
// If there are no problems just keep going straight
//Console.WriteLine("KEEP GOING STRAIGHT!");
goStraight();
}
//port.readLineString();
// Send the determined string to the Arduino for the motors
port.sendString(motorControlString);
System.Threading.Thread.Sleep(500);
//port.clearStream();
/*
string leftTick = "";
string rightTick = "";
port.sendString("F");
leftTick = port.readLineString();
//Console.WriteLine("LEFT: " + leftTick);
//leftEncoderTick = tick;
/*byte[] asciiBytes = Encoding.ASCII.GetBytes(leftTick);
Console.WriteLine("Left ASCII Values");
foreach (byte b in asciiBytes)
{
Console.Write(b);
}
* */
//Console.WriteLine();
//System.Threading.Thread.Sleep(200);
/* port.sendString("H");
rightTick = port.readLineString();
Console.WriteLine("Right Ticks: " + rightTick);
Console.WriteLine();
try
{
leftEncoderTick = long.Parse(leftTick);
}
catch (Exception e)
{
Console.WriteLine("ERROR LEFT");
}
try
{
rightEncoderTick = long.Parse(rightTick);
}
catch (Exception e)
{
Console.WriteLine("ERROR RIGHT");
}
Console.WriteLine("NEW TICKS");
Console.WriteLine("Left New: " + leftEncoderTick);
Console.WriteLine("Right New: " + rightEncoderTick);
// System.Threading.Thread.Sleep(500);
*/
}
// Close the connection to the Arduino
port.closeConnection();
}
public ForceCommand()
{
port = COMConnection.COMConnection.Instance;
}
public StrainCommand()
{
port = COMConnection.COMConnection.Instance;
}