Arduino Controlled Car Factory: Body Shop

#include <l293d.h>
#include <Servo.h>
#include <Wire.h>




/*Controller Setup*******************************************************

  Pin mapping Arduino Mega Shield
 ***********************************************************************/
// constants describing the pin mapping of the L293 chip
// const unint8_t[3] "motor_name" = {chip_enable, input1, input2};
const uint8_t M1[3] = {8, 45, 43};
const uint8_t M2[3] = {9, 40, 38};
const uint8_t M3[3] = {7, 41, 30};
const uint8_t M4[3] = {10, 36, 32};
const uint8_t M5[3] = {6, 48, 46};
const uint8_t M6[3] = {11, 26, 24};
const uint8_t M7[3] = {5, 44, 28};
const uint8_t M8[3] = {4, 42, 34};

// Pin mapping of digital I/O pins
const uint8_t D1 = 16;
const uint8_t D2 = 35;
const uint8_t D3 = 37;
const uint8_t D4 = 39;
const uint8_t D5 = 23;
const uint8_t D6 = 3;   // Interrupt 1
const uint8_t D7 = 2;   // Interrupt 0
const uint8_t D8 = 14;
const uint8_t D9 = 15;
const uint8_t D10 = 19; // Interrupt 4
const uint8_t D11 = 18; // Interrupt 5
const uint8_t D12 = 17;

// Pin mapping of analog I/O pins
const uint8_t a1 = A1;
const uint8_t a2 = A4;
const uint8_t a3 = A5;
const uint8_t a4 = A7;
const uint8_t a5 = A0;
const uint8_t a6 = A2;
const uint8_t a7 = A3;
const uint8_t a8 = A6;
const uint8_t a9 = A11;
const uint8_t a10 = A10;
const uint8_t a11 = A9;
const uint8_t a12 = A8;
const uint8_t a13 = A12;
const uint8_t a14 = A13;

// Pin mapping extension board
int exDataPin = 25;
int exClockPin = 33;
int exLatchPin = 29;
int exOEPin = 27;
int exLEDgreen = 47;
int exLEDred = 49;

/*Machine Setup**********************************************************

  maschinenspezifische Variablen und Konstanten
 ***********************************************************************/

l293d motorAxis1(0);
l293d motorAxis2(0);
l293d motorAxis3(0);
Servo servoAxis5;
l293d vakuum(0);

l293d laser(0);
l293d millingAxisX(0);
l293d millingMachine(0);

// declare limit switch variables
const uint8_t limitSwitch1 = a10;
const uint8_t limitSwitch2 = D9;
const uint8_t limitSwitch3 = D8;

const uint8_t limitSwitchMillingX = D5;

// declare counter Pin variables
const uint8_t counter1Pin = D11;
const uint8_t counter2Pin = D6;
const uint8_t counter3Pin = D7;

const uint8_t counterMillingXPin = D10;

// declare dimensions [Millimeter]
const float l1_0 = 15; // horizontaler Abstand von Drehpunkt zum Mittelpunkt der Achse 2 (X-Y)
const float l1_1 = 103.5; // vertikaler Abstand vom Punkt (0|0|0) zum Mittlepunkt der Achse 2 (nur Z)
const float l2 = 150;
const float l3 = 142.5;
const float l5 = 60;
const float l6 = 30;

const float l_tool = 5;

// declare minimum / maximum range
const float rangeAxis1[2] = { -160.0, 0};
const float rangeAxis2[2] = { 30.0, 110.0};
const float rangeAxis3[2] = { 45.0, 140.0};
const float rangeAxis3s[2] = { 0.0, 80.0};
const float rangeAxis5[2] = {0.0 , 180.0};

const float degree2encoder = 23.978; // Impulse / Grad (Drehscheibe)
const float degree2impulse = 1.256;

const float pi = 3.14159;
const float rad2deg = 180 / pi;
const float deg2rad = 1 / rad2deg;

/*Program Setup**********************************************************

  enthaelt globale programmspezifische Variablen
 ***********************************************************************/

