// test a stepper motor with a Pololu A4988 driver board, onboard led will flash at each step
// this version uses delay() to manage timing
#include <pt.h> // include protothread library
#include <AccelStepper.h>
#define EN 8
byte directionPin = 8;
byte stepPin = 9;
byte directionPin2 = 6;
byte stepPin2 = 7;
int numberOfSteps = 200;
byte ledPin = 13;
static struct pt pt1, pt2, pt3, pt4, pt5; // each protothread needs one of these
AccelStepper stepperX(1, stepPin, directionPin);// direction Digital 9 (CCW), pulses Digital 8 (CLK)
AccelStepper stepperY(1, stepPin2, directionPin2);// direction Digital 9 (CCW), pulses Digital 8 (CLK)
//User-defined values
long receivedSteps = 0; //Number of steps
long receivedSpeed = 0; //Steps / second
long receivedAcceleration = 0; //Steps / second^2
long receivedPositionX = 0; //Steps / second^2
long receivedPositionY = 0; //Steps / second^2
long newPositionX = 0; //Steps / second^2
long newPositionY = 0; //Steps / second^2
char receivedCommand;
float pos1, pos2;
//-------------------------------------------------------------------------------
int directionMultiplier = 1; // = 1: positive direction, = -1: negative direction
bool newData, runallowed = false; // booleans for new data from serial, and runallowed flag
void setup() {
Serial.begin(115200);
Serial.println("Starting StepperTest");
PT_INIT(&pt1); // initialise the two
PT_INIT(&pt2); // protothread variables
PT_INIT(&pt3); // protothread variables
PT_INIT(&pt4); // protothread variables
PT_INIT(&pt5); // protothread variables
digitalWrite(ledPin, LOW);
pinMode(EN, OUTPUT);
pinMode(directionPin, OUTPUT);
pinMode(stepPin, OUTPUT);
pinMode(directionPin2, OUTPUT);
pinMode(stepPin2, OUTPUT);
pinMode(ledPin, OUTPUT);
pinMode(3, INPUT);
pinMode(4, INPUT);
pinMode(2, INPUT_PULLUP);
stepperX.setMaxSpeed(400); //SPEED = Steps / second
stepperX.setAcceleration(800); //ACCELERATION = Steps /(second)^2
stepperY.setMaxSpeed(400); //SPEED = Steps / second
stepperY.setAcceleration(800); //ACCELERATION = Steps /(second)^2
stepperX.disableOutputs(); //disable outputs
stepperY.disableOutputs(); //disable outputs
stepperX.setCurrentPosition(0);
stepperY.setCurrentPosition(0);
}
static int resetX(struct pt *pt, int interval){
static unsigned long timestamp = 0;
PT_BEGIN(pt);
pos1 = stepperX.currentPosition();
if(receivedCommand == 'K'){
while(pos1 != 0){
PT_WAIT_UNTIL(pt, millis() - timestamp > interval );
timestamp = millis(); // take a new timestamp
if(pos1 > 0){
digitalWrite(directionPin, LOW);
digitalWrite(stepPin, HIGH);
delayMicroseconds(50);
digitalWrite(stepPin, LOW);
delayMicroseconds(50);
pos1 -= 1;
Serial.println(pos1);
}else if(pos1 < 0){
digitalWrite(directionPin, HIGH);
digitalWrite(stepPin, HIGH);
delayMicroseconds(50);
digitalWrite(stepPin, LOW);
delayMicroseconds(50);
pos1 += 1;
Serial.println(pos1);
}
if(pos1 == 0){
stepperX.disableOutputs();
break;
}
}
if(pos1 == 0){
Serial.println("sudah nol 1");
stepperX.disableOutputs();
}
}
PT_END(pt);
}
static int resetY(struct pt *pt, int interval){
static unsigned long timestamp = 0;
PT_BEGIN(pt);
pos2 = stepperY.currentPosition();
if(receivedCommand == 'K'){
while(pos2 != 0){
PT_WAIT_UNTIL(pt, millis() - timestamp > interval );
timestamp = millis(); // take a new timestamp
if(pos2 > 0){
digitalWrite(directionPin2, LOW);
digitalWrite(stepPin2, HIGH);
delayMicroseconds(50);
digitalWrite(stepPin2, LOW);
delayMicroseconds(50);
pos2 -= 1;
Serial.println(pos2);
}else if(pos2 < 0){
digitalWrite(directionPin2, HIGH);
digitalWrite(stepPin2, HIGH);
delayMicroseconds(50);
digitalWrite(stepPin2, LOW);
delayMicroseconds(50);
pos2 += 1;
Serial.println(pos2);
}
if(pos2 == 0){
stepperY.disableOutputs();
break;
}
}
if(pos2 == 0){
Serial.println("sudah nol 2");
stepperY.disableOutputs();
}
}
PT_END(pt);
}
void RotateRelativeX()
{
runallowed == true;
stepperX.enableOutputs(); //enable pins
stepperX.setMaxSpeed(receivedSpeed); //set speed
stepperX.move(directionMultiplier * receivedSteps); //set relative distance and direction
stepperX.runToPosition(); //step the motor (this will step the motor by 1 step at each loop)
}
void RotateRelativeY()
{
runallowed == true;
stepperY.enableOutputs(); //enable pins
stepperY.setMaxSpeed(receivedSpeed); //set speed
stepperY.move(directionMultiplier * receivedSteps); //set relative distance and direction
stepperY.runToPosition(); //step the motor (this will step the motor by 1 step at each loop)
}
static int motorX(struct pt *pt, int interval){
static unsigned long timestamp = 0;
PT_BEGIN(pt);
if(digitalRead(2) == LOW){
Serial.println("test 1");
while(digitalRead(3) == HIGH){
PT_WAIT_UNTIL(pt, millis() - timestamp > interval );
timestamp = millis(); // take a new timestamp
digitalWrite(directionPin, LOW);
digitalWrite(stepPin, HIGH);
delayMicroseconds(50);
digitalWrite(stepPin, LOW);
delayMicroseconds(50);
if(digitalRead(3) == LOW){
break;
}
}
}
PT_END(pt);
}
static int motorY(struct pt *pt, int interval){
static unsigned long timestamp = 0;
PT_BEGIN(pt);
if(digitalRead(2) == LOW){
while(digitalRead(4) == HIGH){
PT_WAIT_UNTIL(pt, millis() - timestamp > interval );
timestamp = millis(); // take a new timestamp
digitalWrite(directionPin2, HIGH);
digitalWrite(stepPin2, HIGH);
delayMicroseconds(50);
digitalWrite(stepPin2, LOW);
delayMicroseconds(50);
if(digitalRead(4) == LOW){
break;
}
}
}
PT_END(pt);
}
static int checkSerial(struct pt *pt, int interval)
{
static unsigned long timestamp = 0;
PT_BEGIN(pt);
if (Serial.available() > 0) //if something comes from the computer
{
receivedCommand = Serial.read(); // pass the value to the receivedCommad variable
newData = true; //indicate that there is a new data by setting this bool to true
if (newData == true) //we only enter this long switch-case statement if there is a new command from the computer
{
PT_WAIT_UNTIL(pt, millis() - timestamp > interval );
timestamp = millis(); // take a new timestamp
switch (receivedCommand) //we check what is the command
{
case 'N': //N uses the move() function of the AccelStepper library, which means that it moves relatively to the current position.
runallowed == true;
receivedSteps = Serial.parseFloat(); //value for the steps
receivedSpeed = Serial.parseFloat(); //value for the speed
directionMultiplier = -1; //We define the direction
Serial.println("Negative direction."); //print action
RotateRelativeX(); //Run the function
break;
case 'P': //P uses the move() function of the AccelStepper library, which means that it moves relatively to the current position.
runallowed == true;
receivedSteps = Serial.parseFloat(); //value for the steps
receivedSpeed = Serial.parseFloat(); //value for the speed
directionMultiplier = 1; //We define the direction
Serial.println("Positive direction."); //print the action
RotateRelativeX(); //Run the function
break;
case 'B': //N uses the move() function of the AccelStepper library, which means that it moves relatively to the current position.
receivedSteps = Serial.parseFloat(); //value for the steps
receivedSpeed = Serial.parseFloat(); //value for the speed
directionMultiplier = 1; //We define the direction
Serial.println("Positive direction."); //print action
RotateRelativeY(); //Run the function
break;
case 'D': //P uses the move() function of the AccelStepper library, which means that it moves relatively to the current position.
receivedSteps = Serial.parseFloat(); //value for the steps
receivedSpeed = Serial.parseFloat(); //value for the speed
directionMultiplier = -1; //We define the direction
Serial.println("Negative direction."); //print the action
RotateRelativeY(); //Run the function
break;
case 'U':
runallowed = false; //we still keep running disabled
stepperX.disableOutputs(); //disable power
stepperY.disableOutputs(); //disable power
receivedPositionX = Serial.parseFloat();
receivedPositionY = Serial.parseFloat();
newPositionX = receivedPositionX*9000;
newPositionY = receivedPositionY*9000;
stepperX.setCurrentPosition(newPositionX); //Reset current position. "new home"
stepperY.setCurrentPosition(newPositionY); //Reset current position. "new home"
Serial.print("The current position is updated to: "); //Print message
Serial.println(stepperX.currentPosition()); //Check position after reset.
Serial.println(stepperY.currentPosition()); //Check position after reset.
break;
case 'L': //L: Location
runallowed = false; //we still keep running disabled
stepperX.disableOutputs(); //disable power
stepperY.disableOutputs(); //disable power
Serial.print("Current location of the motor: ");//Print the message
Serial.println("x: " + String(stepperX.currentPosition()) + ", y: " + String(stepperY.currentPosition())); //Printing the current position in steps.
break;
default:
break;
}
}
newData = false;
}
PT_END(pt);
}
void loop() {
resetX(&pt1, 1);
resetY(&pt2, 1);
checkSerial(&pt3, 1);
motorX(&pt4, 1);
motorY(&pt5, 1);
}