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();
}
