ソースを参照

initialize head position

master
Thorsten Riess 2年前
コミット
bafd9a8c0b
1個のファイルの変更67行の追加22行の削除
  1. +67
    -22
      steuerung/steuerung.ino

+ 67
- 22
steuerung/steuerung.ino ファイルの表示

@@ -31,31 +31,36 @@ Initialisierung zur Positionsbestimmung?

#define SERVO_TOP_PIN 5
#define SERVO_BOTTOM_PIN 6
#define SIGN_PIN 3
#define INTERRUPT_PIN 2 // or 0?
#define SIGN_PIN 4
#define INTERRUPT_PIN 2
#define HEAD_MOTOR_PIN1 9
#define HEAD_MOTOR_PIN2 10
#define MAX_SWITCH 3 // only allow 2 bits for now
#define BUMPER_PIN 3

Servo servoTop, servoBottom;

const int posDown=30, posUp=160, posMid=90;
const int servoDelay=1000;
const int boundLeft=0;
const int initBoundRight=10;
const int velo=33;
int boundRight=200;

volatile bool interruptPinChanged=false;
volatile int headPos=0;
volatile bool bump = false;

bool switches[12]={false,false,false,false,false,false,false,false,false,false,false,false};
bool switches_old[12]={false,false,false,false,false,false,false,false,false,false,false,false};
int switchPositions[6]={0,10,20,30,40,50};
int switchPositions[6]={10,20,30,40,50,60};

int currentSwitch=-1;
bool waitForInput=true;
bool waitForInput=false;

byte readarr[11];
int spos = 0;
byte initialize=0;

void setup()
{
@@ -65,28 +70,61 @@ void setup()
// head motor pins to output
pinMode(HEAD_MOTOR_PIN1, OUTPUT);
pinMode(HEAD_MOTOR_PIN2, OUTPUT);
attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), interruptPositionUpdate, RISING);
attachInterrupt(digitalPinToInterrupt(BUMPER_PIN), interruptBumper, RISING);
Serial.begin(19200);
}

void loop()
{
if((!waitForInput) && interruptPinChanged) {
interruptPinChanged = false;
Serial.println(headPos);

if(headPos <= boundLeft) {
Serial.println("Start Position reached");// und warten auf neue Eingabe durch Nutzer
digitalWrite(HEAD_MOTOR_PIN1, LOW);
digitalWrite(HEAD_MOTOR_PIN2, LOW);
waitForInput = true; //
if(initialize==0) {
analogWrite(HEAD_MOTOR_PIN1, velo); //Kopf geht nach rechts
digitalWrite(HEAD_MOTOR_PIN2, LOW);
initialize=1;
} else {
if(initialize==1) {
if(interruptPinChanged) {
interruptPinChanged = false;
if (headPos >= initBoundRight) {
digitalWrite(HEAD_MOTOR_PIN1, LOW);
digitalWrite(HEAD_MOTOR_PIN2, LOW);
delay(100);
digitalWrite(HEAD_MOTOR_PIN1, LOW); //Kopf geht nach links
analogWrite(HEAD_MOTOR_PIN2, velo);
initialize = 2;
}
}
} else {
if (headPos >= boundRight) {
Serial.println("Position reached");//Motor anhalten und Schalterprogramm ausführen
digitalWrite(HEAD_MOTOR_PIN1, LOW);
digitalWrite(HEAD_MOTOR_PIN2, LOW);
flipswitch();
updateBoundRight();
if(initialize==2) {
if(bump) {
digitalWrite(HEAD_MOTOR_PIN1, LOW);
digitalWrite(HEAD_MOTOR_PIN2, LOW);
detachInterrupt(digitalPinToInterrupt(BUMPER_PIN));
headPos = 0;
initialize=3;
waitForInput=true;
}
} else {
if((!waitForInput) && interruptPinChanged) {
interruptPinChanged = false;
//Serial.println(headPos);
if(headPos <= boundLeft) {
Serial.println("Start Position reached");// und warten auf neue Eingabe durch Nutzer
digitalWrite(HEAD_MOTOR_PIN1, LOW);
digitalWrite(HEAD_MOTOR_PIN2, LOW);
waitForInput = true; //
} else {
if (headPos >= boundRight) {
Serial.println("Position reached");//Motor anhalten und Schalterprogramm ausführen
digitalWrite(HEAD_MOTOR_PIN1, LOW);
digitalWrite(HEAD_MOTOR_PIN2, LOW);
flipswitch();
updateBoundRight();
}
}
}
}
}
}
@@ -103,6 +141,10 @@ void interruptPositionUpdate() {
}
}

void interruptBumper() {
bump = true;
}

void updateBoundRight() {
/*
Es wird bestimmt welche Position als nächstes angefahren wird.
@@ -113,13 +155,12 @@ void updateBoundRight() {
while((currentSwitch<6) && (switches[currentSwitch]==switches_old[currentSwitch]) && (switches[currentSwitch+6]==switches_old[currentSwitch+6])) {
currentSwitch++;
}
if(currentSwitch == 6) {
if(currentSwitch >= MAX_SWITCH) {
currentSwitch=-1;
if(oldCurrentSwitch>=0) {
digitalWrite(HEAD_MOTOR_PIN1, LOW); //Kopf geht nach links
analogWrite(HEAD_MOTOR_PIN2, velo);
}
waitForInput=true;
} else {
boundRight=switchPositions[currentSwitch];
analogWrite(HEAD_MOTOR_PIN1, velo); //Kopf geht nach rechts
@@ -141,21 +182,25 @@ void flipswitch(){
servoTop.write(posDown);
delay(servoDelay);
servoTop.write(posMid);
delay(servoDelay);
}
if(!switches[currentSwitch] && (switches_old[currentSwitch])) {
servoTop.write(posUp);
delay(servoDelay);
servoTop.write(posMid);
delay(servoDelay);
}
if(switches[currentSwitch+6] && (!switches_old[currentSwitch+6])) {
servoBottom.write(posDown);
delay(servoDelay);
servoBottom.write(posMid);
delay(servoDelay);
}
if(!switches[currentSwitch+6] && (switches_old[currentSwitch+6])) {
servoBottom.write(posUp);
delay(servoDelay);
servoBottom.write(posMid);
delay(servoDelay);
}
// optional: have both switchs switch at the same time
/*
@@ -174,8 +219,8 @@ void flipswitch(){
delay(servoDelay);
servoTop.write(posMid);
servoBottom.write(posMid);
*/
delay(servoDelay);
*/
switches_old[currentSwitch] = switches[currentSwitch];
switches_old[currentSwitch+6] = switches[currentSwitch+6];
}


読み込み中…
キャンセル
保存