/// <summary> /// Main constructor /// </summary> /// <param name="leftForwardPwm">PWM port responsible for left motor moving forward</param> /// <param name="leftBackPwm">PWM port responsible for left motor moving backward</param> /// <param name="rightForwardPwm">PWM port responsible for right motor moving forward</param> /// <param name="rightBackPwm">PWM port responsible for right motor moving backward</param> /// <param name="gearPwm">PWM port responsible for controling gear changing servo</param> public MotorController(IPwmPort rightForwardPwm, IPwmPort rightBackPwm, IPwmPort leftForwardPwm, IPwmPort leftBackPwm, IPwmPort gearPwm, IDigitalInputPort leftCounterPort, IDigitalInputPort righCounterPort, BNO055 posSens, CameraGimbal gimb, Action clQueue) { quarterCircle = fullCircle / 4; halfCircle = fullCircle / 2; positionSensor = posSens; gimbal = gimb; clearQueue = clQueue; circumference = teethCount * chainPitch / magnetsCount; leftMotor = new Motor(leftForwardPwm, leftBackPwm); rightMotor = new Motor(rightForwardPwm, rightBackPwm); gearbox = new GearBox(gearPwm); rightCounter = new HallEffectCounter(righCounterPort); leftCounter = new HallEffectCounter(leftCounterPort); rightCounter.Name = Side.Right; leftCounter.Name = Side.Left; #if DEBUG rightCounter.RegisterForCount(CountChanged); leftCounter.RegisterForCount(CountChanged); #endif rightCounter.RegisterForLimitReached(MoveForwardStop); leftCounter.RegisterForLimitReached(MoveForwardStop); positionSensor.RegisterForHeadingChanged(HeadingChanged); moveForwardResetEventLeft = new AutoResetEvent(false); moveForwardResetEventRight = new AutoResetEvent(false); turnResetEvent = new AutoResetEvent(false); turnTokenSource = new CancellationTokenSource(); turnPid = new IdealPidController(); turnPid.ProportionalComponent = 1.8f; turnPid.IntegralComponent = 0.05f; turnPid.DerivativeComponent = 0.01f; turnPid.OutputMax = 50; turnPid.OutputMin = -50; stabilizePid = new IdealPidController(); stabilizePid.ProportionalComponent = 1.8f; stabilizePid.IntegralComponent = 0.05f; stabilizePid.DerivativeComponent = 0.01f; stabilizePid.OutputMax = stabilizeTurnRate; stabilizePid.OutputMin = -stabilizeTurnRate; }
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() { Debug.Print("BNO055 Test Application."); // _resetPort.Write(false); Thread.Sleep(10); _resetPort.Write(true); Thread.Sleep(50); // _comPort.Open(); WriteText("Orientation Sensor Test"); WriteText(""); WriteText("Sensor: BNO050"); WriteText("Driver Ver: 1"); WriteText("Unique ID: "); WriteText("Max Value: 0.0"); WriteText("Min Value: 0.0"); WriteText("Resolution: 0.01"); WriteText(""); Thread.Sleep(500); // var bno055 = new BNO055(0x29); bno055.DisplayRegisters(); bno055.PowerMode = BNO055.PowerModes.Normal; bno055.OperatingMode = BNO055.OperatingModes.ConfigurationMode; bno055.OperatingMode = BNO055.OperatingModes.InertialMeasurementUnit; bno055.DisplayRegisters(); Debug.Print("Current temperature: " + bno055.Temperature.ToString("f2")); while (true) { bno055.Read(); var reading = bno055.EulerOrientation; string orientationMessage = "Orientation: " + reading.Heading + " " + (-1 * reading.Roll) + " " + reading.Pitch; string calibrationMessage = "Calibration: " + BooleanToIntString(bno055.IsSystemCalibrated) + " " + BooleanToIntString(bno055.IsGyroscopeCalibrated) + " " + BooleanToIntString(bno055.IsAccelerometerCalibrated) + " " + BooleanToIntString(bno055.IsMagnetometerCalibrated); Debug.Print(orientationMessage); Debug.Print(calibrationMessage); WriteText(orientationMessage); WriteText(calibrationMessage); Thread.Sleep(100); } }
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); } } }
/// <summary> /// Initializes all hardware and software classes /// </summary> void Initialize() { Console.WriteLine("Initialize hardware..."); onboardLed = new RgbPwmLed(device: Device, redPwmPin: Device.Pins.OnboardLedRed, greenPwmPin: Device.Pins.OnboardLedGreen, bluePwmPin: Device.Pins.OnboardLedBlue, 3.3f, 3.3f, 3.3f, Meadow.Peripherals.Leds.IRgbLed.CommonType.CommonAnode); #if DEBUG Console.WriteLine("Initializing bus"); #endif bus = Device.CreateI2cBus(400000); #if DEBUG Console.WriteLine("Initializing display 1"); #endif displaySmall = new DisplayLCD(bus, 0x27, 2, 16); displaySmall.Write("I'm alive!"); #if DEBUG Console.WriteLine("Initializing display 2"); #endif displayBig = new DisplayLCD(bus, 0x23, 4, 20); displayBig.Write("I'm alive!"); #if DEBUG Console.WriteLine("Initializing hc12"); #endif displaySmall.Write("Initializing radio"); serialCom = new HC12Communication(Device.CreateSerialMessagePort(Device.SerialPortNames.Com4, suffixDelimiter: new byte[] { 10 }, preserveDelimiter: true, 115200, 8, Parity.None, StopBits.One)); com = new CommunicationWrapper(serialCom); #if DEBUG Console.WriteLine("Initializing WiFi"); #endif displaySmall.Write("Initializing WiFi"); ipCom = new WifiUdpCommunication(Device.WiFiAdapter, new Action <string>[] { com.SendMessage, displaySmall.Write }); //Trying to connect at startup to speed process up, but router might be still booting so in that case will try again when requested Task.Run(() => { try { ConnectToWiFi(""); } catch (Exception e) { com.SendMessage(ReturnCommandList.exception + e.Message + ReturnCommandList.exceptionTrace + e.StackTrace); #if DEBUG Console.WriteLine(e.Message + "/n/n" + e.StackTrace); #endif } }); #if DEBUG Console.WriteLine("Initializing RTC"); #endif displaySmall.Write("Initializing RTC"); clock = new Rtc(bus, com.SendMessage, SetClock); clock.SetClockFromRtc(); #if DEBUG Console.WriteLine("Initializing watchdog"); #endif displaySmall.Write("Initializing watchdog"); watchdog = new Watchdog(Watchdog.Type.SerialPort); //serialCom.RegisterWatchdog(watchdog.MessageReceived); //ipCom.RegisterWatchdog(watchdog.MessageReceived); watchdog.RegisterSender(com.SendMessage); watchdog.RegisterBlockAction(EmergencyDisable); watchdog.RegisterSwitchToSerial(SwitchToSerial); dict = new MethodsDictionary(); queue = new MethodsQueue(com, dict); ipCom.SubscribeToMessages(queue.MessageReceived); ipCom.SubscribeToMessages(watchdog.MessageReceived); serialCom.SubscribeToMessages(queue.MessageReceived); serialCom.SubscribeToMessages(watchdog.MessageReceived); #if DEBUG Console.WriteLine("Initializing pca @1600Hz"); #endif displaySmall.Write("Initializing PWM @1600Hz"); pwm1600 = new Pca9685(bus, 0x61, 1600); pwm1600.Initialize(); #if DEBUG Console.WriteLine("Initializing pca @50Hz"); #endif displaySmall.Write("Initializing PWM @50Hz"); pwm50 = new Pca9685(bus, 0x60, 50); pwm50.Initialize(); #if DEBUG Console.WriteLine("Initializing expander 1"); #endif displaySmall.Write("Initializing expander 1"); expander1 = new Mcp23x08(bus, 0x20, Device.CreateDigitalInputPort(Device.Pins.D02, InterruptMode.EdgeBoth)); #if DEBUG Console.WriteLine("Initializing expander 2"); #endif displaySmall.Write("Initializing expander 2"); expander2 = new Mcp23x08(bus, 0x21, Device.CreateDigitalInputPort(Device.Pins.D06, InterruptMode.EdgeBoth)); #if DEBUG Console.WriteLine("Initializing fans"); #endif displaySmall.Write("Initializing fans"); LEDsFans = new Fan(expander2.CreateDigitalOutputPort(expander2.Pins.GP4), "LEDs' fans"); motorsFans = new Fan(expander2.CreateDigitalOutputPort(expander2.Pins.GP5), "Motors' fans"); inasFan = new Fan(expander2.CreateDigitalOutputPort(expander2.Pins.GP6), "INAs fan"); #if DEBUG Console.WriteLine("Initializing buzzer"); #endif displaySmall.Write("Initializing buzzer"); buzzer = new Buzzer(expander2.CreateDigitalOutputPort(expander2.Pins.GP3, false, OutputType.OpenDrain)); watchdog.RegisterBuzzer(buzzer.Buzz); #pragma warning disable CS4014 // To wywołanie nie jest oczekiwane, dlatego wykonywanie bieżącej metody będzie kontynuowane do czasu ukończenia wywołania buzzer.Buzz(); #pragma warning restore CS4014 // To wywołanie nie jest oczekiwane, dlatego wykonywanie bieżącej metody będzie kontynuowane do czasu ukończenia wywołania #if DEBUG Console.WriteLine("Initializing power sensors"); #endif displaySmall.Write("Initializing power sensors"); INA219.INA219Configuration config = new INA219.INA219Configuration(INA219.BusVoltageRangeSettings.range32v, INA219.PGASettings.Gain320mV, INA219.ADCsettings.Samples128, INA219.ADCsettings.Samples128, INA219.ModeSettings.ShuntBusContinuous); ina219s = new INA219Array(new INA219[] { new INA219(3.2f, 0.1f, 1, bus, 0x41, config, "C"), new INA219(10f, 0.01f, 1, bus, 0x44, config, "L"), new INA219(10f, 0.01f, 1, bus, 0x45, config, "R") }, buzzer, displayBig, EmergencyDisable); ina219s.RegisterSender(com.SendMessage); ina219s.RegisterFan(inasFan.SetState); ina219s.RegisterFan(motorsFans.SetState); ina219s.StartMonitoringVoltage(); //com = new ComCommunication(Device.CreateSerialMessagePort(Device.SerialPortNames.Com4, suffixDelimiter: new byte[] { 10 }, preserveDelimiter: true, 921600, 8, Parity.None, StopBits.One)); #if DEBUG Console.WriteLine("Initializing temperature sensor"); #endif displaySmall.Write("Initializing temperature sensor"); tempPresSensor = new TempPressureSensor(bus); tempPresSensor.RegisterSender(com.SendMessage); #if DEBUG Console.WriteLine("Initializing position sensor"); #endif displaySmall.Write("Initializing position sensor"); positionSensor = new BNO055(bus, 0x28); positionSensor.RegisterSender(com.SendMessage); positionSensor.RegisterScreen(displaySmall.Write); #if DEBUG Console.WriteLine("Initializing motor controller"); #endif displaySmall.Write("Initializing motor controller"); motor = new MotorController(pwm1600.CreatePwmPort(3, 0), pwm1600.CreatePwmPort(2, 0), pwm1600.CreatePwmPort(1, 0), pwm1600.CreatePwmPort(0, 0), pwm50.CreatePwmPort(0), Device.CreateDigitalInputPort(Device.Pins.D03, InterruptMode.EdgeRising, ResistorMode.InternalPullDown, 20, 20), Device.CreateDigitalInputPort(Device.Pins.D04, InterruptMode.EdgeRising, ResistorMode.InternalPullDown, 20, 20), positionSensor, gimbal, queue.ClearQueue); #if DEBUG Console.WriteLine("Initializing gimbal"); #endif displaySmall.Write("Initializing camera gimbal"); gimbal = new CameraGimbal(pwm50.CreatePwmPort(2), pwm50.CreatePwmPort(1), positionSensor); #if DEBUG Console.WriteLine("Initializing proximity sensors"); #endif displaySmall.Write("Initializing proximity sensors"); proxSensors = new ProximitySensorsArray(new ProximitySensor[] { new ProximitySensor(expander1.CreateDigitalInputPort(expander1.Pins.GP0, InterruptMode.EdgeRising, ResistorMode.InternalPullUp, 500, 500), Direction.Forward, StopBehavior.Stop, "Front, right", queue, motor), new ProximitySensor(expander1.CreateDigitalInputPort(expander1.Pins.GP1, InterruptMode.EdgeRising, ResistorMode.InternalPullUp, 500, 500), Direction.Forward, StopBehavior.Stop, "Front, left", queue, motor), new ProximitySensor(expander1.CreateDigitalInputPort(expander1.Pins.GP2, InterruptMode.EdgeRising, ResistorMode.InternalPullUp, 500, 500), Direction.Forward, StopBehavior.Stop, "Front, center", queue, motor), new ProximitySensor(expander1.CreateDigitalInputPort(expander1.Pins.GP3, InterruptMode.EdgeRising, ResistorMode.InternalPullUp, 500, 500), Direction.Backward, StopBehavior.Stop, "Back, center", queue, motor) }); proxSensors.Register(com.SendMessage); //proxSensors.Register(displaySmall.Write); #if DEBUG Console.WriteLine("Initializing lamps"); #endif displaySmall.Write("Initializing lamps"); narrowLed = new LedLamp(pwm1600.CreatePwmPort(4, 0), LEDsFans, "Front, narrow"); wideLed = new LedLamp(pwm1600.CreatePwmPort(5, 0), LEDsFans, "Front, wide"); #if DEBUG Console.WriteLine("Initializing cameras"); #endif displaySmall.Write("Initializing cameras"); camera = new Camera(expander2.CreateDigitalOutputPort(expander2.Pins.GP7)); camera.SetCamera(true); #if DEBUG Console.WriteLine("Finishing initialization"); #endif displaySmall.Write("Finishing initialization"); RegisterMethods(); //watchdog.StartCheckingMessages(); #if DEBUG Console.WriteLine("All hardware initialized!"); #endif displaySmall.Clear(); displaySmall.Write("Ready!"); com.SendMessage("Ready!"); #pragma warning disable CS4014 // To wywołanie nie jest oczekiwane, dlatego wykonywanie bieżącej metody będzie kontynuowane do czasu ukończenia wywołania buzzer.BuzzPulse(100, 100, 3); #pragma warning restore CS4014 // To wywołanie nie jest oczekiwane, dlatego wykonywanie bieżącej metody będzie kontynuowane do czasu ukończenia wywołania }