public static void ModeSelect(ICANBus CANBus, bool Priority, byte Sender, byte Receiver, byte Mode)
{
byte[] Payload = new byte[2];
Payload[0] = 0;
Payload[1] = Mode;
CANBus.Write(ConstructCanID(Priority, Sender, Receiver), Payload);
}
public static void SetTicksPerRev(ICANBus CANBus, bool Priority, byte Sender, byte Receiver, UInt16 TPR)
{
byte[] Payload = new byte[3];
Payload[0] = 15;
Payload[1] = (byte)(TPR & 0xFF);
Payload[2] = (byte)((TPR >> 8) & 0xFF);
CANBus.Write(ConstructCanID(Priority, Sender, Receiver), Payload);
}
public static void SpeedDir(ICANBus CANBus, bool Priority, byte Sender, byte Receiver, byte Speed, byte Direction)
{
byte[] Payload = new byte[3];
Payload[0] = 2;
Payload[1] = Speed;
Payload[2] = Direction;
CANBus.Write(ConstructCanID(Priority, Sender, Receiver), Payload);
}
public Arm(ICANBus CANBus)
{
ReceiveQueue = new Queue <ArmPacket>();
this.CANBus = CANBus;
// Start Read
ReadThread = new Thread(new ThreadStart(ReadOnThread));
ReadThread.Start();
}
private static void TwoData(ICANBus CANBus, bool Priority, byte Sender, byte Receiver, byte DataID, UInt16 val1, UInt16 val2)
{
byte[] Payload = new byte[5];
Payload[0] = DataID;
Payload[1] = (byte)(val1 & 0xFF);
Payload[2] = (byte)((val1 >> 8) & 0xFF);
Payload[3] = (byte)(val2 & 0xFF);
Payload[4] = (byte)((val2 >> 8) & 0xFF);
CANBus.Write(ConstructCanID(Priority, Sender, Receiver), Payload);
}
/// <summary> Initializes a VESC Motor controller </summary>
/// <param name="CANBus"> CAN output to control the motor controller </param>
/// <param name="MaxRPM"> Limiting factor for speed (should never exceed + or - this val) </param>
/// <param name="RPMFilter"> Filter to use with MC. Good for ramp-up protection and other applications </param>
public VESC(ICANBus CANBus, int MaxRPM, int MotorMaxRpm, int ErpmPerRpm, uint CANID, IFilter <int> RPMFilter = null)
{
IsCAN = true;
this.CANBus = CANBus;
this.MaxRPM = Math.Abs(MaxRPM);
this.CANID = CANID;
this.RPMFilter = RPMFilter;
this.ERPM_PER_RPM = Math.Abs(ErpmPerRpm);
this.MOTOR_MAX_RPM = Math.Abs(MotorMaxRpm);
this.SetRPMDirectly(0);
SetSpeedThreadFactory().Start();
}
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);
}
}
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 ModelReq(ICANBus CANBus, bool Priority, byte Sender, byte Receiver)
{
byte[] Payload = new byte[1];
Payload[0] = 16;
CANBus.Write(ConstructCanID(Priority, Sender, Receiver), Payload);
}
public static void SetD(ICANBus CANBus, bool Priority, byte Sender, byte Receiver, UInt16 val1, UInt16 val2)
{
TwoData(CANBus, Priority, Sender, Receiver, 14, val1, val2);
}
public static void AngleSpeed(ICANBus CANBus, bool Priority, byte Sender, byte Receiver, UInt16 Angle, UInt16 Velocity)
{
TwoData(CANBus, Priority, Sender, Receiver, 4, Angle, Velocity);
}
public VESC(ICANBus CANBus, float MaxSpeed, int MotorMaxRpm, int ErpmPerRpm, uint CANID, IFilter <int> RPMFilter = null)
: this(CANBus, (int)(MaxSpeed * MotorMaxRpm), MotorMaxRpm, ErpmPerRpm, CANID, RPMFilter)
{
}
public static void Main(string[] args)
{
Console.WriteLine("Initializing");
StateStore.Start("CAN");
BeagleBone.Initialize(SystemMode.DEFAULT, true);
//BBBPinManager.AddMappingsCAN(BBBPin.P9_20, BBBPin.P9_19);
//BBBPinManager.AddMappingUART(BBBPin.P9_24);
//BBBPinManager.AddMappingUART(BBBPin.P9_26);
//BBBPinManager.AddMappingsI2C(BBBPin.P9_19, BBBPin.P9_20);
//BBBPinManager.ApplyPinSettings(BBBPinManager.ApplicationMode.APPLY_IF_NONE);
bool Read = args.Length > 0;
ICANBus can = CANBBB.CANBus0;
for (; ;)
{
Console.WriteLine(Read);
if (Read)
{
var(id, read) = can.Read();
Console.Write($"ID { id }: ");
foreach (byte b in read)
{
Console.Write(Convert.ToChar(b));
}
Console.WriteLine();
}
else
{
string s = Console.ReadLine();
byte[] send = new byte[Math.Min(8, s.Length)];
for (int i = 0; i < Math.Min(8, s.Length); i++)
{
send[i] = Convert.ToByte(s[i]);
}
can.Write(0x123, send);
Thread.Sleep(10);
}
Read = !Read;
}
//II2CBus i2c = I2CBBB.I2CBus2;
//RaspberryPi.Initialize();
//II2CBus i2c = new I2CBusPi();
//BNO055 mag = new BNO055(i2c);
//mag.Begin();
/*
* IUARTBus uart = UARTBBB.UARTBus1;
* Scarlet.Components.Sensors.MTK3339 mtk = new Scarlet.Components.Sensors.MTK3339(uart);
* int i = 0;
* for (;;)
* {
* mtk.GetCoords();
* Console.WriteLine($"({mtk.Latitude}, {mtk.Longitude})");
* i %= 5;
* if (i == 0)
* Console.WriteLine("Fix: " + mtk.HasFix());
* i++;
* }*/
/*
* BBBPinManager.AddMappingADC(BBBPin.P9_39);
* BBBPinManager.ApplyPinSettings(BBBPinManager.ApplicationMode.APPLY_IF_NONE);
* PID pid = new PID(P);
* IAnalogueIn Analog = new AnalogueInBBB(BBBPin.P9_39);
* AdafruitMotor[] Motor = new AdafruitMotor[4];
* for (int i = 0; i < 4; i++)
* {
* var pwm = new AdafruitMotorPWM((AdafruitMotorPWM.Motors)i, I2CBBB.I2CBus2, 0x60);
* Motor[i] = new AdafruitMotor(pwm);
* }
* GamePadState State;
* do
* {
* Console.WriteLine("Reading!");
* State = GamePad.GetState(0);
* if (State.IsConnected)
* Console.WriteLine($"Left: {State.Triggers.Left}, Right: {State.Triggers.Right}");
* else
* Console.WriteLine("NOT CONNECTED");
* double Val = 1.0;
* pid.Feed(Analog.GetInput());
* if (!ReceivingInput(State))
* {
* Val = Analog.GetInput();
* }
* Console.WriteLine(Val);
* Motor[0].Speed = State.Triggers.Left * -75.0f;
* Motor[0].UpdateState();
* Motor[1].Speed = State.Triggers.Right * 75.0f;
* Motor[1].UpdateState();
* Motor[2].Speed = State.Triggers.Left * 75.0f;
* Motor[2].UpdateState();
* Motor[3].Speed = State.Triggers.Right * 75.0f;
* Motor[3].UpdateState();
* } while (State.Buttons.Start != ButtonState.Pressed);
*/
}
