Hi everyone!
I am novice in this but I am working on a project that involves controlling 3 DC motors, 2 DC servos and 1 DC Stepper. I have purchased Adafruit Motor Shield and trying to port Netduino driver for it with limited success.
Here is original Netduino driver:
using System;
using System.Threading;
using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;
using SecretLabs.NETMF.Hardware;
using SecretLabs.NETMF.Hardware.Netduino;
namespace NetduinoApplication1
{
// taken from http://forums.netduino.com/index.php?/topic/780-netduino-with-adafruit-motor-shield/
// As mentioned previously, the Netduino only has 4 PWM pins.
// On the Adafruit MotorShield pins 5 and 6 are connected to M3/M4,
// while 9 and 10 are connected to the servo headers in the top left.
public sealed class MotorShield
{
public const Cpu.Pin PWM_0A = Pins.GPIO_PIN_D6; // M4
public const Cpu.Pin PWM_0B = Pins.GPIO_PIN_D5; // M3
private static MotorShield _instance = new MotorShield();
public static MotorShield Instance { get { return _instance; } }
private OutputPort _motorLatch;
private OutputPort _motorClock;
private OutputPort _motorEnable;
private OutputPort _motorData;
internal byte LatchState = 0x00;
private MotorShield()
{
_motorLatch = new OutputPort(Pins.GPIO_PIN_D12, false);
_motorClock = new OutputPort(Pins.GPIO_PIN_D4, false);
_motorEnable = new OutputPort(Pins.GPIO_PIN_D7, false);
_motorData = new OutputPort(Pins.GPIO_PIN_D8, false);
}
internal void LatchTx()
{
//LATCH_PORT &= ~_BV(LATCH);
_motorLatch.Write(false);
//SER_PORT &= ~_BV(SER);
_motorData.Write(false);
for (int i = 0; i < 8; i++)
{
//CLK_PORT &= ~_BV(CLK);
_motorClock.Write(false);
int mask = (1 << (7 - i));
if ((LatchState & mask) != 0)
{
//SER_PORT |= _BV(SER);
_motorData.Write(true);
}
else
{
//SER_PORT &= ~_BV(SER);
_motorData.Write(false);
}
//CLK_PORT |= _BV(CLK);
_motorClock.Write(true);
}
//LATCH_PORT |= _BV(LATCH);
_motorLatch.Write(true);
}
}
public sealed class DcMotor
{
private PWM _pwm;
private byte _motorBitA, _motorBitB;
public DcMotor(MotorHeaders header)
{
switch (header)
{
case MotorHeaders.M3:
_motorBitA = (int)MotorBits.Motor3_A;
_motorBitB = (int)MotorBits.Motor3_B;
_pwm = new PWM(MotorShield.PWM_0B);
break;
case MotorHeaders.M4:
_motorBitA = (int)MotorBits.Motor4_A;
_motorBitB = (int)MotorBits.Motor4_B;
_pwm = new PWM(MotorShield.PWM_0A);
break;
default:
throw new InvalidOperationException("Invalid motor header specified.");
}
MotorShield.Instance.LatchState &= (byte)(~(1 << _motorBitA) & ~(1 << _motorBitB));
MotorShield.Instance.LatchTx();
_pwm.SetPulse(100, 0);
}
public void Run(MotorDirection dir)
{
switch (dir)
{
case MotorDirection.Release:
MotorShield.Instance.LatchState &= (byte)(~(1 << _motorBitA));
MotorShield.Instance.LatchState &= (byte)(~(1 << _motorBitB));
break;
case MotorDirection.Forward:
MotorShield.Instance.LatchState |= (byte)(1 << _motorBitA);
MotorShield.Instance.LatchState &= (byte)(~(1 << _motorBitB));
break;
case MotorDirection.Reverse:
MotorShield.Instance.LatchState &= (byte)(~(1 << _motorBitA));
MotorShield.Instance.LatchState |= (byte)(1 << _motorBitB);
break;
default:
throw new InvalidOperationException("Invalid motor direction specified");
}
MotorShield.Instance.LatchTx();
}
public void SetSpeed(uint speed)
{
if (speed > 100)
{
speed = 100;
}
_pwm.SetDutyCycle(speed);
}
}
public sealed class Stepper
{
#if MICROSTEPS_8
private static byte[] microstepCurve = {0, 50, 98, 142, 180, 212, 236, 250, 255};
#else
private static byte[] microstepCurve = { 0, 25, 50, 74, 98, 120, 141, 162, 180, 197, 212, 225, 236, 244, 250, 253, 255 };
#endif
/// <summary>
/// Steps per revolution
/// </summary>
public readonly uint StepsPerRevolution;
/// <summary>
/// The port to which the stepper is connected
/// </summary>
public readonly StepperPorts stepperPort;
private uint usPerStep, stepCounter;
private uint currentStep = 0;
private PWM coilA, coilB;
private uint microsteps;
public enum StepperPorts
{
//M1_M2,
M3_M4,
}
public enum StepType
{
/// <summary>
/// Single Coil activation
/// </summary>
Single,
/// <summary>
/// Double Coil activation
/// </summary>
/// <remarks>Higher torque than Single</remarks>
Double,
/// <summary>
/// Alternating between Single and Double.
/// </summary>
/// <remarks>Twice the resolution, but half the speed</remarks>
Interleave,
/// <summary>
///
/// </summary>
Microstep,
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="stepsPerRev">The number of steps per complete revolution of the output shaft</param>
/// <param name="port">The port to which the stepper is connected</param>
public Stepper(uint stepsPerRev, StepperPorts port)
{
StepsPerRevolution = stepsPerRev;
stepperPort = port;
currentStep = 0;
microsteps = (uint)microstepCurve.Length;
int latchState = 0;
switch (stepperPort)
{
/*
case StepperPorts.M1_M2:
// Turn off all motor pins
latchState &= ~(1 << (int)MotorBits.Motor1_A) &
~(1 << (int)MotorBits.Motor1_B) &
~(1 << (int)MotorBits.Motor2_A) &
~(1 << (int)MotorBits.Motor2_B);
coilA = new PWM(PwmPins.pwm2A);
coilB = new PWM(PwmPins.pwm2B);
break;
*/
case StepperPorts.M3_M4:
// turn off all motor pins
latchState &= ~(1 << (int)MotorBits.Motor3_A) &
~(1 << (int)MotorBits.Motor3_B) &
~(1 << (int)MotorBits.Motor4_A) &
~(1 << (int)MotorBits.Motor4_B);
coilA = new PWM(MotorShield.PWM_0B);
coilB = new PWM(MotorShield.PWM_0A);
break;
default:
throw new InvalidOperationException("Invalid motor header specified");
}
MotorShield.Instance.LatchState = (byte)latchState;
MotorShield.Instance.LatchTx(); // Enable channels
coilA.SetPulse(1000000 / 64000, 0); // 64KHz microstep pwm
coilB.SetPulse(1000000 / 64000, 0); // 64KHz microstep pwm
}
/// <summary>
/// Move the stepper a specific number of steps
/// </summary>
/// <param name="steps">How many steps to move</param>
/// <param name="dir">The direction in which to rotate</param>
/// <param name="style">The type of stepping to perform</param>
public void Step(uint steps, MotorDirection dir, StepType style = StepType.Single)
{
uint uspers = usPerStep;
uint ret = 0;
if (style == StepType.Interleave)
{
uspers /= 2;
}
else if (style == StepType.Microstep)
{
uspers /= microsteps;
steps *= microsteps;
}
while (steps-- > 0)
{
ret = OneStep(dir, style);
Thread.Sleep((int)uspers / 1000); // in ms
stepCounter += (uspers % 1000);
if (stepCounter >= 1000)
{
Thread.Sleep(1);
stepCounter -= 1000;
}
}
if (style == StepType.Microstep)
{
while ((ret != 0) && (ret != microsteps))
{
ret = OneStep(dir, style);
Thread.Sleep((int)uspers / 1000); // in ms
stepCounter += (uspers % 1000);
if (stepCounter >= 1000)
{
Thread.Sleep(1);
stepCounter -= 1000;
}
}
}
}
/// <summary>
/// Sets the stepper speed
/// </summary>
/// <param name="rpm">The speed in revolutions per minute</param>
public void SetSpeed(uint rpm)
{
usPerStep = 60000000 / (StepsPerRevolution * rpm);
stepCounter = 0;
}
/// <summary>
/// Movse the stepper one step
/// </summary>
/// <param name="dir">The direction in which to move</param>
/// <param name="style">The type of stepping to use</param>
/// <returns>Current step count</returns>
protected uint OneStep(MotorDirection dir, StepType style = StepType.Single)
{
byte a, b, c, d;
byte ocrb, ocra;
ocra = ocrb = 255;
switch (stepperPort)
{
/*
case StepperPorts.M1_M2;
a = (1<<(int)MotorBits.Motor1_A);
b = (1<<(int)MotorBits.Motor2_A);
c = (1<<(int)MotorBits.Motor1_B);
d = (1<<(int)MotorBits.Motor2_B);
break;
*/
case StepperPorts.M3_M4:
a = (1 << (int)MotorBits.Motor3_A);
b = (1 << (int)MotorBits.Motor4_A);
c = (1 << (int)MotorBits.Motor3_B);
d = (1 << (int)MotorBits.Motor4_B);
break;
default:
return 0;
}
// next determine what sort of stepping procedure we're up to
if (style == StepType.Single)
{
if ((currentStep / (microsteps / 2)) % 2 == 0) // we're at an odd step, weird
{
if (dir == MotorDirection.Forward)
{
currentStep += microsteps / 2;
}
else
{
currentStep -= microsteps / 2;
}
}
else // go to the next even step
{
if (dir == MotorDirection.Forward)
{
currentStep += microsteps;
}
else
{
currentStep -= microsteps;
}
}
}
else if (style == StepType.Double)
{
if ((currentStep / (microsteps / 2) % 2) != 0) // we're at an even step, weird
{
if (dir == MotorDirection.Forward)
{
currentStep += microsteps / 2;
}
else
{
currentStep -= microsteps / 2;
}
}
else // go to the next odd step
{
if (dir == MotorDirection.Forward)
{
currentStep += microsteps;
}
else
{
currentStep -= microsteps;
}
}
}
else if (style == StepType.Interleave)
{
if (dir == MotorDirection.Forward)
{
currentStep += microsteps / 2;
}
else
{
currentStep -= microsteps / 2;
}
}
if (style == StepType.Microstep)
{
if (dir == MotorDirection.Forward)
{
currentStep++;
}
else
{
// BACKWARDS
currentStep--;
}
currentStep += microsteps * 4;
currentStep %= microsteps * 4;
ocra = ocrb = 0;
if ((currentStep >= 0) && (currentStep < microsteps))
{
ocra = microstepCurve[microsteps - currentStep];
ocrb = microstepCurve[currentStep];
}
else if ((currentStep >= microsteps) && (currentStep < microsteps * 2))
{
ocra = microstepCurve[currentStep - microsteps];
ocrb = microstepCurve[microsteps * 2 - currentStep];
}
else if ((currentStep >= microsteps * 2) && (currentStep < microsteps * 3))
{
ocra = microstepCurve[microsteps * 3 - currentStep];
ocrb = microstepCurve[currentStep - microsteps * 2];
}
else if ((currentStep >= microsteps * 3) && (currentStep < microsteps * 4))
{
ocra = microstepCurve[currentStep - microsteps * 3];
ocrb = microstepCurve[microsteps * 4 - currentStep];
}
}
currentStep += microsteps * 4;
currentStep %= microsteps * 4;
coilA.SetDutyCycle(ocra);
coilB.SetDutyCycle(ocrb);
// release all
MotorShield.Instance.LatchState &= (byte)(~a & ~b & ~c & ~d); // all motor pins to 0
//Serial.println(step, DEC);
if (style == StepType.Microstep)
{
if ((currentStep >= 0) && (currentStep < microsteps))
MotorShield.Instance.LatchState |= (byte)(a | b);
if ((currentStep >= microsteps) && (currentStep < microsteps * 2))
MotorShield.Instance.LatchState |= (byte)(b | c);
if ((currentStep >= microsteps * 2) && (currentStep < microsteps * 3))
MotorShield.Instance.LatchState |= (byte)(c | d);
if ((currentStep >= microsteps * 3) && (currentStep < microsteps * 4))
MotorShield.Instance.LatchState |= (byte)(d | a);
}
else
{
switch (currentStep / (microsteps / 2))
{
case 0:
MotorShield.Instance.LatchState |= (byte)(a); // energize coil 1 only
break;
case 1:
MotorShield.Instance.LatchState |= (byte)(a | b); // energize coil 1+2
break;
case 2:
MotorShield.Instance.LatchState |= (byte)(b); // energize coil 2 only
break;
case 3:
MotorShield.Instance.LatchState |= (byte)(b | c); // energize coil 2+3
break;
case 4:
MotorShield.Instance.LatchState |= (byte)(c); // energize coil 3 only
break;
case 5:
MotorShield.Instance.LatchState |= (byte)(c | d); // energize coil 3+4
break;
case 6:
MotorShield.Instance.LatchState |= (byte)(d); // energize coil 4 only
break;
case 7:
MotorShield.Instance.LatchState |= (byte)(d | a); // energize coil 1+4
break;
}
}
MotorShield.Instance.LatchTx();
return currentStep;
}
/// <summary>
/// Releases the motor, allowing it to spin freely
/// </summary>
public void Release()
{
int latchState = 0;
switch (stepperPort)
{
/*
case StepperPorts.M1_M2:
// Turn off all motor pins
latchState &= ~(1 << (int)MotorBits.Motor1_A) &
~(1 << (int)MotorBits.Motor1_B) &
~(1 << (int)MotorBits.Motor2_A) &
~(1 << (int)MotorBits.Motor2_B);
break;
*/
case StepperPorts.M3_M4:
// turn off all motor pins
latchState &= ~(1 << (int)MotorBits.Motor3_A) &
~(1 << (int)MotorBits.Motor3_B) &
~(1 << (int)MotorBits.Motor4_A) &
~(1 << (int)MotorBits.Motor4_B);
break;
default:
throw new InvalidOperationException("Invalid motor header specified");
}
MotorShield.Instance.LatchState = (byte)latchState;
MotorShield.Instance.LatchTx(); // disable channels
// Ste speed to 0
coilA.SetDutyCycle(0);
coilB.SetDutyCycle(0);
}
}
public enum MotorBits
{
Motor4_A = 0,
Motor4_B = 6,
Motor3_A = 5,
Motor3_B = 7
}
public enum MotorDirection
{
Release, Forward, Reverse
}
public enum MotorHeaders
{
M3, M4
}
}
Here is my code:
using System;
using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;
using System.Threading;
using GHIElectronics.NETMF.Hardware;
using GHIElectronics.NETMF.FEZ;
namespace GHIElectronics.NETMF
{
// taken from http://forums.netduino.com/index.php?/topic/780-netduino-with-adafruit-motor-shield/
// As mentioned previously, the Netduino only has 4 PWM pins.
// On the Adafruit MotorShield pins 5 and 6 are connected to M3/M4,
// while 9 and 10 are connected to the servo headers in the top left.
public sealed class MotorShield
{
public const Cpu.Pin PWM_0A = (Cpu.Pin)FEZ_Pin.PWM.Di10; // Pins.GPIO_PIN_D11; // M2
public const Cpu.Pin PWM_0B = (Cpu.Pin)FEZ_Pin.PWM.Di9; //Pins.GPIO_PIN_D3; // M1
public const Cpu.Pin PWM_1A = (Cpu.Pin)FEZ_Pin.PWM.Di5; // Pins.GPIO_PIN_D6; // M4
public const Cpu.Pin PWM_1B = (Cpu.Pin)FEZ_Pin.PWM.Di6; //Pins.GPIO_PIN_D5; // M3
// public PWM Coil0A; // M2
// public PWM Coil0B; // M1
// public PWM Coil1A; // M4
// public PWM Coil1B; // M3
private static MotorShield _instance = new MotorShield();
public static MotorShield Instance { get { return _instance; } }
private OutputPort _motorLatch;
private OutputPort _motorClock;
private OutputPort _motorEnable;
private OutputPort _motorData;
internal byte LatchState = 0x00;
private MotorShield()
{
_motorLatch = new OutputPort((Cpu.Pin)FEZ_Pin.Digital.Di12, false);
_motorClock = new OutputPort((Cpu.Pin)FEZ_Pin.Digital.Di4, false);
_motorEnable = new OutputPort((Cpu.Pin)FEZ_Pin.Digital.Di7, false);
_motorData = new OutputPort((Cpu.Pin)FEZ_Pin.Digital.Di11, false);
// Coil0A = new PWM((PWM.Pin)FEZ_Pin.PWM.Di10); // M2
// Coil0B = new PWM((PWM.Pin)FEZ_Pin.PWM.Di5); // M1
// Coil1A = new PWM((PWM.Pin)FEZ_Pin.PWM.Di9); // M4
// Coil1B = new PWM((PWM.Pin)FEZ_Pin.PWM.IO4); // M3
//_motorData = new OutputPort((Cpu.Pin)FEZ_Pin.Digital.Di8, false);
}
internal void LatchTx()
{
//LATCH_PORT &= ~_BV(LATCH);
_motorLatch.Write(false);
//SER_PORT &= ~_BV(SER);
_motorData.Write(false);
for (int i = 0; i < 8; i++)
{
//CLK_PORT &= ~_BV(CLK);
_motorClock.Write(false);
int mask = (1 << (7 - i));
if ((LatchState & mask) != 0)
{
//SER_PORT |= _BV(SER);
_motorData.Write(true);
}
else
{
//SER_PORT &= ~_BV(SER);
_motorData.Write(false);
}
//CLK_PORT |= _BV(CLK);
_motorClock.Write(true);
}
//LATCH_PORT |= _BV(LATCH);
_motorLatch.Write(true);
}
}
public sealed class DcMotor
{
private PWM _pwm;
private byte _motorBitA, _motorBitB;
public DcMotor(MotorHeaders header)
{
switch (header)
{
case MotorHeaders.M1:
_motorBitA = (int)MotorBits.Motor1_A;
_motorBitB = (int)MotorBits.Motor1_B;
_pwm = new PWM((PWM.Pin)MotorShield.PWM_0B);
//_pwm = new PWM(MotorShield.PWM_0B);
break;
case MotorHeaders.M2:
_motorBitA = (int)MotorBits.Motor2_A;
_motorBitB = (int)MotorBits.Motor2_B;
_pwm = new PWM((PWM.Pin)MotorShield.PWM_0A);
//_pwm = new PWM(MotorShield.PWM_0B);
break;
case MotorHeaders.M3:
_motorBitA = (int)MotorBits.Motor3_A;
_motorBitB = (int)MotorBits.Motor3_B;
_pwm = new PWM((PWM.Pin)MotorShield.PWM_1B);
//_pwm = new PWM(MotorShield.PWM_0B);
break;
case MotorHeaders.M4:
_motorBitA = (int)MotorBits.Motor4_A;
_motorBitB = (int)MotorBits.Motor4_B;
_pwm = new PWM((PWM.Pin)MotorShield.PWM_1A);
break;
default:
throw new InvalidOperationException("Invalid motor header specified.");
}
MotorShield.Instance.LatchState &= (byte)(~(1 << _motorBitA) & ~(1 << _motorBitB));
MotorShield.Instance.LatchTx();
_pwm.SetPulse(100, 0);
}
public void Run(MotorDirection dir)
{
switch (dir)
{
case MotorDirection.Release:
MotorShield.Instance.LatchState &= (byte)(~(1 << _motorBitA));
MotorShield.Instance.LatchState &= (byte)(~(1 << _motorBitB));
break;
case MotorDirection.Forward:
MotorShield.Instance.LatchState |= (byte)(1 << _motorBitA);
MotorShield.Instance.LatchState &= (byte)(~(1 << _motorBitB));
break;
case MotorDirection.Reverse:
MotorShield.Instance.LatchState &= (byte)(~(1 << _motorBitA));
MotorShield.Instance.LatchState |= (byte)(1 << _motorBitB);
break;
default:
throw new InvalidOperationException("Invalid motor direction specified");
}
MotorShield.Instance.LatchTx();
}
public void SetSpeed(uint speed)
{
if (speed > 100)
{
speed = 100;
}
//_pwm.Set(0, (byte)speed);
_pwm.Set(500, (byte)speed);
//_pwm.SetDutyCycle(speed);
}
}
public sealed class Stepper
{
#if MICROSTEPS_8
private static byte[] microstepCurve = {0, 50, 98, 142, 180, 212, 236, 250, 255};
#else
private static byte[] microstepCurve = { 0, 25, 50, 74, 98, 120, 141, 162, 180, 197, 212, 225, 236, 244, 250, 253, 255 };
#endif
/// <summary>
/// Steps per revolution
/// </summary>
public readonly uint StepsPerRevolution;
/// <summary>
/// The port to which the stepper is connected
/// </summary>
public readonly StepperPorts stepperPort;
private uint usPerStep, stepCounter;
private uint currentStep = 0;
private PWM coilA, coilB;
private uint microsteps;
// private MotorShield AFMotorSheild;
public enum StepperPorts
{
M1_M2,
M3_M4,
}
public enum StepType
{
/// <summary>
/// Single Coil activation
/// </summary>
Single,
/// <summary>
/// Double Coil activation
/// </summary>
/// <remarks>Higher torque than Single</remarks>
Double,
/// <summary>
/// Alternating between Single and Double.
/// </summary>
/// <remarks>Twice the resolution, but half the speed</remarks>
Interleave,
/// <summary>
///
/// </summary>
Microstep,
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="stepsPerRev">The number of steps per complete revolution of the output shaft</param>
/// <param name="port">The port to which the stepper is connected</param>
public Stepper(uint stepsPerRev, StepperPorts port)
{
StepsPerRevolution = stepsPerRev;
stepperPort = port;
currentStep = 0;
microsteps = (uint)microstepCurve.Length;
// AFMotorSheild = MotorShield.Instance;
int latchState = 0;
switch (stepperPort)
{
case StepperPorts.M1_M2:
// Turn off all motor pins
latchState &= ~(1 << (int)MotorBits.Motor1_A) &
~(1 << (int)MotorBits.Motor1_B) &
~(1 << (int)MotorBits.Motor2_A) &
~(1 << (int)MotorBits.Motor2_B);
//coilA = new PWM(PwmPins.pwm2A);
//coilB = new PWM(PwmPins.pwm2B);
coilA = new PWM((PWM.Pin)MotorShield.PWM_0B);
coilB = new PWM((PWM.Pin)MotorShield.PWM_0A);
//coilA = AFMotorSheild.Coil0A;
//coilB = AFMotorSheild.Coil0B;
break;
case StepperPorts.M3_M4:
// turn off all motor pins
latchState &= ~(1 << (int)MotorBits.Motor3_A) &
~(1 << (int)MotorBits.Motor3_B) &
~(1 << (int)MotorBits.Motor4_A) &
~(1 << (int)MotorBits.Motor4_B);
coilA = new PWM((PWM.Pin)MotorShield.PWM_1B);
coilB = new PWM((PWM.Pin)MotorShield.PWM_1A);
// coilA = new PWM(MotorShield.PWM_0B);
//coilB = new PWM(MotorShield.PWM_0A);
//coilA = AFMotorSheild.Coil1A;
//coilB = AFMotorSheild.Coil1B;
break;
default:
throw new InvalidOperationException("Invalid motor header specified");
}
// AFMotorSheild.LatchState = (byte)latchState;
// AFMotorSheild.LatchTx(); // Enable channels
MotorShield.Instance.LatchState = (byte)latchState;
MotorShield.Instance.LatchTx(); // Enable channels
// 600 pps max !!!
coilA.SetPulse(1000000 / 64000, 0); // 64KHz microstep pwm
coilB.SetPulse(1000000 / 64000, 0); // 64KHz microstep pwm
}
/// <summary>
/// Move the stepper a specific number of steps
/// </summary>
/// <param name="steps">How many steps to move</param>
/// <param name="dir">The direction in which to rotate</param>
/// <param name="style">The type of stepping to perform</param>
public void Step(uint steps, MotorDirection dir, StepType style = StepType.Single)
{
uint uspers = usPerStep;
uint ret = 0;
if (style == StepType.Interleave)
{
uspers /= 2;
}
else if (style == StepType.Microstep)
{
uspers /= microsteps;
steps *= microsteps;
}
while (steps-- > 0)
{
ret = OneStep(dir, style);
Thread.Sleep((int)uspers / 1000); // in ms
stepCounter += (uspers % 1000);
if (stepCounter >= 1000)
{
Thread.Sleep(1);
stepCounter -= 1000;
}
}
if (style == StepType.Microstep)
{
while ((ret != 0) && (ret != microsteps))
{
ret = OneStep(dir, style);
Thread.Sleep((int)uspers / 1000); // in ms
stepCounter += (uspers % 1000);
if (stepCounter >= 1000)
{
Thread.Sleep(1);
stepCounter -= 1000;
}
}
}
}
/// <summary>
/// Sets the stepper speed
/// </summary>
/// <param name="rpm">The speed in revolutions per minute</param>
public void SetSpeed(uint rpm)
{
usPerStep = 60000000 / (StepsPerRevolution * rpm);
stepCounter = 0;
}
/// <summary>
/// Movse the stepper one step
/// </summary>
/// <param name="dir">The direction in which to move</param>
/// <param name="style">The type of stepping to use</param>
/// <returns>Current step count</returns>
protected uint OneStep(MotorDirection dir, StepType style = StepType.Single)
{
byte a, b, c, d;
byte ocrb, ocra;
ocra = ocrb = 255;
switch (stepperPort)
{
case StepperPorts.M1_M2:
a = (1 << (int)MotorBits.Motor1_A);
b = (1 << (int)MotorBits.Motor2_A);
c = (1 << (int)MotorBits.Motor1_B);
d = (1 << (int)MotorBits.Motor2_B);
break;
case StepperPorts.M3_M4:
a = (1 << (int)MotorBits.Motor3_A);
b = (1 << (int)MotorBits.Motor4_A);
c = (1 << (int)MotorBits.Motor3_B);
d = (1 << (int)MotorBits.Motor4_B);
break;
default:
return 0;
}
// next determine what sort of stepping procedure we're up to
if (style == StepType.Single)
{
if ((currentStep / (microsteps / 2)) % 2 == 0) // we're at an odd step, weird
{
if (dir == MotorDirection.Forward)
{
currentStep += microsteps / 2;
}
else
{
currentStep -= microsteps / 2;
}
}
else // go to the next even step
{
if (dir == MotorDirection.Forward)
{
currentStep += microsteps;
}
else
{
currentStep -= microsteps;
}
}
}
else if (style == StepType.Double)
{
if ((currentStep / (microsteps / 2) % 2) != 0) // we're at an even step, weird
{
if (dir == MotorDirection.Forward)
{
currentStep += microsteps / 2;
}
else
{
currentStep -= microsteps / 2;
}
}
else // go to the next odd step
{
if (dir == MotorDirection.Forward)
{
currentStep += microsteps;
}
else
{
currentStep -= microsteps;
}
}
}
else if (style == StepType.Interleave)
{
if (dir == MotorDirection.Forward)
{
currentStep += microsteps / 2;
}
else
{
currentStep -= microsteps / 2;
}
}
if (style == StepType.Microstep)
{
if (dir == MotorDirection.Forward)
{
currentStep++;
}
else
{
// BACKWARDS
currentStep--;
}
currentStep += microsteps * 4;
currentStep %= microsteps * 4;
ocra = ocrb = 0;
if ((currentStep >= 0) && (currentStep < microsteps))
{
ocra = microstepCurve[microsteps - currentStep];
ocrb = microstepCurve[currentStep];
}
else if ((currentStep >= microsteps) && (currentStep < microsteps * 2))
{
ocra = microstepCurve[currentStep - microsteps];
ocrb = microstepCurve[microsteps * 2 - currentStep];
}
else if ((currentStep >= microsteps * 2) && (currentStep < microsteps * 3))
{
ocra = microstepCurve[microsteps * 3 - currentStep];
ocrb = microstepCurve[currentStep - microsteps * 2];
}
else if ((currentStep >= microsteps * 3) && (currentStep < microsteps * 4))
{
ocra = microstepCurve[currentStep - microsteps * 3];
ocrb = microstepCurve[microsteps * 4 - currentStep];
}
}
currentStep += microsteps * 4;
currentStep %= microsteps * 4;
//coilA.SetDutyCycle(ocra);
//coilB.SetDutyCycle(ocrb);
coilA.Set(600, ocra); // 600 ppa max
coilB.Set(600, ocrb); // 600 ppa max
// frequency in hZ, dutycycle byte
// release all
MotorShield.Instance.LatchState &= (byte)(~a & ~b & ~c & ~d); // all motor pins to 0
//Serial.println(step, DEC);
if (style == StepType.Microstep)
{
if ((currentStep >= 0) && (currentStep < microsteps))
MotorShield.Instance.LatchState |= (byte)(a | b);
if ((currentStep >= microsteps) && (currentStep < microsteps * 2))
MotorShield.Instance.LatchState |= (byte)(b | c);
if ((currentStep >= microsteps * 2) && (currentStep < microsteps * 3))
MotorShield.Instance.LatchState |= (byte)(c | d);
if ((currentStep >= microsteps * 3) && (currentStep < microsteps * 4))
MotorShield.Instance.LatchState |= (byte)(d | a);
}
else
{
switch (currentStep / (microsteps / 2))
{
case 0:
MotorShield.Instance.LatchState |= (byte)(a); // energize coil 1 only
break;
case 1:
MotorShield.Instance.LatchState |= (byte)(a | b); // energize coil 1+2
break;
case 2:
MotorShield.Instance.LatchState |= (byte)(b); // energize coil 2 only
break;
case 3:
MotorShield.Instance.LatchState |= (byte)(b | c); // energize coil 2+3
break;
case 4:
MotorShield.Instance.LatchState |= (byte)(c); // energize coil 3 only
break;
case 5:
MotorShield.Instance.LatchState |= (byte)(c | d); // energize coil 3+4
break;
case 6:
MotorShield.Instance.LatchState |= (byte)(d); // energize coil 4 only
break;
case 7:
MotorShield.Instance.LatchState |= (byte)(d | a); // energize coil 1+4
break;
}
}
MotorShield.Instance.LatchTx();
return currentStep;
}
/// <summary>
/// Releases the motor, allowing it to spin freely
/// </summary>
public void Release()
{
int latchState = 0;
switch (stepperPort)
{
case StepperPorts.M1_M2:
// Turn off all motor pins
latchState &= ~(1 << (int)MotorBits.Motor1_A) &
~(1 << (int)MotorBits.Motor1_B) &
~(1 << (int)MotorBits.Motor2_A) &
~(1 << (int)MotorBits.Motor2_B);
break;
case StepperPorts.M3_M4:
// turn off all motor pins
latchState &= ~(1 << (int)MotorBits.Motor3_A) &
~(1 << (int)MotorBits.Motor3_B) &
~(1 << (int)MotorBits.Motor4_A) &
~(1 << (int)MotorBits.Motor4_B);
break;
default:
throw new InvalidOperationException("Invalid motor header specified");
}
MotorShield.Instance.LatchState = (byte)latchState;
MotorShield.Instance.LatchTx(); // disable channels
// Ste speed to 0
coilA.Set(64000, 0);
coilB.Set(64000, 0);
//coilA.SetDutyCycle(0);
//coilB.SetDutyCycle(0);
}
}
public enum MotorBits
{
Motor1_A = 0,
Motor1_B = 1,
Motor2_A = 2,
Motor2_B = 3,
Motor3_A = 4,
Motor3_B = 5,
Motor4_A = 6,
Motor4_B = 7
}
public enum MotorDirection
{
Release, Forward, Reverse
}
public enum MotorHeaders
{
M1, M2, M3, M4
}
}
I can’t figure out how to convert the following lines:
coilA.SetDutyCycle(ocra);
coilB.SetDutyCycle(ocrb);