Cablecam code

////////////////////////////////Relais////////////////////////////////
int motor = 7;
int motorv = A0;
int motorr = A1;
int ledvg = A2;
int ledvr = A3;
int ledhg = A4;
int ledhr = A5;

////////////////////////////////Sensoren////////////////////////////////
int v1=2;
int v2=5;
int d2=8;
int d1=9;

////////////////////////////////Schalter////////////////////////////////
int powerSwitch = 10;
int testSwitch = 11;
int startSwitch = 12;
int killSwitch = 13;
int steuergeraetStart=4;

////////////////////////////////UART Kommandos////////////////////////////////
byte wSpeed = 0x00;
byte rSpeed = 0x40;
byte wCurrentLimit = 0x01;
byte rCurrentLimit = 0x41;
byte wRegenCurrentLimit = 0x02;
byte rRegenCurrentLimit = 0x42;
byte wAcceleration = 0x03;
byte rAcceleration = 0x43;
byte wDeceleration = 0x04;
byte rDeceleration = 0x44;
byte wMinVoltOff = 0x05;
byte rMinVoltOff = 0x45;
byte wMinVoltOn = 0x06;
byte rMinVoltOn = 0x46;
byte wMaxVoltOn = 0x07;
byte rMaxVoltOn = 0x47;
byte wMaxVoltOff = 0x08;
byte rMaxVoltOff = 0x48;
byte wLockVar = 0x20;
byte wUnlockVar = 0x21;
byte wSave = 0x22;
byte wAddress = 0x26;
byte wSetConfigBits = 0x30;
byte wResetConfigBits = 0x31;
byte rSuppVolt = 0x4B;
byte rSpeedAdcInput = 0x4C;
byte rLoadCurrent = 0x4D;
byte rStatusRegister = 0x60;

////////////////////////////////Variablen////////////////////////////////
int stop1 = 0;
int stop2;
int distanz;
int counter = 0;
int start = 3;
int a = 10;
int b = 50;
int c = 80;
int spot1;
int spot2;
int spot3;
int safety=5;
int ende = 0;
boolean C1 =false;
boolean C2 =false;
boolean C3 =false;
boolean endeC;
boolean power=false;
boolean auge;
boolean playback = false;
int i;


int address = 0x80;
int answer;
boolean error = true;
int data;
int data1;
int data2;
int crc;
int suppVolt;


void setup() {
  // put your setup code here, to run once:


  Serial.begin(38400);

  pinMode(v1,INPUT_PULLUP);
pinMode(v2,INPUT_PULLUP);
pinMode(d1,INPUT_PULLUP);
pinMode(d2,INPUT_PULLUP);
  pinMode(powerSwitch, INPUT_PULLUP);
  pinMode(testSwitch, INPUT_PULLUP);
  pinMode(startSwitch, INPUT_PULLUP);
  pinMode(killSwitch, INPUT_PULLUP);
  pinMode(motor, OUTPUT);
  pinMode(motorv, OUTPUT);
  pinMode(motorr, OUTPUT);
  pinMode(ledhr, OUTPUT);
  pinMode(ledvr, OUTPUT);
  pinMode(ledhg, OUTPUT);
  pinMode(ledvg, OUTPUT);
  pinMode(steuergeraetStart, OUTPUT);


}//end of setup