int i2c_befehl = 0;
int i2c_status = 0;

// Interrupt
volatile int posA1, posA2, posA3, posA5;
volatile int posMX = 0;
int directA1 = 1, directA2 = 1, directA3 = 1, directMX = 1;
int clearPos1 = 1 , clearPos2 = 1, clearPos3 = 1, clearPosMX = 1;

// Geschwindigkeitssteuerung Motoren
uint8_t speedA1 = 140;
uint8_t speedA2 = 160;
uint8_t speedA3 = 130;

// Geschwindigkeitssteuerung Servo
int servoA5ot, servoA5nt;
// Debounce
volatile unsigned int dbotMX = 0;
volatile unsigned int dbntMX = 0;

// Winkel
float q1, q2, q3s, q3, q4, q5, q5s; // q2s: Stellwinkel fuer Achse 2

// Positionsdatenbank
float posLib[][3] =
{
  { -150.0, -92.0, 144.0},
  { -202.0, -114.0, 18.0}, // Werkstueckaufnahme
  { -190.0, -92.0, 55.0},
  { -177.0, -40.0, 55.0}, // Laser
  { -177.0, -40.0, 55.0},
  { -150.0, 65.0, 66.0}, // Fraese
  { -33.0, 229.0, 100.0},
  { -30.0, 229.0, 80.0}, // Montage
};

/************************************************************************
  Unterprogramme
 ***********************************************************************/

// ISR

// ISR Achse 1
void counterA1()
{
  posA1 = (posA1 + directA1) * clearPos1;
}

// ISR Achse 2
void counterA2()
{
  posA2 = (posA2 + directA2) * clearPos2;
}
// ISR Achse 3
void counterA3()
{
  posA3 = (posA3 + directA3) * clearPos3;
}
// ISR Milling Axis X
void counterMX()
{
  dbntMX = millis();
  if ((dbntMX - dbotMX) > 1)
  {
    posMX = (posMX + directMX);
    dbotMX = dbntMX;
  }
  posMX *= clearPosMX;
}

bool pos(l293d & axis_ref, int limitSwitch, int currentPos, int & clearPos_ref, int & directAxis_ref, int target, int motorSpeed)
{
  if (target == 0)
  {
    if (digitalRead(limitSwitch) == 0)
    {
      clearPos_ref = 0;
      axis_ref.ccw(motorSpeed);
      return false;
    }
    else
    {
      axis_ref.stop();
      clearPos_ref = 1;
      return true;
    }
  }
  else if (target == currentPos)
  {
    axis_ref.stop();
    // remove in order to keep measureing: directAxis_ref = 0;
    return true;
  }
  else if (target > currentPos)
  {
    axis_ref.cw(motorSpeed);
    directAxis_ref = 1;
    return false;
  }
  else if (target < currentPos)
  {
    axis_ref.ccw(motorSpeed);
    directAxis_ref = -1;
    return false;
  }
}

int pos1(int target, int motorspeed)
{
  int currentPos = posA1;
  return pos(motorAxis1, limitSwitch1, currentPos, clearPos1, directA1, target, motorspeed);
}
int pos2(int target, int motorspeed)
{
  int currentPos = posA2;
  return pos(motorAxis2, limitSwitch2, currentPos, clearPos2, directA2, target, motorspeed);
}
int pos3(int target, int motorspeed)
{
  int currentPos = posA3;
  return pos(motorAxis3, limitSwitch3, currentPos, clearPos3, directA3, target, motorspeed);
}
int posMillingX(int target, int motorspeed)
{
  int currentPos = posMX;
  return pos(millingAxisX, limitSwitchMillingX, currentPos, clearPosMX, directMX, target, motorspeed);
}

// Achssteuerung
// auf Achsteuerung soll NICHT von auen zugegriffen werden, nur ber die moveJ Befehle
int axis1(float angle, int motorspeed)
{
  /* Definition Achse 1:

      Nullpunkt bei q1 = 0
      Arbeitsbereich einseitig fr q1 <= 0
  */

  // Pruefe ob der Winkel im gueltigen Arbeitsbereich der Maschine liegt
  if (angle < rangeAxis1[0] || angle > rangeAxis1[1])
  {
    errorDisplay(301, 1);
    return 1;
  }
  else
  {
    int impulse = (angle * -1) * degree2encoder;
    return pos1(impulse, motorspeed);
  }
}

