/* MonsterMoto Shield Example Sketch
To work with 6 wire stepper motor
by John Stapko 10/29/14
*/
#define BRAKEVCC 0
#define CW 1
#define CCW 2
#define BRAKEGND 3
#define CS_THRESHOLD 100
/* VNH2SP30 pin definitions
xxx[0] controls '1' outputs
xxx[1] controls '2' outputs */
int inApin[2] = {7, 4}; // INA: Clockwise input
int inBpin[2] = {8, 9}; // INB: Counter-clockwise input
int pwmpin[2] = {5, 6}; // PWM input
int cspin[2] = {2, 3}; // CS: Current sense ANALOG input
int enpin[2] = {0, 1}; // EN: Status of switches output (Analog pin)
int statpin = 13;
void setup()
{
Serial.begin(9600);
pinMode(statpin, OUTPUT);
// Initialize digital pins as outputs
for (int i=0; i<2; i++)
{
pinMode(inApin[i], OUTPUT);
pinMode(inBpin[i], OUTPUT);
pinMode(pwmpin[i], OUTPUT);
}
// Initialize braked
for (int i=0; i<2; i++)
{
digitalWrite(inApin[i], LOW);
digitalWrite(inBpin[i], LOW);
}
// motorGo(0, CW, 1023);
// motorGo(1, CCW, 1023);
}
void loop()
{
motorGo(0, CW, 1023);
delay(10);
motorGo(0, CW, 0);
delay(100);
motorGo(1, CCW, 1023);
delay(10);
motorGo(1, CCW, 0);
delay(10);
motorGo(0, CCW, 1023);
delay(10);
motorGo(0, CCW, 0);
delay(10);
motorGo(1, CW, 1023);
delay(10);
motorGo(1, CW, 0);
delay(10);
motorGo(0, CW, 1023);
delay(10);
motorGo(0, CW, 0);
delay(10);
if ((analogRead(cspin[0]) < CS_THRESHOLD) && (analogRead(cspin[1]) < CS_THRESHOLD))
digitalWrite(statpin, HIGH);
}
void motorOff(int motor)
{
// Initialize braked
for (int i=0; i<2; i++)
{
digitalWrite(inApin[i], LOW);
digitalWrite(inBpin[i], LOW);
}
analogWrite(pwmpin[motor], 0);
}
/* motorGo() will set a motor going in a specific direction
the motor will continue going in that direction, at that speed
until told to do otherwise.
motor: this should be either 0 or 1, will selet which of the two
motors to be controlled
direct: Should be between 0 and 3, with the following result
0: Brake to VCC
1: Clockwise
2: CounterClockwise
3: Brake to GND
pwm: should be a value between ? and 1023, higher the number, the faster
it'll go
*/
void motorGo(uint8_t motor, uint8_t direct, uint8_t pwm)
{
if (motor <= 1)
{
if (direct <=4)
{
// Set inA[motor]
if (direct <=1)
digitalWrite(inApin[motor], HIGH);
else
digitalWrite(inApin[motor], LOW);
// Set inB[motor]
if ((direct==0)||(direct==2))
digitalWrite(inBpin[motor], HIGH);
else
digitalWrite(inBpin[motor], LOW);
analogWrite(pwmpin[motor], pwm);
}
}
}