Hi all,
I am running into a problem I cannot seem to fix with other forums and debugging. My partner and I are designing a point picking and plasma cutting robot for our senior project and have had a "slight" issue with our code for our linear actuator
#include <ArduinoSTL.h>
#include <AccelStepper.h>
#include <cmath>
#include <MultiStepper.h>
//outputs
// Global ints
volatile int counterbase = 0;
volatile int counterarm = 0;
bool math = true;
bool zero = false;
bool firstpoint = false;
float radians = 0.00104666; // multiplier to get steps to radians
float res = 0.1;
// Vectors for math
std::vector<float> xpoint;
std::vector<float> ypoint;
std::vector<int> angles;
std::vector<int> anglestwo;
// steppers
int baseStepPin = 22;
int baseDirPin = 23;
int armStepPin = 24;
int armDirPin = 25;
AccelStepper baseStepper(AccelStepper::DRIVER, baseStepPin, baseDirPin);
AccelStepper armStepper(AccelStepper::DRIVER, armStepPin, armDirPin);
float stepsperrev = 2000;
float motorspeed = 100;
float movespeed = 200;
void setup() {
//pins
pinMode(5, OUTPUT);
pinMode(44, OUTPUT);
pinMode(45, OUTPUT);
Serial.begin(9600); // Initialize Serial communication
digitalWrite(51, LOW);
// Sets pins as interrupt
pinMode(2, INPUT_PULLUP);
pinMode(3, INPUT_PULLUP);
pinMode(18, INPUT_PULLUP);
pinMode(19, INPUT_PULLUP);
// Sets which functions run at interrupt
attachInterrupt(0, ai0, RISING);
attachInterrupt(1, ai1, RISING);
attachInterrupt(4, ai2, RISING);
attachInterrupt(5, ai3, RISING);
baseStepper.setMaxSpeed(movespeed);
baseStepper.setAcceleration(1000);
armStepper.setMaxSpeed(movespeed);
armStepper.setAcceleration(1000);
digitalWrite(5, LOW);
while (analogRead(A10) < 1015) {// both while loops set arms to zero
baseStepper.setSpeed(-motorspeed);
baseStepper.runSpeed();
}
Serial.println("base zero'd");
while (analogRead(A12) < 1015) {
armStepper.setSpeed(motorspeed);
armStepper.runSpeed();
}
Serial.println("arm zero'd");
digitalWrite(5, HIGH);
baseStepper.setCurrentPosition(0);
armStepper.setCurrentPosition(0);
counterbase = counterarm = 0;
digitalWrite(45, HIGH);
delay(4000);
digitalWrite(45, LOW);
}
void loop() {
// Button checks at the start of the loop each time
int buttontwo = analogRead(A3);
int buttonthree = analogRead(A6);
int buttonfour = analogRead(A8);
Serial.println("looping");
if (buttontwo > 1015) { // Add the x and y positions to the vectors
xpoint.push_back(((cos(M_PI - (counterbase * radians))) * 18) +
(cos((M_PI - (counterbase * radians) - (counterarm * radians)))) * 18); // add to xvector
ypoint.push_back(sin(M_PI - (counterbase * radians)) * 18 +
sin((M_PI - (counterbase * radians) - (counterarm * radians))) * 18); // add to yvector
// Print the contents of the vectors
Serial.println("X Positions:");
for (size_t i = 0; i < xpoint.size(); ++i) {
Serial.print(xpoint[i]);
Serial.print(", ");
}
Serial.println();
Serial.println("Y Positions:");
for (size_t i = 0; i < ypoint.size(); ++i) {
Serial.print(ypoint[i]);
Serial.print(", ");
}
Serial.println();
delay(1000);
}
if (buttonthree > 1015 && math) { // interpolate the point between the x and y values based on which is farther
for (int i = 1; i < xpoint.size(); ++i) {
float x = (xpoint.at(i) - xpoint.at(i - 1));
float y = (ypoint.at(i) - ypoint.at(i - 1));
float rx = std::abs(x) / res;
float ry = std::abs(y) / res;
if (std::abs(x) >= std::abs(y)) {
for (int g = 0; g <= rx; ++g) {
float x1 = (xpoint.at(i - 1) + (res * x / std::abs(x) * g));
float y1 = (ypoint.at(i - 1) + ((y / rx) * g));
float H = sqrt(pow(x1, 2) + pow(y1, 2));
angles.push_back((180-((acos(x1 / H) + acos((pow(18, 2) + pow(H, 2) - pow(18, 2)) / (2 * 18 * H))) * (180 / M_PI)))* (stepsperrev / 360)* 3);
anglestwo.push_back((180-(acos((pow(18, 2) + pow(18, 2) - pow(H, 2)) / (2 * 18 * 18)) * (180 / M_PI))) * (stepsperrev / 360)* 3* -1);
}
} else {
for (int g = 0; g <= ry; ++g) {
float x1 = (xpoint.at(i - 1) + ((x / ry) * g));
float y1 = (ypoint.at(i - 1) + (res * y / std::abs(y) * g));
float H = sqrt(pow(x1, 2) + pow(y1, 2));
angles.push_back((180-((acos(x1 / H) + acos((pow(18, 2) + pow(H, 2) - pow(18, 2)) / (2 * 18 * H))) * (180 / M_PI)))* (stepsperrev / 360)* 3);
anglestwo.push_back((180-(acos((pow(18, 2) + pow(18, 2) - pow(H, 2)) / (2 * 18 * 18)) * (180 / M_PI))) * (stepsperrev / 360)* 3* -1);
}
}
}
digitalWrite(5, LOW);
math = false;
Serial.println("math done");
Serial.println(angles.size());
Serial.println("Encoder vector elements: "); // Print vector for testing
for (int i = 0; i < angles.size(); ++i) {
Serial.print(angles[i]);
Serial.print(", .");
Serial.print(anglestwo[i]);
Serial.println(" ");
}
while (analogRead(A10) < 1015) {// both while loops set arms to zero
baseStepper.setSpeed(-motorspeed);
baseStepper.runSpeed();
}
while (analogRead(A12) < 1015) {
armStepper.setSpeed(motorspeed);
armStepper.runSpeed();
}
baseStepper.setCurrentPosition(0);
armStepper.setCurrentPosition(0);
zero = true;
delay(1000);
Serial.println("zero'd");
}
if (buttonfour > 1015 && zero == true) {
Serial.println("motor move");
for (int i = 0; i < 1; ++i) {
// Move the motors listed position
baseStepper.moveTo(angles[i]);
armStepper.moveTo(anglestwo[i]);
Serial.println("motor moving");
// Wait until both steppers reach their target position
while (baseStepper.distanceToGo() != 0 || armStepper.distanceToGo() != 0) {
baseStepper.run();
armStepper.run();
}
}
digitalWrite(44, HIGH);
delay(1000);
digitalWrite(44, LOW);
firstpoint = true;
}
if (buttonfour > 1015 && firstpoint == true) {
for (int i = 1; i < angles.size() && i < anglestwo.size(); ++i) {
// Move the motors listed position
baseStepper.moveTo(angles[i]);
armStepper.moveTo(anglestwo[i]);
Serial.println("motor moving");
// Wait until both steppers reach their target position
while (baseStepper.distanceToGo() != 0 || armStepper.distanceToGo() != 0) {
baseStepper.run();
armStepper.run();
}
// Delay between moves
delay(10);
}
digitalWrite(45, HIGH);
}
if (buttonfour > 1015) {
Serial.println(angles.size());
Serial.println("Encoder vector elements: "); // Print vector for testing
for (int i = 0; i < angles.size(); ++i) {
Serial.print(anglestwo[i]);
Serial.print(" ");
Serial.print(angles[i]);
Serial.println(" ");
}
}
delay(200);
}
void ai0() { // Encoder one one interrupt
if (digitalRead(3) == LOW) {
counterarm++;
} else {
counterarm--;
}
}
void ai1() { // Encoder one two interrupt
if (digitalRead(2) == LOW) {
counterarm--;
} else {
counterarm++;
}
}
void ai2() { // Encoder two one interrupt
if (digitalRead(18) == LOW) {
counterbase++;
} else {
counterbase--;
}
}
void ai3() { // Encoder two two interrupt
if (digitalRead(19) == LOW) {
counterbase--;
} else {
counterbase++;
}
}
The issue arises around line 197 when the actuator on the end of the arm is commanded to lower and stop. it does, and if i have a print statement saying end moved directly after setting pin 44 to low(controlling a relay) but then it resets the code back to the set up, I know this because it redoes the print statements "base zeroed" and "Arm zeroed" but then it rockets back to the second point in the list. The program runs fine with everything wired and without that little section of code (lines 197 to 200). max draw on the 5 volt pin at the time of reset is only 5mA plus whatever the board is using.
Any help is greatly appreciated.