int axis2(float angle, int motorspeed)
{
  // Pruefe ob der Winkel im gueltigen Arbeitsbereich der Maschine liegt
  if (angle < rangeAxis2[0] || angle > rangeAxis2[1])
  {
    errorDisplay(301, 2);
    return 1;
  }
  else
  {
    // Umrechnung von Zielwinkel in Maschinenwinkel, Multiplikation mit der Encoderkonstante
    int impulse = (-71 + (180 - angle)) * degree2encoder;
    return pos2(impulse, motorspeed);
  }
}

int axis3(float angle, float  rq2, int motorspeed)
{
  /* Randbediengungen Achse 3
      Stellwinkel: der vom Roboter gestellte Winkel der Achse 3
      q3: Winkel zwischgen l2 und l3:
      q3 = (180-q2) - q3s
      q3s = 180 - q2 - q3

      Bedingungen
      1) q3s: (180-q2) - q3s > 60, damit Bewegung gueltig ist.
      2) q3s: 0 < q3s < 80
      Bei Fehler 1) Bewegung unterbrechen
      Bei Fehler 2) Bewegung abbrechen
  */

  // Pruefe ob der Zielwinkel im gueltigen Arbeitsbereich der Maschine liegt (Bedingung 1 und 2)
  q3s = 180 - rq2 - angle;
  if (q3s < rangeAxis3s[0] || q3s > rangeAxis3s[1] || angle < rangeAxis3[0] || angle > rangeAxis3[1])
  {
    errorDisplay(301, 3);
    return 1;
  }
  else
  {
    // Umrechnung von Zielwinkel in Maschinenwinkel, Multiplikation mit der Encoderkonstante
    // Pruefe in Echtzeit, ob der Winkel zwischen l2 und l3 > 80 ist (Bedinung 1)
    // lq2 und lq3 enthalten Winkel in Echtzeit
    float lq2 = (109.0 - (posA2 / degree2encoder));
    int lq3s = posA3 / degree2encoder;
    if ((180 - lq2 - lq3s) < rangeAxis3[0] || (180 - lq2 - lq3s) > rangeAxis3[1])
    {
      motorAxis3.stop();
      errorDisplay(302, 3);
      return 0;
    }
    else
    {
      int impulse = q3s * degree2encoder;
      return pos3(impulse, motorspeed);

    }
  }
}

int axis5(int angle, int servospeed)
{
  // variable posA5 enthaelt Winkel in Grad
  // servospeed: Zeit in ms
  if (angle < rangeAxis5[0] || angle > rangeAxis5[1])
  {
    errorDisplay(301, 5);
    return 1;
  }
  else
  {
    if (servospeed == 0)
    {
      posA5 = q5s + 140 + (posA1 / degree2encoder * -1);
      servoAxis5.write(posA5);
      return 1;
    }
    else {
      servoA5nt = millis();
      if ((servoA5nt - servoA5ot) > servospeed)
      {
        servoA5ot = servoA5nt;

        if (angle > posA5)
        {
          posA5++;
          servoAxis5.write(posA5);
        }
        else if (angle < posA5)
        {
          posA5--;
          servoAxis5.write(posA5);
        }
        else
        {
          return 1;
        }
      }
      else
      {
        return 0;
      }
    }
  }
}

void referencePoint()
{
  int refP = 0;
  while (!refP)
  {
    refP = pos1(0, speedA1);
    refP &= pos2(0, speedA2);
    refP &= pos3(0, speedA3);
    refP &= axis5(140, 15);
  }
}

