Browse Source

complete feature set, yet untested

master
Thorsten Riess 2 years ago
parent
commit
df86da7b60
1 changed files with 132 additions and 109 deletions
  1. +132
    -109
      steuerung/steuerung.ino

+ 132
- 109
steuerung/steuerung.ino View File

@@ -27,100 +27,105 @@ deren Wert nicht 0 ist.
Initialisierung zur Positionsbestimmung?
*/

int in1 = 9; //Motortreiber Pins
int in2 = 10;

const int servo1 = 4; // Steuerpins Servos
const int servo2 = 5;
int servoStart = 127; //Ruheposition Servos
int down = 255;
int up = 0;
int servoT = 300; //Schaltzeit für Servos

int velo = 33; //Kopfgeschwindigkeit
volatile int pos = 0; //Zähler Positionssensor Kopf
int NrSchalter = 0; // Spalte Schalter
byte ArraySchalter[12];
byte ArraySchalteralt[12];
int ArrayPosition[6];
const int boundLeft = 0;
int boundRight = 200;

const byte sigPin = 3; // Pins Zähler Positionssensor Kopf
const byte interruptPin = 2;
volatile byte interruptPinChanged = 0;
int spos = 0;
byte readarr[11];
volatile bool inputflag = true; //Freigabe für neue Eingabe
#include <Servo.h>

#define SERVO_TOP_PIN 4
#define SERVO_BOTTOM_PIN 5
#define SIGN_PIN 3
#define INTERRUPT_PIN 2 // or 0?
#define HEAD_MOTOR_PIN1 9
#define HEAD_MOTOR_PIN2 10

Servo servoTop, servoBottom;

const int posDown=0, posUp=180, posMid=90;
const int servoDelay=2000;
const int boundLeft=0;
const int velo=33;
int boundRight=200;

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

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 currentSwitch=-1;
bool waitForInput=false;

byte readarr[11];
int spos = 0;

void setup () {
pinMode(in1, OUTPUT);
pinMode(in2, OUTPUT);
attachInterrupt(0, interruptPositionUpdate, RISING);
void setup()
{
// init servos
servoTop.attach(SERVO_TOP_PIN);
servoBottom.attach(SERVO_BOTTOM_PIN);
// head motor pins to output
pinMode(HEAD_MOTOR_PIN1, OUTPUT);
pinMode(HEAD_MOTOR_PIN2, OUTPUT);
attachInterrupt(INTERRUPT_PIN, interruptPositionUpdate, RISING);
Serial.begin(19200);
updateboundRight();
flipswitch();
waitForInput=true;
}

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

if(pos <= boundLeft) {
if(headPos <= boundLeft) {
Serial.println("Start Position reached");// und warten auf neue Eingabe durch Nutzer
digitalWrite(in1, LOW);
digitalWrite(in2, LOW);
inputflag = true; //
} else if (pos >= boundRight) {
Serial.println("Position reached");//Motor anhalten und Schalterprogramm ausführen
digitalWrite(in1, LOW);
digitalWrite(in2, LOW);
flipswitch();
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();
}
}

}
}


void interruptPositionUpdate() {
interruptPinChanged = 1;
if(digitalRead(sigPin))
pos--;
else
pos++;
if(!waitForInput) {
interruptPinChanged = true;
if(digitalRead(SIGN_PIN)) {
headPos--;
} else {
headPos++;
}
}
}

