public static void DriveBackwards()
{
backwards = true;
foreach (Motor m in xMotors)
{
changingMotor = m;
new Thread(new DirectMotorLink().RunMotor).Start();
System.Threading.Thread.Sleep(100);
}
}
public void boxTest()
{
//get all the connected motors and instantiate them
List <DeviceListItem> devices = Jrk.getConnectedDevices();
Motor current;
foreach (DeviceListItem d in devices)
{
System.Console.WriteLine("Checking device " + d.serialNumber);
//if the found device matches the expected serial number, proceed with initialization
foreach (MotorInfo info in HandCodeMotorConstants.handCodedMotors)
{
if (info.serial.Equals(d.serialNumber))
{
System.Console.WriteLine("Found motor " + info.serial);
current = new Motor(info, new Jrk(d));
motors.Add(current);
if (info.horzTheta < 0.1 && info.horzTheta > -0.1 && info.vertTheta < 0.1 && info.vertTheta > -0.1)
{
xMotors.Add(current);
}
if (info.horzTheta < 1.1 && info.horzTheta > 0.9 && info.vertTheta < 0.1 && info.vertTheta > -0.1)
{
yMotors.Add(current);
}
if (info.horzTheta < 0.1 && info.horzTheta > -0.1 && info.vertTheta < 1.1 && info.vertTheta > 0.9)
{
zMotors.Add(current);
}
}
}
}
int time = 2000;//how many milliseconds to run for
//wait before starting tests
//System.Threading.Thread.Sleep(time);
//System.Console.WriteLine("Starting box drive test");
//System.Console.WriteLine("Diving.");
//dive();
//System.Threading.Thread.Sleep(time);
//foreach (Motor m in motors)
//{
// System.Console.WriteLine("Stopping motor " + m.info.serial + ".");
// m.KillMotors();
//}
//System.Console.WriteLine("Forward.");
//driveForward();
//System.Threading.Thread.Sleep(time);
//foreach (Motor m in motors)
//{
// System.Console.WriteLine("Stopping motor " + m.info.serial + ".");
// m.KillMotors();
//}
//System.Console.WriteLine("Backward.");
//driveBackwards();
//System.Threading.Thread.Sleep(time);
//foreach (Motor m in motors)
//{
// System.Console.WriteLine("Stopping motor " + m.info.serial + ".");
// m.KillMotors();
//}
ConsoleKey key = ConsoleKey.P;
System.Console.WriteLine("Ready for input");
while (key != ConsoleKey.Q)
{
key = Console.ReadKey().Key;
time = 2000;//in milliseconds
switch (key)
{
case ConsoleKey.W:
System.Console.WriteLine("\nDriving Forward");
driveForward();
break;
case ConsoleKey.S:
System.Console.WriteLine("\nDriving Backwards");
driveBackwards();
break;
case ConsoleKey.R:
System.Console.WriteLine("\nSurfacing");
surface();
break;
case ConsoleKey.F:
System.Console.WriteLine("\nDiving");
dive();
break;
case ConsoleKey.P:
System.Console.WriteLine("\nKilling Peasants");
foreach (Motor m in motors)
{
System.Console.WriteLine("Stopping motor " + m.info.serial + ".");
m.KillMotors();
}
break;
}
}
//System.Console.WriteLine("Surface.");
//surface();
//System.Threading.Thread.Sleep(time);
//foreach (Motor m in motors)
//{
// System.Console.WriteLine("Stopping motor " + m.info.serial + ".");
// m.KillMotors();
//}
//System.Console.WriteLine("Press Enter to quit.");
//System.Console.ReadLine();
}
public static void DriveForward()
{
foreach (Motor m in xMotors)
{
changingMotor = m;
backwards = false;
new Thread(new DirectMotorLink().RunMotor).Start();
System.Threading.Thread.Sleep(100);
}
}
/// <summary>
/// Runs the given motor several random different speeds.
/// </summary>
/// <param name="m"></param>
private void TestMotor(Motor m)
{
m.info.controller.setJrkParameter(jrkParameter.PARAMETER_FEEDBACK_MODE, 0);
m.info.controller.reinitialize();
System.Console.WriteLine("Status:");
foreach (String s in m.GetStatus())
{
System.Console.WriteLine(s);
}
Random rng = new Random();
sbyte speed;
//for (int i = 0; i < 5; i++)
{
//speed = (sbyte)(rng.Next(101));//only test half speed max
speed = -30;
if (backwards)
{
speed *= -1;
}
m.RequestSpeed(speed);
System.Console.WriteLine("Target speed: " + (2047 + speed * 20) + " ");
//System.Threading.Thread.Sleep(10000000);
List<String> output = m.GetStatus();
System.Console.WriteLine("Motor target: " + output.ElementAt(0) + " current: " + output.ElementAt(1)
+ " duty cycle: " + output.ElementAt(2));
System.Console.WriteLine();
}
//m.RequestSpeed(0);//set motor to stop
//stop the motor when done testing
// m.KillMotors();
}
private static bool LostPower(Motor m)
{
//return m.info.controller.getVariables().current <= 0;
return (m.info.controller.getVariables().errorOccurredBits >> 1) % 2 == 1;//this will work if the error flag is the second bit of the errorOrcurredBits
}
private void RunMotor(Motor m)
{
if (killed)
{
return;
}
m.info.controller.setJrkParameter(jrkParameter.PARAMETER_FEEDBACK_MODE, 0);
m.info.controller.reinitialize();
sbyte speed;
{
speed = -25;
if (backwards)
{
speed *= -1;
}
m.RequestSpeed(speed);
}
}
public void RunTests()
{
//get all the connected motors and instantiate them
List<DeviceListItem> devices = Jrk.getConnectedDevices();
foreach (DeviceListItem d in devices)
{
System.Console.WriteLine("Checking device " + d.serialNumber);
//if the found device matches the expected serial number, proceed with initialization
foreach (MotorInfo info in HandCodeMotorConstants.handCodedMotors)
{
if (info.serial.Equals(d.serialNumber))
{
System.Console.WriteLine("Found motor " + info.serial);
motors.Add(new Motor(info, new Jrk(d)));
}
}
}
//run random tests
// do
{
foreach (Motor m in motors)
{
System.Console.WriteLine("Testing motor " + m.info.serial + ".");
changingMotor = m;
//TestMotor(m);
new Thread(new DirectMotorLink().TestMotor).Start();
System.Console.WriteLine();
System.Threading.Thread.Sleep(500);
if (System.Console.KeyAvailable)
{
break;
}
}
System.Threading.Thread.Sleep(10000);
}// while (!System.Console.KeyAvailable);
System.Console.WriteLine("Press Enter to quit.");
System.Console.ReadLine();
}
public void singleMotorTest()
{
InitializeMotors();
long time;
int numberOfMotors = motors.Count;
int selection;
char key = 'P';
System.Console.WriteLine();
System.Console.WriteLine("Single Motor Control Test");
System.Console.WriteLine("Type motor index to start that motor.");
System.Console.WriteLine("Type 'q' to quit program.");
System.Console.WriteLine("Type any other key to stop all motors.");
while (key != 'q')
{
key = Console.ReadKey(true).KeyChar;
time = 2000;//in milliseconds
if (Int32.TryParse(key.ToString(), out selection))
{
System.Console.WriteLine("Index selected: " + selection);
if (selection >= motors.Count)
{
continue;
}
//fire selected motor
changingMotor = motors[selection];
//TestMotor(m);
new Thread(new DirectMotorLink().TestMotor).Start();
System.Console.WriteLine();
System.Console.WriteLine("Serial: " + motors[selection].info.serial);
System.Threading.Thread.Sleep(500);
if (System.Console.KeyAvailable)
{
break;
}
}
else
{
KillAllMotors();
}
}
foreach (Motor m in motors)
{
System.Console.WriteLine("Stopping motor " + m.info.serial + ".");
m.KillMotors();
}
}
public static void Surface()
{
backwards = false;
foreach (Motor m in zMotors)
{
changingMotor = m;
new Thread(new DirectMotorLink().RunMotor).Start();
System.Threading.Thread.Sleep(100);
}
}
public static void YawLeft()
{
foreach (Motor m in yawMotors)
{
changingMotor = m;
backwards = true;
new Thread(new DirectMotorLink().RunMotor).Start();
System.Threading.Thread.Sleep(100);
}
}
public static void PitchUp()
{
foreach (Motor m in zMotors)
{
changingMotor = m;
if (m.info.xPos > 0)
{
backwards = false;
}
else
{
backwards = true;
}
new Thread(new DirectMotorLink().RunMotor).Start();
System.Threading.Thread.Sleep(100);
}
}
public static void InitializeMotors()
{
//get all the connected motors and instantiate them
List<DeviceListItem> devices = Jrk.getConnectedDevices();
Motor current;
foreach (DeviceListItem d in devices)
{
System.Console.WriteLine("Checking device " + d.serialNumber);
//if the found device matches the expected serial number, proceed with initialization
foreach (MotorInfo info in HandCodeMotorConstants.handCodedMotors)
{
if (info.serial.Equals(d.serialNumber))
{
System.Console.WriteLine("Found motor " + info.serial);
current = new Motor(info, new Jrk(d));
motors.Add(current);
if (info.horzTheta == 0 && info.vertTheta == 0)
{
xMotors.Add(current);
}
if (info.horzTheta != 0 && info.vertTheta == 0)
{
yMotors.Add(current);
}
if (info.horzTheta == 0 && info.vertTheta != 0)
{
zMotors.Add(current);
}
//yaw if horizontal angle
}
}
}
}
public void boxTest()
{
//get all the connected motors and instantiate them
List<DeviceListItem> devices = Jrk.getConnectedDevices();
Motor current;
foreach (DeviceListItem d in devices)
{
System.Console.WriteLine("Checking device " + d.serialNumber);
//if the found device matches the expected serial number, proceed with initialization
foreach (MotorInfo info in HandCodeMotorConstants.handCodedMotors)
{
if (info.serial.Equals(d.serialNumber))
{
System.Console.WriteLine("Found motor " + info.serial);
current = new Motor(info, new Jrk(d));
motors.Add(current);
if (info.horzTheta < 0.1 && info.horzTheta > -0.1 && info.vertTheta < 0.1 && info.vertTheta > -0.1)
{
xMotors.Add(current);
}
if (info.horzTheta < 1.1 && info.horzTheta > 0.9 && info.vertTheta < 0.1 && info.vertTheta > -0.1)
{
yMotors.Add(current);
}
if (info.horzTheta < 0.1 && info.horzTheta > -0.1 && info.vertTheta < 1.1 && info.vertTheta > 0.9)
{
zMotors.Add(current);
}
}
}
}
int time = 2000;//how many milliseconds to run for
//wait before starting tests
//System.Threading.Thread.Sleep(time);
//System.Console.WriteLine("Starting box drive test");
//System.Console.WriteLine("Diving.");
//dive();
//System.Threading.Thread.Sleep(time);
//foreach (Motor m in motors)
//{
// System.Console.WriteLine("Stopping motor " + m.info.serial + ".");
// m.KillMotors();
//}
//System.Console.WriteLine("Forward.");
//driveForward();
//System.Threading.Thread.Sleep(time);
//foreach (Motor m in motors)
//{
// System.Console.WriteLine("Stopping motor " + m.info.serial + ".");
// m.KillMotors();
//}
//System.Console.WriteLine("Backward.");
//driveBackwards();
//System.Threading.Thread.Sleep(time);
//foreach (Motor m in motors)
//{
// System.Console.WriteLine("Stopping motor " + m.info.serial + ".");
// m.KillMotors();
//}
ConsoleKey key = ConsoleKey.P;
System.Console.WriteLine("Ready for input");
while (key != ConsoleKey.Q)
{
key = Console.ReadKey().Key;
time = 2000;//in milliseconds
switch (key)
{
case ConsoleKey.W:
System.Console.WriteLine("\nDriving Forward");
driveForward();
break;
case ConsoleKey.S:
System.Console.WriteLine("\nDriving Backwards");
driveBackwards();
break;
case ConsoleKey.R:
System.Console.WriteLine("\nSurfacing");
surface();
break;
case ConsoleKey.F:
System.Console.WriteLine("\nDiving");
dive();
break;
case ConsoleKey.P:
System.Console.WriteLine("\nKilling Peasants");
foreach (Motor m in motors)
{
System.Console.WriteLine("Stopping motor " + m.info.serial + ".");
m.KillMotors();
}
break;
}
}
//System.Console.WriteLine("Surface.");
//surface();
//System.Threading.Thread.Sleep(time);
//foreach (Motor m in motors)
//{
// System.Console.WriteLine("Stopping motor " + m.info.serial + ".");
// m.KillMotors();
//}
//System.Console.WriteLine("Press Enter to quit.");
//System.Console.ReadLine();
}
public void surface()
{
backwards = true;
foreach (Motor m in zMotors)
{
// System.Console.WriteLine("Testing motor " + m.info.serial + ".");
testing = m;
//TestMotor(m);
new Thread(new DirectMotorLinkTest().TestMotor).Start();
System.Console.WriteLine();
System.Threading.Thread.Sleep(500);
if (System.Console.KeyAvailable)
{
break;
}
}
System.Threading.Thread.Sleep(1000);
}
private static bool LostPower(Motor m)
{
//return m.info.controller.getVariables().current <= 0;
return((m.info.controller.getVariables().errorOccurredBits >> 1) % 2 == 1); //this will work if the error flag is the second bit of the errorOrcurredBits
}
