public static void InitBeagleBone()
{
StateStore.Start("MainRover");
BeagleBone.Initialize(SystemMode.NO_HDMI, true);
PinConfig();
Sensors = new List <ISensor>();
try
{
//Sensors.Add(new BNO055(I2CBBB.I2CBus2));
//Sensors.Add(new MTK3339(UARTBBB.UARTBus4));
}
catch (Exception e)
{
Log.Output(Log.Severity.ERROR, Log.Source.SENSORS, "Failed to initalize sensors (gps and/or mag)");
Log.Exception(Log.Source.SENSORS, e);
}
/*
* // Add back in when limit switch is complete
* LimitSwitch Switch = new LimitSwitch(new DigitalInBBB(Pins.SteeringLimitSwitch));
* Switch.SwitchToggle += (object sender, LimitSwitchToggle e) => Console.WriteLine("PRESSED!");
* Sensors.Add(Switch);
*/
foreach (ISensor Sensor in Sensors)
{
if (Sensor is MTK3339)
{
previousCoords = ((MTK3339)Sensor).GetCoords();
}
}
}
public static void InitBeagleBone()
{
StateStore.Start("MainRover");
BeagleBone.Initialize(SystemMode.NO_HDMI, true);
PinConfig();
IPWMOutputConfig();
Sensors = new List <ISensor>();
try
{
//Sensors.Add(new BNO055(I2CBBB.I2CBus2));
//Sensors.Add(new MTK3339(UARTBBB.UARTBus4));
}
catch (Exception e)
{
Log.Output(Log.Severity.ERROR, Log.Source.SENSORS, "Failed to initalize sensors (gps and/or mag)");
Log.Exception(Log.Source.SENSORS, e);
}
foreach (ISensor Sensor in Sensors)
{
if (Sensor is MTK3339)
{
previousCoords = ((MTK3339)Sensor).GetCoordinates();
}
}
}
static void Main(String[] args)
{
StateStore.Start("Start code");
BeagleBone.Initialize(SystemMode.DEFAULT, true);
BBBPinManager.AddMappingPWM(BBBPin.P9_14);
BBBPinManager.ApplyPinSettings(BBBPinManager.ApplicationMode.APPLY_IF_NONE);
IPWMOutput OutA = PWMBBB.PWMDevice1.OutputA;
OutA.SetFrequency(50);
OutA.SetOutput(0f);
OutA.SetEnabled(true);
float t = 0.1f;
while (t < .9f)
{
OutA.SetOutput(t);
t += 0.0001f;
}
while (t > .1f)
{
OutA.SetOutput(t);
t -= 0.0001f;
}
OutA.Dispose();
}
static void Main(string[] Args)
{
ParseArgs(Args);
StateStore.Start("SciRover");
Log.SetGlobalOutputLevel(Log.Severity.DEBUG);
//Log.SetSingleOutputLevel(Log.Source.NETWORK, Log.Severity.DEBUG);
Log.ErrorCodes = ScienceErrors.ERROR_CODES;
Log.SystemNames = ScienceErrors.SYSTEMS;
Log.Begin();
Log.ForceOutput(Log.Severity.INFO, Log.Source.OTHER, "Science Station - Rover Side");
Client.Start(IP, PortTCP, PortUDP, "SciRover");
BeagleBone.Initialize(SystemMode.DEFAULT, true);
IOHandler = new IOHandler();
IOHandler.InitializeSystems(ApplyDevTree);
((Turntable)IOHandler.TurntableController).TargetAngle = 50;
while (Console.KeyAvailable)
{
Console.ReadKey();
} // Clear previous keypresses
Log.ForceOutput(Log.Severity.INFO, Log.Source.OTHER, "Press any key to exit.");
while (!Console.KeyAvailable)
{
IOHandler.UpdateStates();
Thread.Sleep(20);
}
Environment.Exit(0);
}
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);
}
public ArmMotorController()
{
Scarlet.Utilities.StateStore.Start("ARM");
BBBPinManager.AddMappingPWM(ShoulderPin);
BBBPinManager.AddMappingPWM(ElbowPin);
BBBPinManager.AddMappingPWM(WristPin1);
BBBPinManager.AddMappingPWM(WristPin2);
BBBPinManager.AddMappingGPIO(WristDirectionPin1, true, ResistorState.PULL_UP);
BBBPinManager.AddMappingGPIO(WristDirectionPin2, true, ResistorState.PULL_UP);
BBBPinManager.AddMappingADC(ShoulderPotPin);
BBBPinManager.AddMappingADC(ElbowPotPin);
BBBPinManager.AddMappingADC(WristPotPin);
BBBPinManager.ApplyPinSettings(BBBPinManager.ApplicationMode.APPLY_IF_NONE);
BeagleBone.Initialize(SystemMode.DEFAULT, true);
const double DegToRad = Math.PI / 180.0;
a = new Arm((0, 0, Math.PI / 2, Math.PI / 2, 0, 0, 6.8),
(0, 0, -76 * DegToRad, 100 * DegToRad, 0, 0, 28.0),
(0, 0, -168.51 * DegToRad, -10 * DegToRad, 0, 0, 28.0),
(-2 * Math.PI, 2 * Math.PI, -Math.PI / 2, Math.PI / 2, 0, 0, 12.75));
//(0, 12.75, -Math.PI / 2, Math.PI / 2));
LowPass <float> ShoulderFilter = new LowPass <float>();
LowPass <float> ElbowFilter = new LowPass <float>();
ShoulderPWM = PWMBBB.PWMDevice1.OutputB;
ElbowPWM = PWMBBB.PWMDevice1.OutputA;
IPWMOutput WristPWM1 = PWMBBB.PWMDevice2.OutputB;
IPWMOutput WristPWM2 = PWMBBB.PWMDevice2.OutputA;
IDigitalOut WristDirectionGPIO1 = new DigitalOutBBB(WristDirectionPin1);
IDigitalOut WristDirectionGPIO2 = new DigitalOutBBB(WristDirectionPin2);
IAnalogueIn ShoulderADC = new AnalogueInBBB(ShoulderPotPin);
IAnalogueIn ElbowADC = new AnalogueInBBB(ElbowPotPin);
IAnalogueIn WristADC = new AnalogueInBBB(WristPotPin);
Shoulder = new TalonMC(ShoulderPWM, 0.2f, ShoulderFilter);
Elbow = new TalonMC(ElbowPWM, 0.2f, ElbowFilter);
Wrist1 = new CytronMD30C(WristPWM1, WristDirectionGPIO1, 0.3f);
Wrist2 = new CytronMD30C(WristPWM2, WristDirectionGPIO2, 0.3f);
ShoulderPot = new Potentiometer(ShoulderADC, 300);
ElbowPot = new Potentiometer(ElbowADC, 300);
WristPot = new Potentiometer(WristADC, 300);
}
static void Main(string[] args)
{
int DEGREES_OFFSET = -20;
StateStore.Start("MagTest");
BeagleBone.Initialize(SystemMode.DEFAULT, true);
BBBPinManager.AddMappingsI2C(BBBPin.P9_17, BBBPin.P9_26);
BBBPinManager.ApplyPinSettings(BBBPinManager.ApplicationMode.APPLY_IF_NONE);
II2CBus I2C = I2CBBB.I2CBus1;
Console.WriteLine("Status: BBB Pin Mapings Complete");
if (I2C == null)
{
Console.WriteLine("ERROR: I2C not mapped correctly. Fix First");
}
else
{
Console.WriteLine("Status: I2C Not null");
BNO055 magObj = new BNO055(I2C);
Console.WriteLine("Status: BNO055 object created");
magObj.SetMode(BNO055.OperationMode.OPERATION_MODE_COMPASS);
magObj.Begin();
double direction;
while (true)
{
Tuple <float, float, float> magnets;
magnets = magObj.GetVector(BNO055.VectorType.VECTOR_MAGNETOMETER);
// Convert magnetic readings to degrees
direction = getDegrees(magnets);
//Add the offset from specific location
direction += DEGREES_OFFSET;
// Make sure degrees value is between 0 and 360
direction = standarize(direction);
Console.WriteLine(direction);
}
}
}
public static void Main(string[] args)
{
Telemetry = new Dictionary <Device, JointTelemetry>
{
{ Device.BASE, new JointTelemetry() },
{ Device.SHOULDER, new JointTelemetry() },
{ Device.ELBOW, new JointTelemetry() },
{ Device.WRIST, new JointTelemetry() },
{ Device.DIFFERENTIAL_1, new JointTelemetry() },
{ Device.DIFFERENTIAL_2, new JointTelemetry() },
{ Device.HAND, new JointTelemetry() }
};
Log.Begin();
Log.Output(Log.Severity.INFO, Log.Source.ALL, "Initializing CAN Bus...");
BeagleBone.Initialize(SystemMode.NO_HDMI, true);
BBBPinManager.AddBusCAN(0, false);
BBBPinManager.ApplyPinSettings(BBBPinManager.ApplicationMode.APPLY_IF_NONE);
Log.Output(Log.Severity.INFO, Log.Source.ALL, "CAN Initialized.");
Client.Start(ServerIP, TCPPort, UDPPort, "ArmTestClient");
Arm arm = new Arm(CANBBB.CANBus0);
Parsing.Start(arm);
while (ENABLED)
{
ArmPacket?next = arm.ReadNext();
if (next != null)
{
UpdateTelemetry((ArmPacket)next);
}
SendTelemetry();
Thread.Sleep(Constants.DEFAULT_MIN_THREAD_SLEEP);
}
}
static void Main(string[] args)
{
// Begin setup of Beaglebone pins
StateStore.Start("SmartFlatbot");
BeagleBone.Initialize(SystemMode.DEFAULT, true);
// Add Beaglebone mapings with Scarlet
BBBPinManager.AddMappingPWM(BBBPin.P9_14);
BBBPinManager.AddMappingPWM(BBBPin.P9_16);
BBBPinManager.AddMappingGPIO(BBBPin.P9_15, true, ResistorState.NONE, true);
BBBPinManager.AddMappingGPIO(BBBPin.P9_27, true, ResistorState.NONE, true);
// Apply mappings to Beaglebone
BBBPinManager.ApplyPinSettings(BBBPinManager.ApplicationMode.APPLY_IF_NONE);
IDigitalOut Motor1Output = new DigitalOutBBB(BBBPin.P9_15);
IDigitalOut Motor2Output = new DigitalOutBBB(BBBPin.P9_27);
IPWMOutput OutA = PWMBBB.PWMDevice1.OutputA;
IPWMOutput OutB = PWMBBB.PWMDevice1.OutputB;
Motor1Output.SetOutput(false);
Motor2Output.SetOutput(false);
PWMBBB.PWMDevice1.SetFrequency(10000);;
OutA.SetEnabled(true);
OutB.SetEnabled(true);
// Setup Motor Controls
CytronMD30C[] Motor = new CytronMD30C[2];
Motor[0] = new CytronMD30C(OutA, Motor1Output, (float).5);
Motor[1] = new CytronMD30C(OutB, Motor2Output, (float).5);
Motor[0].SetSpeed(0);
Motor[1].SetSpeed(0);
}
public static void Main(string[] args)
{
Boolean turnMode = false;
Boolean lineMode = false;
double SetTurn = 0;
double PTurn = 0;
double ITurn = 0;
double DTurn = 0;
double SetDis = 0;
double PDis = 0;
double IDis = 0;
double DDis = 0;
// Begin setup of Beaglebone pins
StateStore.Start("SmartFlatbot");
BeagleBone.Initialize(SystemMode.DEFAULT, true);
// Add Beaglebone mapings with Scarlet
BBBPinManager.AddMappingPWM(BBBPin.P9_14);
BBBPinManager.AddMappingPWM(BBBPin.P9_16);
BBBPinManager.AddMappingGPIO(BBBPin.P9_15, true, ResistorState.NONE, true);
BBBPinManager.AddMappingGPIO(BBBPin.P9_27, true, ResistorState.NONE, true);
// Apply mappings to Beaglebone
BBBPinManager.ApplyPinSettings(BBBPinManager.ApplicationMode.APPLY_IF_NONE);
IDigitalOut Motor1Output = new DigitalOutBBB(BBBPin.P9_15);
IDigitalOut Motor2Output = new DigitalOutBBB(BBBPin.P9_27);
IPWMOutput OutA = PWMBBB.PWMDevice1.OutputA;
IPWMOutput OutB = PWMBBB.PWMDevice1.OutputB;
Motor1Output.SetOutput(false);
Motor2Output.SetOutput(false);
PWMBBB.PWMDevice1.SetFrequency(10000);;
OutA.SetEnabled(true);
OutB.SetEnabled(true);
// Setup Motor Controls
CytronMD30C[] Motor = new CytronMD30C[2];
Motor[0] = new CytronMD30C(OutA, Motor1Output, (float).5);
Motor[1] = new CytronMD30C(OutB, Motor2Output, (float).5);
// Make rover (hopefully) stay still in begining.
Motor[0].SetSpeed(0);
Motor[1].SetSpeed(0);
// Set the rover in turn only mode for testing
if (args.Length > 0)
{
if (args[0].Equals("turn"))
{
turnMode = true;
}
else if (args[0].Equals("line"))
{
lineMode = true;
}
}
/* Get the PID values from file PIDValues.txt
* All values will be in double format
* Data must be in this format below:
* ------------------------------------------
* Line 1 Ignored
* Line 2 Ignored
* Line 3 Direction target value
* Line 4 Direction P value
* Line 5 Direction I value
* Line 6 Direction D value
* Line 7 Ignored
* Line 8 Distance target value
* Line 9 Distance P value
* Line 10 Distance I value
* Line 11 Distance D value
* All lines after are ignored
* -----------------------------------------
*/
try
{ // Open the text file using a stream reader.
using (StreamReader sr = new StreamReader("PIDValues.txt"))
{
// Read the stream to a string, and write the string to the console.
sr.ReadLine();
sr.ReadLine(); // Ignore first two lines
SetTurn = Convert.ToDouble(sr.ReadLine());
PTurn = Convert.ToDouble(sr.ReadLine());
ITurn = Convert.ToDouble(sr.ReadLine());
DTurn = Convert.ToDouble(sr.ReadLine());
sr.ReadLine();
SetDis = Convert.ToDouble(sr.ReadLine());
PDis = Convert.ToDouble(sr.ReadLine());
IDis = Convert.ToDouble(sr.ReadLine());
DDis = Convert.ToDouble(sr.ReadLine());
}
}
catch (Exception e)
{
Console.WriteLine("PIDValues.txt is either missing or in improper format:");
Console.WriteLine(e.Message);
}
Console.WriteLine("Direction Target, P, I, D: " + SetTurn + " " + PTurn + " " + ITurn + " " + DTurn);
Console.WriteLine("Distance Target, P, I, D: " + SetDis + " " + PDis + " " + IDis + " " + DDis);
// Set the PIDs
PID directionPID = new PID(PTurn, ITurn, DTurn);
directionPID.SetTarget(SetTurn);
PID distancePID = new PID(PDis, IDis, DDis);
directionPID.SetTarget(SetDis);
// Start UDP communication
// Listen for any IP with port 9000
UdpClient udpServer = new UdpClient(9000);
IPEndPoint remoteEP = new IPEndPoint(IPAddress.Any, 9000);
// Main Loop. Keep running until program is stopped
do
{
// Get data from UDP. Convert it to a String
// Note this is a blocking call.
var data = udpServer.Receive(ref remoteEP);
String dataString = Encoding.ASCII.GetString(data);
Console.WriteLine(dataString);
// Set the wheel values to be zero for now
Double leftWheel = 0;
Double rightWheel = 0;
// Make sure the data received is not an empty string
if (!String.IsNullOrEmpty(dataString))
{
// Split the String into distance and direction
// Is in format "Distance,Direction"
// Distance is from 1 to 150 ish
// Direction is from -10 to 10 degrees
String[] speedDis = dataString.Split(',');
Double distance = Convert.ToDouble(speedDis[0]);
Double direction = (Convert.ToDouble(speedDis[1]));
// If mode is set to not turn only the run
if (!turnMode)
{
distancePID.Feed(distance);
if (!Double.IsNaN(distancePID.Output))
{
rightWheel += distancePID.Output;
leftWheel += distancePID.Output;
}
}
if (!lineMode)
{
directionPID.Feed(direction);
if (!Double.IsNaN(directionPID.Output))
{
rightWheel += directionPID.Output;
leftWheel -= directionPID.Output;
}
}
}
// If data received is not a full string, stop motors.
else
{
leftWheel = 0;
rightWheel = 0;
}
// Set the speed for the motor controllers
// Max speed is one
Motor[0].SetSpeed((float)(rightWheel));
Motor[1].SetSpeed((float)(leftWheel));
} while (true);
}
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);
}
}
}
static void Main(string[] args)
{
StateStore.Start("CanTester");
BeagleBone.Initialize(SystemMode.NO_HDMI, true);
BBBPinManager.AddBusCAN(0, false);
BBBPinManager.ApplyPinSettings(BBBPinManager.ApplicationMode.APPLY_IF_NONE);
bool Priority = false;
byte Sender = 0;
byte Receiver = 0;
byte DataID = 0;
bool continuez = true;
UInt16 Val1 = 0;
UInt16 Val2 = 0;
byte Speed = 0;
byte Direction = 0;
byte Mode = 0;
Console.WriteLine("CAN TESTER TOOL");
while (continuez)
{
Console.Write("Write Mode(true) or Read Mode(false)? (true/false): ");
bool Write = oldNew(Console.ReadLine(), true);
if (Write) // Write Mode
{
Console.Write("Enter Priority (true/false) [Previous " + Priority + " ]: ");
Priority = oldNew(Console.ReadLine(), Priority);
Console.Write("Enter Sender (byte) [Previous " + Sender + "]: ");
Sender = oldNewHex(Console.ReadLine(), Sender);
Console.Write("Enter Reciever (byte) [Previous " + Receiver + "]: ");
Receiver = oldNewHex(Console.ReadLine(), Receiver);
Console.Write("Enter DataID (byte) [Previous " + DataID + "]: ");
DataID = oldNew(Console.ReadLine(), DataID);
switch (DataID)
{
case 0x0:
Console.Write("Enter in mode [Previous " + Mode + "]: ");
Mode = oldNew(Console.ReadLine(), Mode);
break;
case 0x2:
Console.Write("Enter in speed [Previous " + Speed + "]: ");
Speed = oldNew(Console.ReadLine(), Speed);
Console.Write("Enter in direction (1 or 0) [Previous " + Direction + "]: ");
Direction = oldNew(Console.ReadLine(), Direction);
break;
case 0x4:
case 0xA:
case 0xC:
case 0xE:
case 0x14:
case 0x18:
Console.Write("Enter in first value [Previous " + Val1 + "]: ");
Val1 = oldNew(Console.ReadLine(), Val1);
Console.Write("Enter in second value [Previous " + Val2 + "]: ");
Val2 = oldNew(Console.ReadLine(), Val2);
break;
case 0x16:
break;
default:
Console.WriteLine("Unknown or unsupported data value");
break;
}
bool keepSending = true;
while (keepSending)
{
switch (DataID)
{
case 0x0:
ModeSelect(CANBBB.CANBus0, Priority, Sender, Receiver, Mode);
break;
case 0x2:
SpeedDir(CANBBB.CANBus0, Priority, Sender, Receiver, Speed, Direction);
break;
case 0x4:
case 0xA:
case 0xC:
case 0xE:
case 0x14:
case 0x18:
TwoData(CANBBB.CANBus0, Priority, Sender, Receiver, DataID, Val1, Val2);
break;
case 0x10:
ModelReq(CANBBB.CANBus0, Priority, Sender, Receiver);
break;
default:
Console.WriteLine("Unknown or unsupported data value");
break;
}
Console.Write("Data Sent. Hold enter to continue sending (true/false): ");
try
{
keepSending = oldNew(Console.ReadLine(), true);
}
catch
{
if (DataID == 2)
{
Console.WriteLine("Speed set to zero");
SpeedDir(CANBBB.CANBus0, Priority, Sender, Receiver, 0, Direction);
keepSending = false;
}
}
}
}
else // Read Mode
{
bool reading = true;
while (reading)
{
int count = 0;
Task <Tuple <uint, byte[]> > CanRead = CANBBB.CANBus0.ReadAsync();
while (!CanRead.IsCompleted || (count > 10))
{
CanRead.Wait(1000);
count++;
}
if (CanRead.IsCompleted)
{
Tuple <uint, byte[]> temp = CanRead.Result;
byte priority = Convert.ToByte(((temp.Item1) >> 0b1111111111) & 0x1F);
byte sender = Convert.ToByte(((temp.Item1) >> 0x1F) & 0x1F);
byte receiver = Convert.ToByte((temp.Item1) & 0x1F);
Console.WriteLine("ID: " + Convert.ToString(temp.Item1, 2));
//Console.WriteLine("ID: " + priority + " " + sender + " " + receiver);
Console.WriteLine("Data: {0}",
string.Join(", ", temp.Item2.Select(v => v.ToString()))
);
}
else
{
Console.WriteLine("TIMEOUT, No CANID found");
}
Console.Write("Continue read?");
reading = oldNew(Console.ReadLine(), true);
}
}
Console.Write("Continue Program (true) or exit (false): ");
continuez = oldNew(Console.ReadLine(), true);
}
}
public static void Start(string[] args)
{
if (args.Length < 2)
{
TestMain.ErrorExit("Device testing needs device to test.");
}
switch (args[1].ToLower())
{
case "hx711":
{
if (args.Length < 5)
{
TestMain.ErrorExit("Insufficient info to run HX711 test. See help.");
}
IDigitalIn DataPin = null;
IDigitalOut ClockPin = null;
if (args[2].Equals("pi", StringComparison.InvariantCultureIgnoreCase))
{
RaspberryPi.Initialize();
DataPin = new DigitalInPi(int.Parse(args[3]));
ClockPin = new DigitalOutPi(int.Parse(args[4]));
}
else if (args[2].Equals("bbb", StringComparison.InvariantCultureIgnoreCase))
{
BeagleBone.Initialize(SystemMode.DEFAULT, true);
BBBPin DataBBBPin = IOBBB.StringToPin(args[3]);
BBBPin ClockBBBPin = IOBBB.StringToPin(args[4]);
BBBPinManager.AddMappingGPIO(DataBBBPin, false, ResistorState.NONE);
BBBPinManager.AddMappingGPIO(ClockBBBPin, true, ResistorState.NONE);
BBBPinManager.ApplyPinSettings(BBBPinManager.ApplicationMode.APPLY_IF_NONE);
DataPin = new DigitalInBBB(DataBBBPin);
ClockPin = new DigitalOutBBB(ClockBBBPin);
}
else
{
TestMain.ErrorExit("HX711 test: Unknown platform. See help.");
}
HX711 DUT = new HX711(ClockPin, DataPin);
while (Console.KeyAvailable)
{
Console.ReadKey();
}
Log.Output(Log.Severity.INFO, Log.Source.GUI, "[w] to increase gain, [s] to decrease. [z] to zero.");
Log.Output(Log.Severity.INFO, Log.Source.GUI, "Press any other key to exit.");
HX711.Gain Gain = HX711.Gain.GAIN_128x;
bool Continue = true;
while (Continue)
{
if (Console.KeyAvailable)
{
char Key = Console.ReadKey().KeyChar;
switch (Key)
{
case 'w':
{
if (Gain == HX711.Gain.GAIN_32x)
{
Gain = HX711.Gain.GAIN_64x;
}
else if (Gain == HX711.Gain.GAIN_64x)
{
Gain = HX711.Gain.GAIN_128x;
}
else
{
Log.Output(Log.Severity.ERROR, Log.Source.SENSORS, "Gain at maximum already.");
}
DUT.SetGain(Gain);
Log.Output(Log.Severity.INFO, Log.Source.SENSORS, "Gain now at " + Gain);
break;
}
case 's':
{
if (Gain == HX711.Gain.GAIN_128x)
{
Gain = HX711.Gain.GAIN_64x;
}
else if (Gain == HX711.Gain.GAIN_64x)
{
Gain = HX711.Gain.GAIN_32x;
}
else
{
Log.Output(Log.Severity.ERROR, Log.Source.SENSORS, "Gain at minimum already.");
}
DUT.SetGain(Gain);
Log.Output(Log.Severity.INFO, Log.Source.SENSORS, "Gain now at " + Gain);
break;
}
case 'z':
{
DUT.Tare();
Log.Output(Log.Severity.INFO, Log.Source.SENSORS, "Tared.");
break;
}
default:
{
Continue = false;
break;
}
}
}
DUT.UpdateState();
Log.Output(Log.Severity.INFO, Log.Source.SENSORS, "HX711 readings: Raw: " + DUT.GetRawReading() + ", Adjusted: " + DUT.GetAdjustedReading());
Thread.Sleep(250);
}
break;
}
default:
{
TestMain.ErrorExit("Unknown device.");
break;
}
}
}
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);
*/
}
