Ejemplo n.º 1
0
        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();
                }
            }
        }
Ejemplo n.º 2
0
        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();
                }
            }
        }
Ejemplo n.º 3
0
        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();
        }
Ejemplo n.º 4
0
        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);
        }
Ejemplo n.º 5
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);
 }
Ejemplo n.º 6
0
        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);
        }
Ejemplo n.º 7
0
        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);
                }
            }
        }
Ejemplo n.º 8
0
        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);
            }
        }
Ejemplo n.º 9
0
        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);
        }
Ejemplo n.º 10
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);
        }
Ejemplo n.º 11
0
        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;
            }
            }
        }
Ejemplo n.º 12
0
        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);
                }
            }
        }
Ejemplo n.º 13
0
        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);
            }
        }
Ejemplo n.º 14
0
        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;
            }
            }
        }
Ejemplo n.º 15
0
        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);
             */
        }