void loop() {                               // put your main code here, to run repeatedly:
  lichtAus();
  while (power == false) {                  //arduino hat strom bekommen und wartet auf start Signal
    Serial.println(1);
    if (digitalRead(powerSwitch) == LOW) {
      Serial.println(2);
      power = true;
    }
  }
  digitalWrite(motor, HIGH);
  lichtGelb();                            //Motorsteuergerät und gelbe Positionslichter anschalten
  digitalWrite(steuergeraetStart,HIGH);
  while (Serial.available() == 0) {
    Serial.print(Serial.available());
    delay(1000);
    lichtAus();
    delay(1000);                            //Auf systemstart des Motordrivers warten und Positionslichter gelb blinken
    lichtGelb();
  }
delay(3000);
  address = Serial.read() + 128;

  transmitt(wCurrentLimit , 2048);
  transmitt(wMaxVoltOff , 1456);
  transmitt(wMaxVoltOn , 1456);
  transmitt(wMinVoltOff , 1133);
  transmitt(wMinVoltOn , 1133);
  transmitt(wLockVar , 1);
  transmitt(wLockVar , 2);
  transmitt(wLockVar , 4);
  transmitt(wLockVar , 8);
  transmitt(wSetConfigBits , 25);            //11001,brakemode,autobrake,no speed reset, enable configuration bits
  lichtGruen();                              //Systemstart des Steuergerätes und treffen von einstellungen durch grüne positionslichter bestätigen
  delay(1000);
  error = true;
  do {
    if (d1 == LOW && d2 == LOW) {
      error = false;
    }
    lichtRot();
    delay(500);                              //schnell rot blinkende positionslichter bei sensor error
    lichtAus();
    delay(500);
  }
  while (error);
  error = true;
motorVorwaerts();                         //motorbremse aktivieren und positionslichter grün rot
  delay(1000);
  lichtGelb();                              //standby gelbe Positionslichter

  //Systemstart abgeschlossen

while(error){


  transmitt(wAcceleration,700);
  transmitt(wSpeed,500);

  delay(7000);
  transmitt(wDeceleration,700);
  transmitt(wSpeed,0);
  delay(7000);
  transmitt(wAcceleration,700);
  transmitt(wSpeed,1000);
  delay(7000);
  transmitt(wDeceleration,700);
  transmitt(wSpeed,0);
  delay(7000);
  transmitt(wAcceleration,700);
  transmitt(wSpeed,2000);
  delay(7000);
  transmitt(wDeceleration,700);
  transmitt(wSpeed,0);
  delay(7000);
  transmitt(wAcceleration,700);
  transmitt(wSpeed,3000);
  delay(7000);
  transmitt(wDeceleration,700);
  transmitt(wSpeed,0);
  delay(7000);
  transmitt(wAcceleration,700);
  transmitt(wSpeed,4000);
  delay(7000);
  transmitt(wDeceleration,700);
  transmitt(wSpeed,0);
  delay(7000);
}


  while (power == true) {                     //dauerbetrieb startet


    if (digitalRead(startSwitch) == HIGH) {
      playback = true;
      C1 = false;
      C2 = false;
      C3 = false;
      ende = false;
      while (playback == true) {

        if(d1==HIGH||d2==HIGH){
          killBreak;
        }
        if(digitalRead(v1) == HIGH) {
          auge = true;
          if (motorv == HIGH) {
            counter++;            //Distanz messen
          }
          if (motorr == HIGH) {
            counter--;            //Distanz messen
          }
          while (auge == true) {
            if (digitalRead(v1) == LOW) {
              auge = false;
            }//end of if
          }//end of while
          while (auge == false) {
            if (digitalRead(v1) == HIGH) {
              auge = true;
            }//end of if
          }//end of while
        }//end of if

        if (counter == start) {
          transmitt(wAcceleration, 1000);
          transmitt(wSpeed, 1000);
        }
        if (counter == spot1 && C1 == false) {
          transmitt(wDeceleration, 1000);
          transmitt(wSpeed, 0);
          C1 = true;
        }
        if (counter == spot2 && C2== false) {
          transmitt(wAcceleration, 0x01);
          transmitt(wSpeed, 3000);
          C2 = true;
        }
        if (counter == spot3 && C3 == false) {
          transmitt(wDeceleration, 300);
          transmitt(wSpeed, 400);
          C3 = true;
        }
        if (counter == ende) {
          transmitt(wDeceleration, 100);
          transmitt(wSpeed, 0)  ;
          endeC = true;
        }
        if (counter > (ende - 20) && C3 == false) { //es muss das Komando zum anhalten in spot3 gegeben werden/Check variable des letzten kommandos vor ende muss geprüft werden
          transmitt(wDeceleration, 300);
          transmitt(wSpeed, 400);
        }

        if (digitalRead(killSwitch) == HIGH) {
          killBreak();
          playback = false;
        }
      }


    }









    if (digitalRead(powerSwitch) == HIGH) {

      lichtAus();
      delay(500);
      lichtGelb();
      delay(1000);
      power = false;                //kick aus dauerbetrieb schleife
    }
  }                                 //end of while  ende des dauerbetriebs
  digitalWrite(motor, LOW);         //abschalt Sequenz
  lichtGruen();
  delay(1000);
  lichtGelb();
  delay(1000);
  for (i = 0; i < 4; i++) {
    lichtRot();
    delay(1000);
    lichtAus();
    delay(1000);
  }
  lichtRot();
  delay(3000);
  lichtAus();
  while (power == false) {
    if (digitalRead(powerSwitch) == HIGH) {
      power = true;                                            //programm neu starten, zu beginn des loop springen, Systemstart
    }
  }





}//end of loop









