I would like to control a tray to move back and forth automatically between several positions automatically.
Hardware: arduino nano / mega, TMC2209 and TB6600 stepper motor drivers, stepper motor 23HS84830.
Power Supply: 12V/5A
Problem: I wrote the code and noticed that although the interval between positions was fixed, the tray would move a little bit more each time therefore missing the position. At the last position it would be really off position.
Attempted Solutions: To solve this I decided to write a serial command sketch that would allow me to simulate this behaviour, define intervals, number of positions, cycles, laps, microstepping, see code below.
I had just run a sequence and made it back to the main menu. these are all the options that can be configured.
Made some trials and noticed that my sketch was outputing exactly what I configured in the right way (see picture of logic analyzer below).
In this picture the what happens is:
- stepper motor rotates 5 laps (8000 steps) and waits 0,5 seconds before rotating again. repeats 5 times.
- changes direction
- does the same as step 1
What I did to try and solve the problem so far:
- changed between TMC2209 and TB6600 to see if the problem would be the driver: it wasn't happens with both drivers
- checked with the logic analyzer if the problem was the code: it wasn't, output of code is consistent and easily measurable
- reviewed the connections of the stepper motor coils. not a problem and according to the motor datasheet/stepper motor driver datasheet.
At this moment, I cannot understand why the motor is not moving correctly and I would appreciate experienced support to solve this since it seems quite trivial but I cannot find the bug.
Thanks!
int PUL=4; //define Pulse pin
int DIR=3; //define Direction pin
int ENA=2; //define Enable Pin
#define left 1
#define right 0
#define LEFT 1
#define RIGHT 0
int steps_per_revolution = 200;
int minutes = 60;
long int input_value = 0;
long int _speed = 0;
long int temp_speed_rpm = 0;
long int speed_rpm = 0;
long int steps = 0;
int cycles = 0;
int positions = 0;
long int laps = 0;
long int rpm = 0;
int microstepping = 0;
long int total_laps = 0;
// Serial Commands
String command;
String inString = "";
// control flag to show the menu
boolean refresh_commands = false;
// DIRECTION LOW - MOVES RIGHT
// DIRECTION HIGH - MOVES LEFT
void setup() {
pinMode (PUL, OUTPUT);
pinMode (DIR, OUTPUT);
pinMode (ENA, OUTPUT);
Serial.begin(115200);
menu_print();
}
void loop()
{
if(Serial.available()){
command = Serial.readStringUntil('\n');
if(command.equals("p")){
variable_print();
refresh_commands = true;
}
if(command.equals("ss")){
Serial.println("0");
_speed = input_data();
Serial.print(_speed);
Serial.println(" uS");
refresh_commands = true;
}
if(command.equals("ssr")){
Serial.println("ssr");
temp_speed_rpm = input_data();
_speed = calculate_speed(temp_speed_rpm);
refresh_commands = true;
}
if(command.equals("sd")){ // checks if one direction is set, changes and then changes back again
if(digitalRead(DIR) == HIGH)
{
change_direction(right);
Serial.println("Direction changed to: RIGHT");
}
else
{
if(digitalRead(DIR) == LOW)
{
change_direction(left);
Serial.println("Direction changed to: LEFT");
}
}
refresh_commands = true;
}
if(command.equals("sst")){
Serial.println("sst");
steps = input_data();
Serial.println(" ");
Serial.print(steps);
Serial.println(" - steps configured");
refresh_commands = true;
}
if(command.equals("sm")){
Serial.println("sm");
Serial.println("sm --> how many microsteps?");
microstepping = input_data();
Serial.println(microstepping);
refresh_commands = true;
}
if(command.equals("sp")){
Serial.println("sp");
positions = input_data();
Serial.println("sp --> Configure Positions");
Serial.println(positions);
refresh_commands = true;
}
if(command.equals("sc")){
Serial.println("sc");
cycles = input_data();
Serial.println("sc --> Configure Cycles");
Serial.println(cycles);
refresh_commands = true;
}
if(command.equals("sl")){
Serial.println("sl");
Serial.println("sl --> how many laps?");
laps = input_data();
Serial.print(laps);
refresh_commands = true;
}
if(command.equals("1")){
Serial.println("1");
change_direction(left);
steps = input_data(); // Asks before for how many steps to rotate and changes the value.
rotate_motor(steps);
refresh_commands = true;
}
if(command.equals("2")){
Serial.println("2");
change_direction(right);
steps = input_data(); // Asks before for how many steps to rotate and changes the value.
rotate_motor(steps);
refresh_commands = true;
}
if(command.equals("3")){
Serial.println("3");
change_direction(left);
rotate_motor(steps);
refresh_commands = true;
}
if(command.equals("4")){
Serial.println("4");
change_direction(right);
rotate_motor(steps);
refresh_commands = true;
}
if(command.equals("rs")){
Serial.println("rs --> Run Sequence");
variable_print();
delay(3000);
total_laps = laps * steps_per_revolution * microstepping;
// run the planned sequence
run_sequence(cycles, positions);
refresh_commands = true;
}
if(command.equals("rl")){
Serial.println("rl");
Serial.println("rl --> how any laps? ");
variable_print();
delay(3000);
total_laps = laps * steps_per_revolution * microstepping;
// calculate laps and activate motor
run_laps();
refresh_commands = true;
}
else{
Serial.println("Invalid command");
refresh_commands = true;
}
refresh_commands = true; // added here to remove from all other commands
}
if (refresh_commands == true){
menu_print();
refresh_commands = false;
}
}
void run_sequence(int cycles, int positions){
int j = 0;
for(j=0; j<cycles; j++)
{
Serial.print("Cycle: ");
Serial.println(j);
// Start always moving to LEFT
change_direction(left);
Serial.println("Direction changed to: LEFT");
for(int i=0; i<positions; i++)
{
Serial.print("FWD Pos.: ");
Serial.println(i);
rotate_motor(total_laps);
delay(500);
}
delay(1000);
//change_direction(right);
change_direction(right);
Serial.println("Direction changed to: RIGHT");
delay(100);
for(int i=0; i<positions; i++)
{
Serial.print("BWD Pos.: ");
Serial.println(i);
rotate_motor(total_laps);
delay(500);
}
}
}
void run_laps(){
// long int total_laps = 0;
// total_laps = laps * steps_per_revolution * microstepping;
// moved to the main cycle and changed run_sequence to include the laps
Serial.print("Total Laps Steps: "); // to be tested.
Serial.println(total_laps);
Serial.print("Laps: "); // to be tested.
Serial.println(laps);
Serial.print("Steps/rev: "); // to be tested.
Serial.println(steps_per_revolution);
Serial.print("MicroStepping: "); // to be tested.
Serial.println(microstepping);
// change direction
change_direction(left);
// rotate motor
rotate_motor(total_laps); // variables must be long otherwise we cannot do the same number of steps as others
delay(1000);
// change direction
change_direction(right);
// rotate motor
rotate_motor(total_laps);
Serial.print(laps);
Serial.println(" - Laps completed.");
}
int input_data(){
int inChar = 0;
boolean flag = false;
Serial.println("How many?");
Serial.println(" ");
do{
while (Serial.available() > 0) {
inChar = Serial.read();
if (isDigit(inChar)) {
// convert the incoming byte to a char and add it to the string:
inString += (char)inChar;
}
// if you get a newline, print the string, then the string's value:
if (inChar == '\n') {
input_value = inString.toInt();
// clear the string for new input:
inString = "";
flag = true;
}
// // if the received char is an 'Z', then it triggers a flag to leave the menu
// if(inChar == 'Z')
// {
// flag = true;
// }
}
}while(!flag);
return input_value;
}
void rotate_motor(long int motor_steps){
Serial.print("Starting Rotation --> ");
Serial.println(motor_steps);
long int i=0;
digitalWrite(ENA, LOW);
for (i=0; i<motor_steps; i++)
{
digitalWrite(PUL,HIGH);
delayMicroseconds(_speed);
digitalWrite(PUL,LOW);
delayMicroseconds(_speed);
}
Serial.print("i --> ");
Serial.print(i);
digitalWrite(ENA, HIGH);
Serial.println("Finished rotation!");
}
void change_direction(bool direction){
delayMicroseconds(500);
digitalWrite(ENA,HIGH);
delayMicroseconds(100);
digitalWrite(DIR,direction);
delayMicroseconds(500);
digitalWrite(ENA,LOW);
delayMicroseconds(100);
Serial.print("Read Dir: ");
Serial.println(digitalRead(DIR));
}
void menu_print(){
Serial.println(" ");
Serial.println("p --> Display Parameters");
Serial.println("ss --> Set Speed (tON/tOFF)");
Serial.println("ssr --> Set Speed (RPM)");
Serial.println("sd --> Configure direction");
Serial.println("sst --> Set Steps");
Serial.println("sm --> Set MicroStepping (default = 1)");
Serial.println("sp --> Set Positions");
Serial.println("sc --> Set Cycles");
Serial.println("sl --> Set Laps");
Serial.println("1 --> Move LEFT x steps");
Serial.println("2 --> Move RIGHT x steps");
Serial.println("3 --> Move LEFT 1 position");
Serial.println("4 --> Move RIGHT 1 position");
Serial.println("rs --> Run Sequence");
Serial.println("rl --> Run Laps");
Serial.println(" ");
}
void variable_print(){
Serial.print("Speed: ");
Serial.print(_speed);
Serial.println(" us");
Serial.print("Speed: ");
Serial.print(temp_speed_rpm);
Serial.println(" RPM");
Serial.print("Steps: ");
Serial.println(steps);
Serial.print("Microstepping: ");
Serial.println(microstepping);
Serial.print("Positions: ");
Serial.println(positions);
Serial.print("Cycles: ");
Serial.println(cycles);
Serial.print("Laps: ");
Serial.println(laps);
Serial.print("Direction: ");
read_direction();
}
bool read_direction(){
//bool dir_state = 0;
if(digitalRead(DIR) == HIGH)
{
Serial.println("LEFT");
}
else
{
if(digitalRead(DIR) == LOW)
{
Serial.println("RIGHT");
}
}
return digitalRead(DIR);
}
long int calculate_speed(long int _speed){
float steps_per_second = 0; // truncating a float to int -> error chance here
float temp_speed = 0;
Serial.print("FUNCTION: Calculate_speed: ");
Serial.print(_speed);
Serial.println(" RPM");
steps_per_second = (_speed * steps_per_revolution) / minutes;
Serial.print("FUNCTION: steps_per_second: ");
Serial.println(steps_per_second);
temp_speed = (1 / steps_per_second);
temp_speed = temp_speed / 2; // to find Ton and Toff
temp_speed = temp_speed / 0.000001; // to convert to microseconds (input to delayMicroseconds() function)
temp_speed = (int) temp_speed;
// Serial.print("FUNCTION: _speed in microseconds: ");
// Serial.print(temp_speed,5);
// Serial.println(" uS");
Serial.print("FUNCTION: _speed in microseconds: ");
Serial.print(temp_speed);
Serial.println(" uS");
return temp_speed;
}