void errorDisplay(int errorCode, int detail)
{
  /*  Fehler Codes

      Achsensteuerung
      - 301: Ungueltige Bewegungsanfrage Achse
      - 302: Konflikt mit anderer Achse

      Systemfehler:
      - 100: Unbekannter Fehler
  */
  switch (errorCode)
  {
    case 301:
      Serial.print("301: Ungueltige Bewegungsanfrage Achse ");
      Serial.println(detail);
      break;
    case 302:
      Serial.print("302: Konflikt mit anderer Achse - Achse ");
      Serial.println(detail);
      break;
    default:
      Serial.println("100: Unbekannter Fehler  aufgetreten");
      break;
  }
}

/*Inverse Kinematic******************************************************

  errechnet aus Zielpunkt mit Vektor (ggf. inklusive Werkzeug die Stell-
  winkel des Roboters

  Definition:
  - alpha: Winkel des Werkzeugs auf Element
  - targetPoint: Zielpunkt des TCPs am Objekt
  - int toolParameter: Index fr die Auswahl passender Paramter aus einer
    Werkzeugdatenbank

  Vektor:
  - Darstellung als  Array
  - Datentyp: float
  - eventueller vierter Index enthlt die Laenge des Vektors

  Algorithmus:
  - Zielvektor errechnen: x1 = 0
                          x2 = 0
                          x3 = -1
  - Inverse Kinematik nach: https://www.youtube.com/watch?v=3s2x4QsD3uM
    Vorbereitung: Maschinendaten laden und vect_5toTool berechnen

    q1: vect_0to5 = vect_0toPoint - vect_5toTool
        q1 = atan (vect_0to5[1]/vect0to5[0])
    q3: vect_2to5 = vect_0to5 - vect_0to2
                                (cos q1 * l1_0)
                  = vect_0to5 - (sin qi * l1_0)
                                (l1_1         )
        beliebiges Dreieck l2, l3, |vect_2to5|, q3 ueber Kosinussatz:
        q3 = acos (l3^2 + l2^2 - |vect_2to5|^2) / (2*l3*l2)
    q2: beliebiges Dreieck l2, l3, |vect_2to5|, q2 ueber Kosinussatz:
        q2' = acos (|vect_2to5|^2 + l2^2 - l3^2) / (2*|vect_2to5|*l2)
        q2'' = asin (vect_2to5[2] / |vect_2to5|)
        q2 = q2' + q2''
    q5: beta = q1
        alpha = Zielwinkel (alpha)
        q5 = 180 - alpha -beta


 ***********************************************************************/
void inverseKinematic(float alpha, float targetPoint[])
{
  // Errechne Zielvektor
  float unit_vect_5toTool[4];
  float vect_5toTool[4];
  // Automatische Ausrichtung nach unten
  unit_vect_5toTool[0] = 0;
  unit_vect_5toTool[1] = 0;
  unit_vect_5toTool[2] = -1;
  vect_5toTool[3] = 1; //sqrt (pow(vect_5toTool[0], 2) + pow(vect_5toTool[1], 2) + pow(vect_5toTool[2], 2));

  // Verlaengere Zielvektor auf Abmessungen des Roboters
  for (int i = 0; i < 4; i++)
  {
    vect_5toTool[i] = unit_vect_5toTool[i] * (l5 + l6 + l_tool);
  }

  // Berechne vect_0to5
  float vect_0to5[3];
  for (int i = 0; i < 3; i++)
  {
    vect_0to5[i] = targetPoint[i] - vect_5toTool[i];
    //Serial.println(vect_0to5[i]);
  }

  // Berechne q1
  // q1 = atan (vect_0to5[0] / vect_0to5[1]) * rad2deg; // nur wenn vect_0to5[1] > 0

  // cos-1(y/hyp_0to5)
  q1 = acos (vect_0to5[1] / sqrt(pow(vect_0to5[0], 2) + pow(vect_0to5[1], 2))) * rad2deg;
  if (vect_0to5[0] < 0)
  {
    q1 *= -1;
  }

  // Berechne vect_2to5
  float vect_2to5[4];
  vect_2to5[0] = vect_0to5[0] - (sin(q1 * deg2rad) * l1_0);
  vect_2to5[1] = vect_0to5[1] - (cos(q1 * deg2rad) * l1_0);
  vect_2to5[2] = vect_0to5[2] - l1_1;
  vect_2to5[3] = sqrt (pow(vect_2to5[0], 2) + pow(vect_2to5[1], 2) + pow(vect_2to5[2], 2));

  // Berechne q3
  q3 = acos((pow(l3, 2) + pow(l2, 2) - pow(vect_2to5[3], 2)) / (2 * l3 * l2)) * rad2deg;

  // Berechne q2
  float q2_1, q2_2;
  q2_1 = acos((pow(vect_2to5[3], 2) + pow(l2, 2) - pow(l3, 2)) / (2 * vect_2to5[3] * l2)) * rad2deg;
  q2_2 = atan(vect_2to5[2] / vect_2to5[3]) * rad2deg;
  q2 = q2_1 + q2_2;

  // Berechne q5

  q5s = alpha;
  q5 = q5s + q1 + 140;
}

