public static void Main(string[] args)
{
if (args != null && args.Length > 0)
{
SERVER_IP = args[0];
Console.WriteLine("Using argument Specified IP of: " + SERVER_IP);
}
else
{
Console.WriteLine("Using default IP of: " + SERVER_IP);
}
Quit = false;
InitBeagleBone();
SetupClient();
MotorControl.Initialize();
//MotorBoards.Initialize(CANBBB.CANBus0);
int count = 0;
Console.WriteLine("Finished the initalize");
do
{
Console.WriteLine("Looping");
SendSensorData(count);
ProcessInstructions();
Thread.Sleep(50);
count++;
if (count == 101)
{
count = 0;
}
} while (!Quit);
}
public static void ProcessBasePackets()
{
for (int i = 0; !DrivePackets.IsEmpty() && i < NUM_PACKETS_TO_PROCESS; i++)
{
Packet p = DrivePackets.Dequeue();
switch ((PacketID)p.Data.ID)
{
case PacketID.RPMAllDriveMotors:
MotorControl.SetAllRPM((sbyte)p.Data.Payload[0]);
break;
case PacketID.RPMFrontRight:
case PacketID.RPMFrontLeft:
case PacketID.RPMBackRight:
case PacketID.RPMBackLeft:
int MotorID = p.Data.ID - (byte)PacketID.RPMFrontRight;
MotorControl.SetRPM(MotorID, (sbyte)p.Data.Payload[1]);
break;
case PacketID.RPMSteeringMotor:
float SteerSpeed = UtilData.ToFloat(p.Data.Payload);
//MotorControl.SetSteerSpeed(SteerSpeed);
break;
case PacketID.SteerPosition:
float Position = UtilData.ToFloat(p.Data.Payload);
//MotorControl.SetRackAndPinionPosition(Position);
break;
case PacketID.SpeedAllDriveMotors:
float Speed = UtilData.ToFloat(p.Data.Payload);
MotorControl.SetAllSpeed(Speed);
break;
case PacketID.BaseSpeed:
case PacketID.ShoulderSpeed:
case PacketID.ElbowSpeed:
case PacketID.WristSpeed:
case PacketID.DifferentialVert:
case PacketID.DifferentialRotate:
case PacketID.HandGrip:
byte address = (byte)(p.Data.ID - 0x8A);
byte direction = 0x00;
if (p.Data.Payload[0] > 0)
{
direction = 0x01;
}
UtilCan.SpeedDir(CANBBB.CANBus0, false, 0x02, address, p.Data.Payload[1], direction);
break;
}
}
}
public static void ProcessPathPackets()
{
for (int i = 0; !PathPackets.IsEmpty() && i < NUM_PACKETS_TO_PROCESS; i++)
{
Packet p = PathPackets.Dequeue();
switch ((PacketID)p.Data.ID)
{
// TODO Maybe: Combine pathing speed and turn in same packet???
case PacketID.PathingSpeed:
PathSpeed = UtilData.ToFloat(p.Data.Payload);
MotorControl.SkidSteerDriveSpeed(PathSpeed, PathAngle);
break;
case PacketID.PathingTurnAngle:
PathAngle = UtilData.ToFloat(p.Data.Payload);
MotorControl.SkidSteerDriveSpeed(PathSpeed, PathAngle);
break;
}
}
}
public static void Main(string[] args)
{
if (args.Length > 0)
{
if (args[0].Equals("turn"))
{
turnMode = true;
}
else if (args[0].Equals("line"))
{
lineMode = true;
}
}
double SetTurn = 0;
double PTurn = 1;
double ITurn = 0;
double DTurn = 0;
double SetDis = 1;
double PDis = 1;
double IDis = 0;
double DDis = 0;
UdpClient udpServer = new UdpClient(9000);
IPEndPoint remoteEP = new IPEndPoint(IPAddress.Any, 9000);
/*
* try
* { // Open the text file using a stream reader.
* using (StreamReader sr = new StreamReader("PIDValues.txt"))
* {
* // Read the stream to a string, and write the string to the console.
* sr.ReadLine();
* sr.ReadLine(); // Ignore first two lines
* SetTurn = Convert.ToDouble(sr.ReadLine());
* PTurn = Convert.ToDouble(sr.ReadLine());
* ITurn = Convert.ToDouble(sr.ReadLine());
* DTurn = Convert.ToDouble(sr.ReadLine());
* sr.ReadLine();
* SetDis = Convert.ToDouble(sr.ReadLine());
* PDis = Convert.ToDouble(sr.ReadLine());
* IDis = Convert.ToDouble(sr.ReadLine());
* DDis = Convert.ToDouble(sr.ReadLine());
* }
* }
* catch (Exception e)
* {
* Console.WriteLine("PIDValues.txt is either missing or in improper format:");
* Console.WriteLine(e.Message);
* }
*/
PID directionPID = new PID(PTurn, ITurn, DTurn);
directionPID.SetTarget(SetTurn);
PID distancePID = new PID(PDis, IDis, DDis);
directionPID.SetTarget(SetDis);
Quit = false;
InitBeagleBone();
//SetupClient();
MotorControl.Initialize();
MotorBoards.Initialize(CANBBB.CANBus0);
int count = 0;
do
{
//SendSensorData(count);
//ProcessInstructions();
followTennisBall(udpServer, remoteEP, directionPID, distancePID);
Thread.Sleep(50);
count++;
if (count == 101)
{
count = 0;
}
} while (!Quit);
}
public static void followTennisBall(UdpClient udpServer, IPEndPoint remoteEP, PID directionPID, PID distancePID)
{
// Get data from UDP. Convert it to a String
// Note this is a blocking call.
var data = udpServer.Receive(ref remoteEP);
String dataString = Encoding.ASCII.GetString(data);
Console.WriteLine(dataString);
// Set the wheel values to be zero for now
Double leftWheel = 0;
Double rightWheel = 0;
// Make sure the data received is not an empty string
int realRightSpeed = 0;
int realLeftSpeed = 0;
if (!String.IsNullOrEmpty(dataString))
{
// Split the String into distance and direction
// Is in format "Distance,Direction"
// Distance is from 1 to 150 ish
// Direction is from -10 to 10 degrees
String[] speedDis = dataString.Split(',');
Double distance = Convert.ToDouble(speedDis[0]);
Double direction = (Convert.ToDouble(speedDis[1]));
Double inverse_distance = 30 - distance;
if (inverse_distance < 0)
{
inverse_distance = 0;
}
// If mode is set to not turn only the run
if (!turnMode)
{
distancePID.Feed(inverse_distance);
if (!Double.IsNaN(distancePID.Output))
{
rightWheel += distancePID.Output;
leftWheel += distancePID.Output;
}
}
if (!lineMode)
{
directionPID.Feed(direction);
if (!Double.IsNaN(directionPID.Output))
{
rightWheel += directionPID.Output;
leftWheel -= directionPID.Output;
}
}
realLeftSpeed = -(int)rightWheel;
realRightSpeed = -(int)leftWheel;
if (realLeftSpeed > 45)
{
realLeftSpeed = 45;
}
else if (realLeftSpeed < -45)
{
realLeftSpeed = -45;
}
if (realRightSpeed > 45)
{
realRightSpeed = 45;
}
else if (realRightSpeed < -45)
{
realRightSpeed = -45;
}
if (distance > 30)
{
realLeftSpeed = 0;
realRightSpeed = 0;
}
Console.WriteLine(" Distance: " + distance + ", Turning: " + direction);
Console.WriteLine("Leftwheel: " + realLeftSpeed + ", Rightwheel: " + realRightSpeed);
Console.WriteLine("Leftwheel: " + (sbyte)realLeftSpeed + ", Rightwheel: " + (sbyte)realRightSpeed);
}
// If data received is not a full string, stop motors.
else
{
leftWheel = 0;
rightWheel = 0;
Console.WriteLine("no values recieved");
}
MotorControl.SetRPM(0, (sbyte)realRightSpeed);
MotorControl.SetRPM(2, (sbyte)realRightSpeed);
MotorControl.SetRPM(1, (sbyte)realLeftSpeed);
MotorControl.SetRPM(3, (sbyte)-realLeftSpeed);
//Motor[0].SetSpeed((float)(rightWheel));
//Motor[1].SetSpeed((float)(leftWheel));
}
public static void ProcessBasePackets()
{
for (int i = 0; !DrivePackets.IsEmpty() && i < NUM_PACKETS_TO_PROCESS; i++)
{
Console.WriteLine("Processing Base Packets");
Packet p = DrivePackets.Dequeue();
switch ((PacketID)p.Data.ID)
{
//case PacketID.RPMAllDriveMotors:
// MotorControl.SetAllRPM((sbyte)p.Data.Payload[0]);
// break;
case PacketID.RPMFrontRight:
case PacketID.RPMFrontLeft:
case PacketID.RPMBackRight:
case PacketID.RPMBackLeft:
int MotorID = p.Data.ID - (byte)PacketID.RPMFrontRight;
MotorControl.SetRPM(MotorID, (sbyte)p.Data.Payload[1]);
break;
case PacketID.SpeedAllDriveMotors:
float Speed = UtilData.ToFloat(p.Data.Payload);
MotorControl.SetAllSpeed(Speed);
break;
case PacketID.BaseSpeed:
case PacketID.ShoulderSpeed:
case PacketID.ElbowSpeed:
case PacketID.WristSpeed:
case PacketID.DifferentialVert:
case PacketID.DifferentialRotate:
case PacketID.HandGrip:
byte address = (byte)(p.Data.ID - 0x8A);
byte direction = 0x00;
if (p.Data.Payload[0] > 0)
{
direction = 0x01;
//UtilCan.SpeedDir(CANBBB.CANBus0, false, 2, address, (byte)(-p.Data.Payload[1]), direction);
Console.WriteLine("ADDRESS :" + address + "DIR :" + direction + "PAY :" + (byte)(-p.Data.Payload[1]));
}
else
{
direction = 0x00;
//UtilCan.SpeedDir(CANBBB.CANBus0, false, 2, address, p.Data.Payload[1], direction);
Console.WriteLine("ADDRESS :" + address + "DIR :" + direction + "PAY :" + p.Data.Payload[1]);
}
break;
case PacketID.CameraRotation:
if ((sbyte)p.Data.Payload[1] > 0)
{
ServoSpinner -= 0.005f;
OutB.SetOutput(ServoSpinner);
}
else if ((sbyte)p.Data.Payload[1] < 0)
{
ServoSpinner += 0.005f;
OutB.SetOutput(ServoSpinner);
}
OutB.Dispose();
break;
}
}
}
