public static void ProcessInstructions()
{
if (!StopPackets.IsEmpty())
{
StopPackets = new QueueBuffer();
// Stop the Rover
}
else if (!ModePackets.IsEmpty())
{
ProcessModePackets();
}
else
{
switch (CurDriveMode)
{ // TODO For each case statement, clear undeeded queue buffers
case DriveMode.BaseDrive:
ProcessBasePackets();
PathPackets = new QueueBuffer();
break;
case DriveMode.toGPS:
ProcessPathPackets();
DrivePackets = new QueueBuffer();
break;
case DriveMode.findTennisBall:
DrivePackets = new QueueBuffer();
PathPackets = new QueueBuffer();
break;
case DriveMode.toTennisBall:
DrivePackets = new QueueBuffer();
PathPackets = new QueueBuffer();
break;
case DriveMode.destination:
DrivePackets = new QueueBuffer();
PathPackets = new QueueBuffer();
//Spinning Motor
float t = 0.1f;
while (t < .9f)
{
OutA.SetOutput(t);
t += 0.0001f;
}
while (t > .1f)
{
OutA.SetOutput(t);
t -= 0.0001f;
}
OutA.Dispose();
break;
}
}
}
public static void ProcessInstructions()
{
if (!StopPackets.IsEmpty())
{
StopPackets = new QueueBuffer();
// Stop the Rover
}
else if (!ModePackets.IsEmpty())
{
ProcessModePackets();
}
else
{
switch (CurDriveMode)
{ // TODO For each case statement, clear undeeded queue buffers
case DriveMode.BaseDrive:
ProcessBasePackets();
PathPackets = new QueueBuffer();
break;
case DriveMode.toGPS:
ProcessPathPackets();
DrivePackets = new QueueBuffer();
break;
case DriveMode.findTennisBall:
DrivePackets = new QueueBuffer();
PathPackets = new QueueBuffer();
break;
case DriveMode.toTennisBall:
DrivePackets = new QueueBuffer();
PathPackets = new QueueBuffer();
break;
case DriveMode.destination:
DrivePackets = new QueueBuffer();
PathPackets = new QueueBuffer();
break;
}
}
}
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 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;
}
}
}
