/// <summary>
/// Sets the mode of the motor
/// </summary>
/// <param name="command">mode</param>
public void run(int command)
{
if (MC == null)
{
return;
}
if (command == Motor_Hat.MFORWARD)
{
MC.setPin(IN2pin, 0);
MC.setPin(IN1pin, 1);
}
if (command == Motor_Hat.MBACKWARD)
{
MC.setPin(IN1pin, 0);
MC.setPin(IN2pin, 1);
}
if (command == Motor_Hat.MRELEASE)
{
MC.setPin(IN1pin, 0);
MC.setPin(IN2pin, 0);
}
}
private int oneStep(int dir, int style)
{
int pwm_a = 255, pwm_b = 255;
// first determine what sort of stepping procedure we're up to
if (style == Motor_Hat.SSINGLE)
{
if ((currentstep / (MICROSTEPS / 2) % 2) != 0)
{
// we're at an odd step, weird
if (dir == Motor_Hat.MFORWARD)
{
currentstep += MICROSTEPS / 2;
}
else
{
currentstep -= MICROSTEPS / 2;
}
}
else
{
// go to next even step
if (dir == Motor_Hat.MFORWARD)
{
currentstep += MICROSTEPS;
}
else
{
currentstep -= MICROSTEPS;
}
}
}
if (style == Motor_Hat.SDOUBLE)
{
if ((currentstep / (MICROSTEPS / 2) % 2) == 0)
{
// we're at an even step, weird
if (dir == Motor_Hat.MFORWARD)
{
currentstep += MICROSTEPS / 2;
}
else
{
currentstep -= MICROSTEPS / 2;
}
}
else
{
// go to next odd step
if (dir == Motor_Hat.MFORWARD)
{
currentstep += MICROSTEPS;
}
else
{
currentstep -= MICROSTEPS;
}
}
}
if (style == Motor_Hat.SINTERLEAVE)
{
if (dir == Motor_Hat.MFORWARD)
{
currentstep += MICROSTEPS / 2;
}
else
{
currentstep -= MICROSTEPS / 2;
}
}
if (style == Motor_Hat.SMICROSTEP)
{
if (dir == Motor_Hat.MFORWARD)
{
currentstep += 1;
}
else
{
currentstep -= 1;
}
// go to next 'step' and wrap around
currentstep += MICROSTEPS * 4;
currentstep %= MICROSTEPS * 4;
pwm_a = pwm_b = 0;
if ((currentstep >= 0) && (currentstep < MICROSTEPS))
{
pwm_a = MICROSTEP_CURVE [MICROSTEPS - currentstep];
pwm_b = MICROSTEP_CURVE [currentstep];
}
else if ((currentstep >= MICROSTEPS) && (currentstep < MICROSTEPS * 2))
{
pwm_a = MICROSTEP_CURVE [currentstep - MICROSTEPS];
pwm_b = MICROSTEP_CURVE [MICROSTEPS * 2 - currentstep];
}
else if ((currentstep >= MICROSTEPS * 2) && (currentstep < MICROSTEPS * 3))
{
pwm_a = MICROSTEP_CURVE [MICROSTEPS * 3 - currentstep];
pwm_b = MICROSTEP_CURVE [currentstep - MICROSTEPS * 2];
}
else if ((currentstep >= MICROSTEPS * 3) && (currentstep < MICROSTEPS * 4))
{
pwm_a = MICROSTEP_CURVE [currentstep - MICROSTEPS * 3];
pwm_b = MICROSTEP_CURVE [MICROSTEPS * 4 - currentstep];
}
}
// go to next 'step' and wrap around
currentstep += MICROSTEPS * 4;
currentstep %= MICROSTEPS * 4;
// only really used for microstepping, otherwise always on!
MC._pwm.SetPwm(PWMA, 0, pwm_a * 16);
MC._pwm.SetPwm(PWMB, 0, pwm_b * 16);
int[] coils = { 0, 0, 0, 0 };
int[] newCoils = null;
if (style == Motor_Hat.SMICROSTEP)
{
if ((currentstep >= 0) && (currentstep < MICROSTEPS))
{
newCoils = new int[] { 1, 1, 0, 0 };
}
else if ((currentstep >= MICROSTEPS) && (currentstep < MICROSTEPS * 2))
{
newCoils = new int[] { 0, 1, 1, 0 };
}
else if ((currentstep >= MICROSTEPS * 2) && (currentstep < MICROSTEPS * 3))
{
newCoils = new int[] { 0, 0, 1, 1 };
}
else if ((currentstep >= MICROSTEPS * 3) && (currentstep < MICROSTEPS * 4))
{
newCoils = new int[] { 1, 0, 0, 1 };
}
else
{
Console.WriteLine("Unknown error. Printing vars for troubleshooting\n" +
"Currentstep: {0}\n" +
"MICROSTEPS: {1}", currentstep, MICROSTEPS);
}
Array.Copy(newCoils, 0, coils, 1, 4);
}
else
{
int[][] step2coils = new int[8][] {
new int[] { 1, 0, 0, 0 },
new int[] { 1, 1, 0, 0 },
new int[] { 0, 1, 0, 0 },
new int[] { 0, 1, 1, 0 },
new int[] { 0, 0, 1, 0 },
new int[] { 0, 0, 1, 1 },
new int[] { 0, 0, 0, 1 },
new int[] { 1, 0, 0, 1 }
};
coils = step2coils[currentstep / (MICROSTEPS / 2)];
}
Console.WriteLine("coils state = " + coils);
MC.setPin(AIN2, coils [0]);
MC.setPin(BIN1, coils [1]);
MC.setPin(AIN1, coils [2]);
MC.setPin(BIN2, coils [3]);
return(currentstep);
}