void moveJ(float alpha, float targetPoint[])
{
  inverseKinematic(alpha, targetPoint);
  Serial.println(q5);
  bool i = false;
  while (i == false)
  {
    i = axis1(q1, speedA1);
    i &= axis2(q2, speedA2);
    i &= axis3(q3, q2, speedA3);
    i &= axis5(q5, 0);
  }
}

void moveP(float alpha, float targetPoint[])
{
  inverseKinematic(0.0, targetPoint);
  bool i = false;
  while (i == false)
  {
    i = axis1(q1, speedA1);
    i &= axis5(q5, 0);
  }

  i = false;
  while (i == false)
  {
    i = axis2(q2, speedA2);
    i &= axis3(q3, q2, speedA3);
  }
  delay(250);
}

/*void moveP2P(float startingPoint[], float targetPoint[], float resolution)
  {
  // berechne Fahrtvektor
  float vect_s2t[4];
  vect_s2t[0] = targetPoint[0] - startingPoint[0];
  vect_s2t[1] = targetPoint[1] - startingPoint[1];
  vect_s2t[2] = targetPoint[2] - startingPoint[2];
  vect_s2t[3] = sqrt(pow(vect_s2t[0], 2) + pow(vect_s2t[1], 2) + pow(vect_s2t[2], 2));

  // Fahre zum Startpunkt (sofern noch nicht geschehen)
  moveJ(startingPoint);

  float vect_nxt[3];
  for (float i = 0; i < (vect_s2t[3] - resolution); i += resolution)
    {
    vect_nxt[0] = startingPoint[0] + (i / vect_s2t[3]) * vect_s2t[0];
    vect_nxt[1] = startingPoint[1] + (i / vect_s2t[3]) * vect_s2t[1];
    vect_nxt[2] = startingPoint[2] + (i / vect_s2t[3]) * vect_s2t[2];
    Serial.println(vect_nxt[0]);
    Serial.println(vect_nxt[1]);
    Serial.println(vect_nxt[2]);
    moveJ(alpha, vect_nxt, l_tool);
    }
  moveJ(targetPoint);
  }*/

// Sonstige Unterprogramme

void fraese()
{
  for (int i = 50; i < 255; i++)
  {
    millingMachine.on(i);
    delay(15);
  }
  while (!posMillingX(34, 255)) {}
  while (!axis5(q5 - 25, 35)) {}
  delay(500);
  while (!axis5(q5 + 25, 35)) {}
  delay(500);
  while (!axis5(q5, 35)) {}
  while (!posMillingX(0, 255)) {}
  for (int i = 255; i > 0; i--)
  {
    millingMachine.on(i);
    delay(5);
  }
  millingMachine.off();

}
void referenzfahrt(int befehl)
{
  i2c_befehl = 0;
  i2c_status = 0;
  Serial.println("Referenzfahrt: Programm gestartet");
  millingMachine.off();
  laser.off();
  vakuum.off();
  referencePoint();
  while (!posMillingX(0, 255)) {}
  Serial.println("Referenzfahrt: Programm beendet");
  i2c_status = befehl;
}

