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; } } }