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