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