void updateboundRight() {
void updateBoundRight() {
/*
Es wird bestimmt welche Position als nächstes angefahren wird.
Dann wird der Kopf wieder gestartet
*/
while(!(ArraySchalter[NrSchalter]&&ArraySchalter[NrSchalter+6])){
NrSchalter++; // Überspringen leerer Spalten
int oldCurrentSwitch = currentSwitch;
currentSwitch++;
while((currentSwitch<6) && (switches[currentSwitch]==switches_old[currentSwitch]) && (switches[currentSwitch+6]==switches_old[currentSwitch+6])) {
currentSwitch++;
}
if(NrSchalter==7){
NrSchalter=0;
analogWrite(in1, LOW); //Kopf geht nach links
digitalWrite(in2, velo);
//Warteposition anfahren und Positionsvariable pos bei erreichen der
//Lichtschranke auf 0 setzen
}
else{
boundRight=ArrayPosition[NrSchalter];
NrSchalter++;
analogWrite(in1, velo); //Kopf geht nach rechts
digitalWrite(in2, LOW);
if(currentSwitch == 6) {
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
digitalWrite(HEAD_MOTOR_PIN2, LOW);
}
}

void flipswitch(){
@@ -133,47 +138,57 @@ void flipswitch(){
nach oben oder unten und wieder in Ausgangsstellung.
Genaue Servopositionen sind dabei noch abhängig der Einbauposition zu ermitteln.
*/
if(ArraySchalter[NrSchalter]){//Schalterreihe oben servo 1
if(ArraySchalteralt[NrSchalter]){
//Schalter nach unten kippen
analogWrite (servo1, down);
delay(servoT);
}
else{
//Schalter nach oben kippen
analogWrite (servo1, up);
delay(servoT);
}
analogWrite (servo1, servoStart);
delay(servoT);
digitalWrite (servo1 ,LOW);

if(switches[currentSwitch] && (!switches_old[currentSwitch])) {
servoTop.write(posDown);
delay(servoDelay);
servoTop.write(posMid);
}
if(ArraySchalter[NrSchalter+6]){//Schalterreihe unten servo 2
if(ArraySchalteralt[NrSchalter]){
//Schalter nach unten kippen
analogWrite (servo2, down);
delay(servoT);
}
else{
//Schalter nach oben kippen
analogWrite (servo2, up);
delay(servoT);
}
analogWrite (servo2, servoStart);
delay(servoT);
digitalWrite (servo2 ,LOW);
if(!switches[currentSwitch] && (switches_old[currentSwitch])) {
servoTop.write(posUp);
delay(servoDelay);
servoTop.write(posMid);
}
if(switches[currentSwitch+6] && (!switches_old[currentSwitch+6])) {
servoBottom.write(posDown);
delay(servoDelay);
servoBottom.write(posMid);
}
updateboundRight();
if(!switches[currentSwitch+6] && (switches_old[currentSwitch+6])) {
servoBottom.write(posUp);
delay(servoDelay);
servoBottom.write(posMid);
}
// optional: have both switchs switch at the same time
/*
if(switches[currentSwitch] && (!switches_old[currentSwitch])) {
servoTop.write(posDown);
}
if(!switches[currentSwitch] && (switches_old[currentSwitch])) {
servoTop.write(posUp);
}
if(switches[currentSwitch+6] && (!switches_old[currentSwitch+6])) {
servoBottom.write(posDown);
}
if(!switches[currentSwitch+6] && (switches_old[currentSwitch+6])) {
servoBottom.write(posUp);
}
delay(servoDelay);
servoTop.write(posMid);
servoBottom.write(posMid);
*/
delay(servoDelay);
switches_old[currentSwitch] = switches[currentSwitch];
switches_old[currentSwitch+6] = switches[currentSwitch+6];
}

void serialEvent(){
void serialEvent()
{
/*
Wenn eine Eingabe abgearbeitet wurde inputflag=1 wird der Controller frei für die
nächste Eingabe. Vorherige Eingaben werden somit verworfen. Die Eingabe wird aus
dem EingabeArray in ArraySchalter geschrieben und der Kopf nach rechts gestartet.
*/
while (Serial.available()) {
while(Serial.available()) {
// get the new byte:
byte inChar = Serial.read();
if (inChar == 'A') {
@@ -187,12 +202,20 @@ void serialEvent(){
else {
if (inChar == 'E') {
int posi = 0;
if ( inputflag ) {
// TODO:
// ArraySchalter fuellen
if ( waitForInput ) {
if(readarr[0] == '-') {
switches[6] = true;
} else {
switches[6] = false;
}
for(int i=1;i<6;i++) {
switches[i] = (readarr[i] == '1');
switches[i+6] = (readarr[i+5] == '1');
}
waitForInput = false;
updateBoundRight();
} // else: discard input
}
}
}

}

Loading…
Cancel
Save