void transmitt(int command , int data) {

  do {
    Serial.write(address);
    Serial.write(command);
    data1 = (data - (data % 128)) / 128;              //Datenwert wird in zwei 7Bit Bytes aufgeteilt
    data2 = data % 128;
    Serial.write(data1);
    Serial.write(data2);
    crc = (command + data1 + data2) & 127;            //CRC Byte Berechnung
    Serial.write(crc);
    while (Serial.available() == 0) {}                //auf Bestätigung/ERROR warten
    answer = Serial.read();
    if (answer == address) {
      error = false;
    }//end of if
    if (answer == (0x80 | address)) {
      error = true;                                   //wenn Übertragungserror Befehl wird wiederholt
    }//end of if
  }//end of do
  while (error);

}//end of transmitt



void receive(int command) {

  do {
    Serial.write(address);
    Serial.write(command);
    while (Serial.available() == 0) {}                //auf erstes Datenbyte warten
    data1 = Serial.read();
    while (Serial.available() == 0) {}                //auf zweites Datenbyte warten
    data2 = Serial.read();
    while (Serial.available() == 0) {}                //auf CRC Byte warten
    crc = Serial.read();
    data = data1 * 128 + data2;
    error = false;
    if (crc != (command + data1 + data2) & 127) {
      error = true;                                   //wenn Übertragungserror Befehl wird wiederholt
    }//end of if
  }//end of do
  while (error);

}//end of transmitt






void testlauf() {
  distanz = 0;
  if (digitalRead(d2) == LOW) {
    motorVorwaerts();
    transmitt(wSpeed, 10);
    while (digitalRead(d2) == LOW) {} //in eine Richtung bis zum ende fahren
    transmitt(wSpeed , 0);
    delay(1000);
    motorRueckwaerts();
    transmitt(wSpeed , 10);                     //in die Gegenrichtung bis zum Ende fahren
    while (digitalRead(d1) == LOW) {
      if (digitalRead(v1) == HIGH) {
        auge = true;
        distanz++;                  //Distanz messen
        while (auge == true) {
          if (digitalRead(v1) == LOW) {
            auge = false;
          }//end of if
        }//end of while
        while (auge == false) {
          if (digitalRead(v1) == HIGH) {
            auge = true;
          }//end of if
        }//end of while
      }//end of if
    }//end of while
    transmitt(wSpeed , 0);
    stop2 = distanz;
    start = safety;
    ende = distanz - safety;
    delay(1000);

  }//end of if

}//end of testlauf


void setStrecke() {
  distanz = 45;
  stop1 = 23;
  stop2 = 67;
}//end of setStrecke



void killBreak() {
  motorVorwaerts();
  transmitt(wDeceleration, 0x0001);
  transmitt(wSpeed, 0);
}

void motorVorwaerts() {
  digitalWrite(motorr, LOW);
  lichtGruenRot();
  digitalWrite(motorv, HIGH);
}

void motorRueckwaerts() {
  digitalWrite(motorv, LOW);
  lichtRotGruen();
  digitalWrite(motorr, HIGH);
}

void lichtAus() {
  digitalWrite(ledvr, HIGH);
  digitalWrite(ledhg, HIGH);
  digitalWrite(ledvg, HIGH);
  digitalWrite(ledhr, HIGH);

}
void lichtGelb() {
  digitalWrite(ledvr, LOW);
  digitalWrite(ledhg, LOW);
  digitalWrite(ledvg, LOW);
  digitalWrite(ledhr, LOW);
}
void lichtGruen() {
  digitalWrite(ledvr, HIGH);
  digitalWrite(ledhg, LOW);
  digitalWrite(ledvg, LOW);
  digitalWrite(ledhr, HIGH);
}
void lichtRot() {
  digitalWrite(ledvr, LOW);
  digitalWrite(ledhg, HIGH);
  digitalWrite(ledvg, HIGH);
  digitalWrite(ledhr, LOW);
}
void lichtRotGruen() {
  digitalWrite(ledvr, LOW);
  digitalWrite(ledhg, LOW);
  digitalWrite(ledvg, HIGH);
  digitalWrite(ledhr, HIGH);
}
void lichtGruenRot() {
  digitalWrite(ledvr, HIGH);
  digitalWrite(ledhg, HIGH);
  digitalWrite(ledvg, LOW);
  digitalWrite(ledhr, LOW);
}