Hi leute,
ich Arbeite seit ein paar Tagen einem Roboter Arm. Teile sind schon alle FERTIG.
Bei der Programmierung für die Auto Home Funktion bin ich auf mehrere Probleme gestoßen.
1 Problem:
Erst fährt er beim Start des Programmes in die Home position bis der Endstop betätigt wurde. Dann macht er eine Paar Steps vor und geht in den Loop();. Dort Frag ich ab ob der Schalter HomeBT gedrückt wird(nur vorübergehend als State). Dann geht er in die Go Home position… Bis der Endstop wieder betätigt ist.
Aber wie komm ich jetzt aus dieser While schleife raus?
Code:
#include <Arduino.h>
#include <AccelStepper.h>
//Position
#define poti_01 A5:
#define poti_02 A6;
#define poti_03 A7;
const int SMOOTH_FACTOR = 4;
int analog1_in, analog2_in, analog3_in;
//Endschalter
#define endStop_01 9
#define homeButton 2
int stateHomeBT = 0;
int state_endstop_01 = 0; //1 an endStop_01
//Stepper PINS
const int pin_Stepper1[3] = {53, 51, 45};
const int pin_Stepper2[3] = {49, 47, 43};
const int pin_Stepper3[3] = {41, 39, 37};
//Create Steppers
AccelStepper stepper1(AccelStepper::DRIVER, pin_Stepper1[0], pin_Stepper1[1]);
AccelStepper stepper2(AccelStepper::DRIVER, pin_Stepper2[0], pin_Stepper2[1]);
AccelStepper stepper3(AccelStepper::DRIVER, pin_Stepper3[0], pin_Stepper3[1]);
//Auto Home
long init_Homing = -1; // Home Position
int isHoming = 0;
//millis
unsigned long prev_Millis_Read = 0; //Serial Monitor
void setStepperOptions(){
//Stepper 1:
stepper1.setMaxSpeed(1000);
stepper1.setAcceleration(500);
stepper1.setEnablePin(pin_Stepper1[2]);
//Stepper 2:
stepper1.setMaxSpeed(1000);
stepper1.setAcceleration(500);
stepper1.setEnablePin(pin_Stepper1[2]);
//Stepper 3:
stepper1.setMaxSpeed(1000);
stepper1.setAcceleration(500);
stepper1.setEnablePin(pin_Stepper1[2]);
}
void readPotValue(){
analog1_in = (analog1_in - (analog1_in >> SMOOTH_FACTOR)) + (analogRead(5) >> SMOOTH_FACTOR);
analog2_in = (analog2_in - (analog2_in >> SMOOTH_FACTOR)) + (analogRead(6) >> SMOOTH_FACTOR);
analog3_in = (analog3_in - (analog3_in >> SMOOTH_FACTOR)) + (analogRead(7) >> SMOOTH_FACTOR);
//Stepper1;
if(analog1_in >= 0){
if(analog1_in != analogRead(5)){
if(state_endstop_01 == 0){
setStepperOptions();
stepper1.moveTo(analog1_in * 2);
stepper1.run();
}else{
stepper1.stop();
}
}else{
digitalWrite(pin_Stepper1[2], HIGH);
}
}
//Stepper2;
if(analog2_in >= 0){
if(analog2_in != analogRead(6)){
if(state_endstop_01 == 0){
setStepperOptions();
stepper2.moveTo(analog2_in * 2);
stepper2.run();
}else{
stepper2.stop();
}
}else{
digitalWrite(pin_Stepper2[2], HIGH);
}
}
//Stepper 3;
if(analog3_in >= 0){
if(analog3_in != analogRead(7)){
if(state_endstop_01 == 0){
setStepperOptions();
stepper3.moveTo(analog3_in * 2);
stepper3.run();
}else{
stepper3.stop();
}
}else{
digitalWrite(pin_Stepper3[2], HIGH);
}
}
}
void goHome(){
isHoming = 1;
while (digitalRead(endStop_01)) { // Solange fahren bis Endstop aktiviert
init_Homing--;
//Schulter
stepper1.moveTo(-3000);
stepper1.run();
//Arm
stepper1.moveTo(-3000);
stepper1.run();
delay(5);
Serial.println("1");
}
stepper1.setCurrentPosition(0);
stepper2.setCurrentPosition(0);
stepper1.setMaxSpeed(100.0);
stepper1.setAcceleration(100.0);
stepper2.setMaxSpeed(100.0);
stepper2.setAcceleration(100.0);
Serial.println("2");
if(isHoming == 1){
while (!digitalRead(endStop_01)) { //Er kommt nicht aus der Schleife
//Schulter
stepper1.moveTo(50);
stepper1.run();
//Arm
stepper2.moveTo(50);
stepper2.run();
delay(5);
Serial.println("3");
}
}else{
readPotValue();
}
Serial.println("Homing Complete.............................");
stepper1.setCurrentPosition(0);
stepper2.setCurrentPosition(0);
setStepperOptions();
Serial.println("4");
}
void readEndstop(){
}
void setup() {
Serial.begin(9600);
pinMode(endStop_01, INPUT_PULLUP);
pinMode(homeButton, INPUT_PULLUP);
setStepperOptions();
goHome();
}
void loop() {
if(millis() - prev_Millis_Read >= 10000){
Serial.println(stateHomeBT);
goHome();
}
if(stateHomeBT == 1){
goHome();
}else{
readPotValue();
Serial.println("11");
}
}