private static void sendPIDT(ICANBus CANBus, byte receiver, MotorBoardData MBD) { if (MBD != null) { UtilCan.SetP(CANBus, true, 0x2, receiver, MBD.P, 0); UtilCan.SetI(CANBus, true, 0x2, receiver, MBD.I, 0); UtilCan.SetD(CANBus, true, 0x2, receiver, MBD.D, 0); UtilCan.SetTicksPerRev(CANBus, true, 0x2, receiver, MBD.TicksPerRev); Log.Output(Log.Severity.INFO, Log.Source.HARDWAREIO, "MotorBoard with CAN ID " + receiver + " set: Model=" + MBD.Model + " P=" + MBD.P + " I=" + MBD.I + " D=" + MBD.D); } }
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; } } }
private static void sendPIDT(ICANBus CANBus, byte receiver, MotorBoardData MBD) { if (MBD == null) { Console.WriteLine("MotorBoard ID not found in CSV"); } else { UtilCan.SetP(CANBus, true, 0x2, receiver, MBD.P, 0); UtilCan.SetI(CANBus, true, 0x2, receiver, MBD.I, 0); UtilCan.SetD(CANBus, true, 0x2, receiver, MBD.D, 0); UtilCan.SetTicksPerRev(CANBus, true, 0x2, receiver, MBD.TicksPerRev); } }
public static void Initialize(CANBusBBB CANBus) { GetCSVData CSVobj = new GetCSVData(); CSVobj.Initialize(); // Send Model Request to each Motor Board for (int i = 0; i < 7; i++) { UtilCan.ModelReq(CANBus, true, 2, Convert.ToByte(16 + i)); } int count = 100; // How many reads to do before giving up for (int j = 0; j < count; j++) { Task <Tuple <uint, byte[]> > CanRead = CANBus.ReadAsync(); int msec = 0; while (!CanRead.IsCompleted || (msec > 10)) { CanRead.Wait(100); } if (CanRead.IsCompleted) { Tuple <uint, byte[]> temp = CanRead.Result; uint sender = ((temp.Item1) >> 0x1F) & 0x1F; byte receiver = Convert.ToByte((temp.Item1) & 0x1F); byte[] data = temp.Item2; if (data[0] == 0x12 && receiver == 0x2) { MotorBoardData MBD; switch (receiver) { case 0x10: MBD = CSVobj.getMB1(data[1]); break; case 0x11: MBD = CSVobj.getMB2(data[1]); break; case 0x12: MBD = CSVobj.getMB3(data[1]); break; case 0x13: MBD = CSVobj.getMB4(data[1]); break; case 0x14: MBD = CSVobj.getMB5(data[1]); break; case 0x15: MBD = CSVobj.getMB6(data[1]); break; case 0x16: MBD = CSVobj.getMB7(data[1]); break; default: MBD = null; Console.WriteLine("CAN response receiver not known"); break; } sendPIDT(CANBus, receiver, MBD); } } else { CanRead.Dispose(); count = 100; } } }
public static void Initialize(CANBusBBB CANBus) { GetCSVData CSVobj = new GetCSVData(); CSVobj.Initialize(); // Send Model Request to each Motor Board for (int i = 0; i < 7; i++) { UtilCan.ModelReq(CANBus, true, 2, Convert.ToByte(16 + i)); } // Send Model Request to each Motor Board for (int i = 0; i < 7; i++) { UtilCan.ModeSelect(CANBus, true, 2, Convert.ToByte(16 + i), 0); } /* * int count = 10; // How many reads to do before giving up * for (int j = 0; j < count; j++) * { * Task<Tuple<uint, byte[]>> CanRead = CANBus.ReadAsync(); * int msec = 0; * while (!CanRead.IsCompleted || (msec > 10)) * CanRead.Wait(100); * * if (CanRead.IsCompleted) * { * Tuple<uint, byte[]> temp = CanRead.Result; * byte sender = Convert.ToByte(((temp.Item1) >> 0x1F) & 0x1F); * byte receiver = Convert.ToByte((temp.Item1) & 0x1F); * byte[] data = temp.Item2; * * * if (data[0] == 0x12 && receiver == 0x2) * { * MotorBoardData MBD; * switch (sender) * { * case 0x10: * MBD = CSVobj.getMB1(data[1]); * break; * case 0x11: * MBD = CSVobj.getMB2(data[1]); * break; * case 0x12: * MBD = CSVobj.getMB3(data[1]); * break; * case 0x13: * MBD = CSVobj.getMB4(data[1]); * break; * case 0x14: * MBD = CSVobj.getMB5(data[1]); * break; * case 0x15: * MBD = CSVobj.getMB6(data[1]); * break; * case 0x16: * MBD = CSVobj.getMB7(data[1]); * break; * default: * MBD = null; * Log.Output(Log.Severity.WARNING, Log.Source.HARDWAREIO, "CAN ID " + sender + " is not a known Motorboard ID"); * break; * } * sendPIDT(CANBus, sender, MBD); * } * } * else * { * CanRead.Dispose(); * count = 0; * } * }*/ }