static void SetupBeagleboneAndPins()
{
BBBPinManager.AddMappingUART(Pins.SlaveRX);
BBBPinManager.AddMappingUART(Pins.SlaveTX);
BBBPinManager.AddMappingPWM(Pins.BaseRotation);
BBBPinManager.AddMappingGPIO(Pins.BaseRotationDir, true, ResistorState.NONE);
BBBPinManager.AddMappingPWM(Pins.Elbow);
BBBPinManager.AddMappingPWM(Pins.Shoulder);
BBBPinManager.AddMappingADC(Pins.ShoulderPot);
BBBPinManager.AddMappingGPIO(Pins.ElbowLimitSwitch, false, ResistorState.PULL_UP);
BBBPinManager.ApplyPinSettings(BBBPinManager.ApplicationMode.APPLY_IF_NONE);
Slave = UARTBBB.UARTBus1;
BeagleBone.Initialize(SystemMode.DEFAULT, true);
}
/// <summary>
/// Initializes a new instance of the <see cref="T:Scarlet.Components.Sensors.MTK3339"/> class.
/// Sets it to send GPRMC and GPGGA every 200 milliseconds.
/// </summary>
/// <param name="UART"> The UART bus to read from and write to. </param>
public MTK3339(IUARTBus UART)
{
this.UART = UART ?? throw new Exception("Cannot initialize MTK3339 with null UART bus!");
if (UART.BytesAvailable() > 0)
{
UART.Read(UART.BytesAvailable(), new byte[UART.BytesAvailable()]);
}
Thread ParseThread = new Thread(Parse)
{
IsBackground = true
};
ParseThread.Start();
}
/// <summary>
/// Initializes a new instance of the <see cref="T:Scarlet.Components.Sensors.MTK3339"/> class.
/// Sets it to send GPRMC and GPGGA every 200 milliseconds.
/// </summary>
/// <param name="UART"> The UART bus to read from and write to. </param>
public MTK3339(IUARTBus UART)
{
this.UART = UART;
WriteString(GPRMC_GPGGA);
Thread.Sleep(1);
WriteString(MEAS_200_MSEC);
Thread.Sleep(1);
WriteString(UPDATE_200_MSEC);
Thread.Sleep(1);
if (UART.BytesAvailable() > 0)
{
UART.Read(UART.BytesAvailable(), new byte[UART.BytesAvailable()]);
}
}
public VESC(IUARTBus UARTBus, int MaxRPM, int MotorMaxRpm, int ErpmPerRpm, int CANForwardID = -1, IFilter <int> RPMFilter = null)
{
IsCAN = false;
this.UARTBus = UARTBus;
this.UARTBus.BaudRate = UARTRate.BAUD_115200;
this.UARTBus.BitLength = UARTBitCount.BITS_8;
this.UARTBus.StopBits = UARTStopBits.STOPBITS_1;
this.UARTBus.Parity = UARTParity.PARITY_NONE;
this.CANForwardID = CANForwardID;
this.MaxRPM = Math.Abs(MaxRPM);
this.ERPM_PER_RPM = Math.Abs(ErpmPerRpm);
this.MOTOR_MAX_RPM = Math.Abs(MotorMaxRpm);
this.RPMFilter = RPMFilter;
this.SetRPMDirectly(0);
SetSpeedThreadFactory().Start();
}
/// <summary> Initializes a VESC Motor controller </summary>
/// <param name="UARTBus"> UART output to control the motor controller </param>
/// <param name="MaxSpeed"> Limiting factor for speed (should never exceed + or - this val) </param>
/// <param name="CANForwardID"> CAN ID of the motor controller (-1 to disable CAN forwarding) </param>
/// <param name="RPMFilter"> Filter to use with MC. Good for ramp-up protection and other applications </param>
public VESC(IUARTBus UARTBus, float MaxSpeed, int MotorMaxRpm, int ErpmPerRpm, int CANForwardID = -1, IFilter <int> RPMFilter = null)
: this(UARTBus, (int)(MaxSpeed * MotorMaxRpm), MotorMaxRpm, ErpmPerRpm, CANForwardID, RPMFilter)
{
}
public static void Start(string[] args)
{
if (args.Length < 3)
{
TestMain.ErrorExit("io bbb command requires functionality to test.");
}
BeagleBone.Initialize(SystemMode.DEFAULT, true);
switch (args[2].ToLower())
{
case "digin":
{
if (args.Length < 4)
{
TestMain.ErrorExit("io bbb digin command requires pin to test.");
}
BBBPin InputPin = StringToPin(args[3]);
Log.Output(Log.Severity.INFO, Log.Source.HARDWAREIO, "Testing digital input on BBB pin " + InputPin.ToString());
BBBPinManager.AddMappingGPIO(InputPin, false, ResistorState.PULL_DOWN);
BBBPinManager.ApplyPinSettings(BBBPinManager.ApplicationMode.APPLY_REGARDLESS);
IDigitalIn Input = new DigitalInBBB(InputPin);
while (true)
{
Log.Output(Log.Severity.INFO, Log.Source.HARDWAREIO, "Current pin state: " + (Input.GetInput() ? "HIGH" : "LOW"));
Thread.Sleep(250);
}
}
case "digout":
{
if (args.Length < 4)
{
TestMain.ErrorExit("io bbb digout command requires pin to test.");
}
BBBPin OutputPin = StringToPin(args[3]);
if (args.Length < 5)
{
TestMain.ErrorExit("io bbb digout command requires output mode (high/low/blink).");
}
if (args[4] != "high" && args[4] != "low" && args[4] != "blink")
{
TestMain.ErrorExit("Invalid digout test mode supplied.");
}
Log.Output(Log.Severity.INFO, Log.Source.HARDWAREIO, "Testing digital output on BBB pin " + OutputPin.ToString());
BBBPinManager.AddMappingGPIO(OutputPin, true, ResistorState.PULL_DOWN);
BBBPinManager.ApplyPinSettings(BBBPinManager.ApplicationMode.APPLY_REGARDLESS);
IDigitalOut Output = new DigitalOutBBB(OutputPin);
if (args[4] == "high")
{
Output.SetOutput(true);
}
else if (args[4] == "low")
{
Output.SetOutput(false);
}
else
{
bool Out = false;
while (true)
{
Output.SetOutput(Out);
Out = !Out;
Thread.Sleep(250);
}
}
break;
}
case "pwm":
{
if (args.Length < 4)
{
TestMain.ErrorExit("io bbb pwm command requires pin to test.");
}
BBBPin OutputPin = StringToPin(args[3]);
if (args.Length < 5)
{
TestMain.ErrorExit("io bbb pwm command requires frequency.");
}
int Frequency = int.Parse(args[4]);
if (args.Length < 6)
{
TestMain.ErrorExit("io bbb pwm command requires output mode.");
}
if (args[5] != "per" && args[5] != "sine")
{
TestMain.ErrorExit("io bbb pwm command invalid (per/sine).");
}
if (args[5] == "per" && args.Length < 7)
{
TestMain.ErrorExit("io bbb pwm per must be provided duty cycle.");
}
BBBPinManager.AddMappingPWM(OutputPin);
BBBPinManager.ApplyPinSettings(BBBPinManager.ApplicationMode.APPLY_REGARDLESS);
IPWMOutput Output = PWMBBB.GetFromPin(OutputPin);
Output.SetFrequency(Frequency);
Log.Output(Log.Severity.INFO, Log.Source.HARDWAREIO, "Testing PWM output on BBB pin " + OutputPin.ToString() + " at " + Frequency + "Hz.");
if (args[5] == "per")
{
Output.SetOutput(int.Parse(args[6]) / 100F);
Output.SetEnabled(true);
Thread.Sleep(15000); // Not sure if it stops outputting when the program exits.
}
else
{
int Cycle = 0;
while (true)
{
float Val = (float)((Math.Sin(Cycle * Math.PI / 180.000D) + 1) / 2);
Output.SetOutput(Val);
Thread.Sleep(50);
Cycle += 20;
}
}
break;
}
case "adc":
{
if (args.Length < 4)
{
TestMain.ErrorExit("io bbb adc command requires pin to test.");
}
BBBPin InputPin = StringToPin(args[3]);
BBBPinManager.AddMappingADC(InputPin);
BBBPinManager.ApplyPinSettings(BBBPinManager.ApplicationMode.APPLY_REGARDLESS);
IAnalogueIn Input = new AnalogueInBBB(InputPin);
Log.Output(Log.Severity.INFO, Log.Source.HARDWAREIO, "Testing analogue input on BBB pin " + InputPin.ToString());
while (true)
{
Log.Output(Log.Severity.INFO, Log.Source.HARDWAREIO, "ADC Input: " + Input.GetInput() + " (Raw: " + Input.GetRawInput() + ")");
Thread.Sleep(250);
}
}
case "int":
{
if (args.Length < 4)
{
TestMain.ErrorExit("io bbb int command requires pin to test.");
}
BBBPin InputPin = StringToPin(args[3]);
if (args.Length < 5)
{
TestMain.ErrorExit("io bbb int command requires interrupt mode (rise/fall/both).");
}
if (args[4] != "rise" && args[4] != "fall" && args[4] != "both")
{
TestMain.ErrorExit("Invalid interrupt mode supplied.");
}
BBBPinManager.AddMappingGPIO(InputPin, true, ResistorState.PULL_DOWN);
BBBPinManager.ApplyPinSettings(BBBPinManager.ApplicationMode.APPLY_REGARDLESS);
IDigitalIn Input = new DigitalInBBB(InputPin);
Log.Output(Log.Severity.INFO, Log.Source.HARDWAREIO, "Testing interrupts on BBB pin " + InputPin.ToString());
switch (args[4])
{
case "rise": ((IInterruptSource)Input).RegisterInterruptHandler(GetInterrupt, InterruptType.RISING_EDGE); break;
case "fall": ((IInterruptSource)Input).RegisterInterruptHandler(GetInterrupt, InterruptType.FALLING_EDGE); break;
case "both": ((IInterruptSource)Input).RegisterInterruptHandler(GetInterrupt, InterruptType.ANY_EDGE); break;
}
while (true)
{
Thread.Sleep(50);
} // Program needs to be running to receive.
}
case "mtk3339":
{
BBBPinManager.AddMappingUART(BBBPin.P9_24);
BBBPinManager.AddMappingUART(BBBPin.P9_26);
BBBPinManager.ApplyPinSettings(BBBPinManager.ApplicationMode.APPLY_REGARDLESS);
IUARTBus UART = UARTBBB.UARTBus1;
Log.Output(Log.Severity.INFO, Log.Source.HARDWAREIO, "Press any key to stop.");
while (Console.KeyAvailable)
{
Console.ReadKey();
}
byte[] Buffer = new byte[32];
while (true)
{
Thread.Sleep(10);
if (UART.BytesAvailable() > 0)
{
int Count = UART.Read(32, Buffer);
Console.Write(System.Text.Encoding.ASCII.GetString(UtilMain.SubArray(Buffer, 0, Count)));
}
if (Console.KeyAvailable)
{
break;
}
}
break;
}
}
}
public static void Main(string[] args)
{
Console.WriteLine("Initializing Minibot Code");
StateStore.Start("k den");
BeagleBone.Initialize(SystemMode.DEFAULT, true);
BBBPinManager.AddMappingsI2C(BBBPin.P9_19, BBBPin.P9_20); //Motors
BBBPinManager.AddMappingsI2C(BBBPin.P9_17, BBBPin.P9_18); //Motors
BBBPinManager.AddMappingUART(BBBPin.P9_24); //UART_1 TX GPS
BBBPinManager.AddMappingUART(BBBPin.P9_26); //UART_1 RX GPS
BBBPinManager.AddMappingsI2C(BBBPin.P9_21, BBBPin.P9_22); //Magnetometer Bus 2
BBBPinManager.ApplyPinSettings(BBBPinManager.ApplicationMode.APPLY_IF_NONE);
IUARTBus UARTBus = UARTBBB.UARTBus1;
I2CBusBBB I2C1 = I2CBBB.I2CBus1;
I2CBusBBB I2C2 = I2CBBB.I2CBus2;
MTK3339 gps = new MTK3339(UARTBus);
BNO055 magnetometer = new BNO055(I2C2);
AdafruitMotor[] MinibotMotors = new AdafruitMotor[8];
for (int i = 0; i < 4; i++)
{
var pwm = new AdafruitMotorPWM((AdafruitMotorPWM.Motors)i, I2C1);
MinibotMotors[i] = new AdafruitMotor(pwm);
}
for (int i = 0; i < 4; i++)
{
var pwm = new AdafruitMotorPWM((AdafruitMotorPWM.Motors)i, I2C2);
MinibotMotors[i + 4] = new AdafruitMotor(pwm);
}
//Tests the wheel motors
/*
* for (int i = 0; i < 8; i++)
* {
* Console.WriteLine("Motor " + i);
* MinibotMotors[i].SetSpeed(10);
* Thread.Sleep(500);
* MinibotMotors[i].SetSpeed(0);
* Thread.Sleep(500);
* }
*/
MinibotMotors[0].SetSpeed(0.0f);
MinibotMotors[1].SetSpeed(0.0f);
MinibotMotors[2].SetSpeed(0.0f);
MinibotMotors[3].SetSpeed(0.0f);
MinibotMotors[7].SetSpeed(0.0f);
magnetometer.SetMode(BNO055.OperationMode.OPERATION_MODE_MAGONLY);
Console.WriteLine("initialized");
var orientation = 0.0f;
while (true)
{
GamePadState State = GamePad.GetState(0);
if (State.Buttons.A == ButtonState.Pressed)
{
do
{
State = GamePad.GetState(0);
Console.WriteLine("Reading");
if (State.IsConnected)
{
float rightSpeed = State.Triggers.Right;
float leftSpeed = State.Triggers.Left;
float speed = rightSpeed - leftSpeed;
Console.WriteLine($"Left: {State.ThumbSticks.Left.Y}, Right: {State.ThumbSticks.Right.Y}");
Console.WriteLine("Left Trigger: " + leftSpeed);
Console.WriteLine("Right Trigger: " + rightSpeed);
MinibotMotors[0].SetSpeed((100.0f) * speed);
MinibotMotors[1].SetSpeed((-100.0f) * speed);
MinibotMotors[2].SetSpeed((100.0f) * speed);
MinibotMotors[3].SetSpeed((100.0f) * speed);
MinibotMotors[7].SetSpeed((-75.0f) * (State.ThumbSticks.Left.X));
}
else
{
Console.WriteLine("NOT CONNECTED");
}
}while (State.Buttons.Start != ButtonState.Pressed && State.Buttons.B != ButtonState.Pressed);
}
else
{
if (gps.Latitude == 0.0f || gps.Longitude == 0.0f)
{
var tuple = gps.GetCoords();
//Console.WriteLine($"({tuple.Item1}, {tuple.Item2})");
Thread.Sleep(150);
}
var xTesla = magnetometer.GetVector(BNO055.VectorType.VECTOR_MAGNETOMETER).Item1;
var yTesla = magnetometer.GetVector(BNO055.VectorType.VECTOR_MAGNETOMETER).Item2;
var zTesla = magnetometer.GetVector(BNO055.VectorType.VECTOR_MAGNETOMETER).Item3;
Console.WriteLine("xTesla: " + xTesla);
Console.WriteLine("yTesla: " + yTesla);
//Console.WriteLine(zTesla);
var arcTan = Math.Atan(xTesla / yTesla);
if (yTesla > 0)
{
orientation = (float)(90 - (arcTan * (180 / Math.PI)));
}
else if (yTesla < 0)
{
orientation = (float)(270 - (arcTan * (180 / Math.PI)));
}
else if (Math.Abs(yTesla) < 1e-6 && xTesla < 0)
{
orientation = 180.0f;
}
else if (Math.Abs(yTesla) < 1e-6 && xTesla > 0)
{
orientation = 0.0f;
}
Console.WriteLine(orientation);
Thread.Sleep(500);
}
}
}