void vorbereitung(int befehl)
{
  i2c_befehl = 0;
  Serial.println("Vorbereitung: Programm gestartet");

  // Aufnahme
  moveJ(90.0, posLib[0]);
  moveJ(90.0, posLib[1]);
  delay(1000);
  vakuum.on();
  delay(2000);
  moveJ(90.0, posLib[2]);

  // Laser
  moveJ(0.0, posLib[3]);
  laser.on();
  delay(500);
  while (!axis5(q5 - 30, 20)) {}
  delay(500);
  while (!axis5(q5 + 30, 20)) {}
  delay(500);
  laser.off();
  while (!axis5(q5, 20)) {}
  delay(1000);

  // Fraese
  moveJ(00.0, posLib[5]);
  delay(500);
  fraese();
  delay(500);

  // Referenzpunkt
  referencePoint();
  Serial.println("Vorbereitung: Programm beendet");
  i2c_status = befehl;
}

void montage(int befehl)
{
  i2c_befehl = 0;
  Serial.println("Montage: Programm gestartet");
  moveJ(0.0, posLib[6]);
  delay(500);
  moveJ(-2.0, posLib[7]);
  delay(1000);
  vakuum.off();
  delay(1000);
  referencePoint();
  Serial.println("Montage: Programm beendet");
  i2c_status = befehl;
}
/************************************************************************
  Hauptprogramm
 ***********************************************************************/

void setup()
{
  // Serial
  Serial.begin(9600);

  // I2C setup:
  Wire.begin(0x44);             // join i2c bus with address 0x33
  Wire.onReceive(receiveEvent); // Reaktion auf Befehl
  Wire.onRequest(requestEvent); // Reaktion auf Anfrage

  // Motorinitialisierung
  motorAxis1.setMotor(M8[0], M8[1], M8[2]);
  motorAxis2.setMotor(M6[0], M6[1], M6[2]);
  motorAxis3.setMotor(M5[0], M5[1], M5[2]);
  vakuum.setMotor(M4[0], M4[1], M4[2]);

  millingAxisX.setMotor(M1[0], M1[1], M1[2]);
  millingMachine.setMotor(M2[0], M2[1], M2[2]);
  laser.setMotor(M7[0], M7[1], M7[2]);

  servoAxis5.attach(D12);

  pinMode(limitSwitch1, INPUT_PULLUP);
  pinMode(limitSwitch2, INPUT_PULLUP);
  pinMode(limitSwitch3, INPUT_PULLUP);
  pinMode(limitSwitchMillingX, INPUT_PULLUP);

  pinMode(counter1Pin, INPUT_PULLUP);
  pinMode(counter2Pin, INPUT_PULLUP);
  pinMode(counter3Pin, INPUT_PULLUP);
  pinMode(counterMillingXPin, INPUT_PULLUP);

  attachInterrupt(digitalPinToInterrupt(counter1Pin), counterA1, CHANGE);
  attachInterrupt(digitalPinToInterrupt(counter2Pin), counterA2, CHANGE);
  attachInterrupt(digitalPinToInterrupt(counter3Pin), counterA3, CHANGE);
  attachInterrupt(digitalPinToInterrupt(counterMillingXPin), counterMX, CHANGE);
}

void loop()
{ 
   /*referenzfahrt(1);
    vorbereitung(1);
    montage(1);*/
  
  switch (i2c_befehl)
  {
    case 20:
      referenzfahrt(i2c_befehl);
      break;
    case 21:
      vorbereitung(i2c_befehl);
      break;
    case 22:
      montage(i2c_befehl);
      break;
    default:
      break;
  }
}

/* I2C
   function that executes whenever data is received from master
   this function is registered as an event
*/
void receiveEvent(int howMany) {
  while (1 < Wire.available()) { // loop through all but the last
    char c = Wire.read(); // receive byte as a character
  }
  int x = Wire.read();    // receive byte as an integer
  //Serial.println(x);
  i2c_befehl = x;
}

void requestEvent() {
  Wire.write(i2c_status);
}