/// <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
}
