public static void ResetMotorHat() { DC1Helper = null; DC2Helper = null; DC3Helper = null; DC4Helper = null; mh = null; driver = null; driver = new I2cDriver(ConnectorPin.P1Pin3.ToProcessor(), ConnectorPin.P1Pin5.ToProcessor()); mh = new Motor_Hat(driver, 0x60, 50); DC1Helper = new MotorHelper(mh.GetMotor(1)); DC2Helper = new MotorHelper(mh.GetMotor(2)); DC3Helper = new MotorHelper(mh.GetMotor(3)); DC4Helper = new MotorHelper(mh.GetMotor(4)); Console.WriteLine("Resetted Motor Hat"); }
public static void motorCalcs(float speed, float wheelPos1, float wheelPos2, int freq = 500) { Motor_Hat mh = new Motor_Hat (0x60, freq); var DC1 = mh.GetMotor (1); var DC2 = mh.GetMotor (2); var DC1Helper = new MotorHelper (DC1); var DC2Helper = new MotorHelper (DC2); int DC1motorSpeed = 0, DC2motorSpeed = 0; if (speed > 0) { DC1motorSpeed = Convert.ToInt32 (Math.Floor ((speed * 2.55) - (wheelPos1 * 2.55))); DC2motorSpeed = Convert.ToInt32 (Math.Floor ((speed * 2.55) - (wheelPos2 * 2.55))); } else if (speed < 0) { DC1motorSpeed = Convert.ToInt32 (Math.Floor ((speed * 2.55) + (wheelPos1 * 2.55))); DC2motorSpeed = Convert.ToInt32 (Math.Floor ((speed * 2.55) + (wheelPos2 * 2.55))); } if(speed == 0){ if(wheelPos1 > 0){ DC1motorSpeed = Convert.ToInt32(Math.Floor((wheelPos1 * 2.55) * -1 + 1)); DC2motorSpeed = Convert.ToInt32(Math.Floor((wheelPos1 * 2.55))); } if(wheelPos2 > 0){ DC1motorSpeed = Convert.ToInt32(Math.Floor((wheelPos2 * 2.55))); DC2motorSpeed = Convert.ToInt32(Math.Floor((wheelPos2 * 2.55) * -1 + 1)); } } Console.WriteLine ( "Left motor: {0}\n" + "Right motor: {1}\n", DC1motorSpeed, DC2motorSpeed); if (DC1motorSpeed > 255 || DC2motorSpeed > 255) { DC1motorSpeed -= 255; DC2motorSpeed -= 255; } if(DC1motorSpeed < -255 || DC2motorSpeed < -255) { DC1motorSpeed += 255; DC2motorSpeed += 255; } if(DC1motorSpeed < 0) DC1Helper.Backward(DC1motorSpeed); if(DC1motorSpeed > 0) DC1Helper.Forward(DC1motorSpeed); if(DC1motorSpeed == 0) DC1Helper.Stop(); if(DC2motorSpeed < 0) DC2Helper.Backward(DC2motorSpeed); if(DC2motorSpeed > 0) DC2Helper.Forward(DC2motorSpeed); if(DC2motorSpeed == 0) DC2Helper.Stop(); }