Hey all,
I feel I am 90% there but am having an issue with the logic. Basically I would like to drive a pulley that will have a MPU6050 3 axis Gyro mounted to it to use as limit/feedback control for the stepper motor. I would like to drive the pulley roughly 180 degrees and then reverse 180 degrees and back and forth until power is cut. I realize cutting power will throw it off track but that is why I plan to use overshoot ranges and have it correct when out of both ranges.
My confusion is centered on the fact that I am telling the stepper motor to do two separate things over the same degree range. I had it “working” with an L298N driver but it was not consistent as it was current regulated and it got so hot it fried one of my L298Ns after a lot of experimentation. Here is the code for that setup.
#include<Wire.h>
#include <Stepper.h>
const int stepsPerRevolution = 200; // change this to fit the number of steps per revolution
// for your motor
// initialize the stepper library on pins 8 through 11:
Stepper myStepper(stepsPerRevolution, 8, 9, 10, 11);
const int MPU_addr = 0x68;
int16_t AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ;
int minVal = 265;
int maxVal = 402;
const float MINz1 = 85;
const float MAXz1 = 95;
const float MINz2 = 265;
const float MAXz2 = 275;
//double x;
//double y;
double z;
void setup() {
Wire.begin();
Wire.beginTransmission(MPU_addr);
Wire.write(0x6B);
Wire.write(0);
Wire.endTransmission(true);
Serial.begin(9600);
// set the speed at 60 rpm:
myStepper.setSpeed(25);
}
void loop() {
Wire.beginTransmission(MPU_addr);
Wire.write(0x3B);
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr, 14, true);
AcX = Wire.read() << 8 | Wire.read();
AcY = Wire.read() << 8 | Wire.read();
AcZ = Wire.read() << 8 | Wire.read();
int xAng = map(AcX, minVal, maxVal, -90, 90);
int yAng = map(AcY, minVal, maxVal, -90, 90);
int zAng = map(AcZ, minVal, maxVal, -90, 90);
//x= RAD_TO_DEG * (atan2(-yAng, -zAng)+PI);
//y= RAD_TO_DEG * (atan2(-xAng, -zAng)+PI);
z = RAD_TO_DEG * (atan2(-yAng, -xAng) + PI);
if (z > MINz1 and z < MAXz1) {
// step one revolution in one direction:
Serial.println("clockwise");
myStepper.step(800);
delay(500);
}
if (z > MINz2 and z < MAXz2) {
// step one revolution in the other direction:
Serial.println("counterclockwise");
myStepper.step(-800);
delay(500);
}
else {
Serial.println("Correcting");
myStepper.step(25);
delay(70);
}
//Serial.print("AngleX= ");
//Serial.println(x);
//Serial.print("AngleY= ");
//Serial.println(y);
Serial.print("AngleZ= ");
Serial.println(z);
//Serial.println("-----------------------------------------");
delay(400);
}
I commented out calculations for x and y degrees as I am only using z in this project.
This worked as the program goes “blind” under the 800 and -800 steps routines so as long as it is calibrated in number of steps/speed so that it doesn’t get past either reversal range after the commanded steps are completed it will reverse and do another 800 steps or so in the opposite direction. I used the else statement (with the serial print correcting) as if it should get out of range it will correct until it reaches the first direction range.
With the TB6600, which is amazing in comparison to the L298n, I am having a difficult time making it do the “blind” sequence of steps as before so that it will continue in the assigned direction once hitting one of the target ranges even though they are contradictory. As it is currently written, as soon as it crosses the range threshold, it reverses which makes sense as that is what I am telling it to do and it is not going “blind”.
I tried using a for statement like the one you see made commented out below (with the i++) language) in other previous iterations but it would only follow the first range and ignore anything else as having an if and a for and an else was problematic. Here is where I am at with that program.
/* Example sketch to control a stepper motor with TB6600 stepper motor driver and Arduino without a library: continuous rotation. More info: https://www.makerguides.com */
// Define stepper motor connections:
#include<Wire.h>
#define dirPin 2
#define stepPin 3
const int MPU_addr = 0x68;
int16_t AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ;
int minVal = 265;
int maxVal = 402;
const float MINz1 = 89;
const float MAXz1 = 280;
//double x;
//double y;
double z;
void setup() {
Wire.begin();
Wire.beginTransmission(MPU_addr);
Wire.write(0x6B);
Wire.write(0);
Wire.endTransmission(true);
Serial.begin(9600);
// Declare pins as output:
pinMode(stepPin, OUTPUT);
pinMode(dirPin, OUTPUT);
}
void loop() {
Wire.beginTransmission(MPU_addr);
Wire.write(0x3B);
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr, 14, true);
AcX = Wire.read() << 8 | Wire.read();
AcY = Wire.read() << 8 | Wire.read();
AcZ = Wire.read() << 8 | Wire.read();
int xAng = map(AcX, minVal, maxVal, -90, 90);
int yAng = map(AcY, minVal, maxVal, -90, 90);
int zAng = map(AcZ, minVal, maxVal, -90, 90);
//x= RAD_TO_DEG * (atan2(-yAng, -zAng)+PI);
//y= RAD_TO_DEG * (atan2(-xAng, -zAng)+PI);
z = RAD_TO_DEG * (atan2(-yAng, -xAng) + PI);
Serial.print("Z= ");
Serial.println(z);
if (z > MINz1 and z < MAXz1) {
// step one revolution in one direction:
Serial.println("CCW");
// Set the spinning direction clockwise:
digitalWrite(dirPin, LOW);
// Spin the stepper motor 1 revolution slowly:
// for (int i = 0; i < 20; i++)
// These four lines result in 1 step:
digitalWrite(stepPin, HIGH);
delayMicroseconds(500);
digitalWrite(stepPin, LOW);
delayMicroseconds(500);
}
else {
Serial.println("CW");
// Set the spinning direction clockwise:
digitalWrite(dirPin, HIGH);
// Spin the stepper motor 1 revolution slowly:
// for (int i = 0; i < 20; i++)
// These four lines result in 1 step:
digitalWrite(stepPin, HIGH);
delayMicroseconds(500);
digitalWrite(stepPin, LOW);
delayMicroseconds(500);
}
//Serial.print("AngleX= ");
//Serial.println(x);
//Serial.print("AngleY= ");
//Serial.println(y);
//Serial.println("-----------------------------------------");
}
I realize I am going to have to get a fan or heatsink or something as this thing got up to 260 degrees F before I realized I had the dip switches incorrect (doh). It runs about 150 normally.
Thank you so much in advance for your help or advice!
