# include <PID_v1.h>
#include <Wire.h>
#define hipvl1 22
#define hipvl2 23
#define hipvr1 24
#define hipvr2 25
#define hiphl1 26
#define hiphl2 27
#define hiphr1 28
#define hiphr2 29
#define kneevl1 30
#define kneevl2 31
#define kneevr1 32
#define kneevr2 33
#define kneehl1 34
#define kneehl2 35
#define kneehr1 36
#define kneehr2 37 
#define potkvl A0
#define potkvr A1
#define potkhl A2  
#define potkhr A3
#define pothvl A4
#define pothvr A5
#define pothhl A6
#define pothhr A7
#define remote1 A8
#define remote2 A9
int neigung;
int hiplean = 8; //allgemeiner Neigungswert fr Hfen
int vl;
int vr;
int hl;
int hr;
double Kp=0.03, Ki=0.6, Kd=0.6; 
double kneevlset;
double kneevrset;
double kneehlset;
double kneehrset;
double hipvlset;
double hiphlset;
double hipvrset;
double hiphrset;
double kneevlinp;
double kneevrinp;
double kneehlinp;
double kneehrinp;
double hipvlinp;
double hiphlinp;
double hipvrinp;
double hiphrinp;
double kneevloutp;
double kneevroutp;
double kneehloutp;
double kneehroutp;
double hipvloutp;
double hiphloutp;
double hipvroutp;
double hiphroutp;
int kneevl_;
int kneevr_;
int kneehl_;
int kneehr_;
int hipvl_;
int hipvr_;
int hiphl_;
int hiphr_;
PID kneevlPID (&kneevlinp, &kneevloutp, &kneevlset, Kp, Ki, Kd, DIRECT);
PID kneevrPID (&kneevrinp, &kneevroutp, &kneevrset, Kp, Ki, Kd, DIRECT);
PID kneehlPID (&kneehlinp, &kneehloutp, &kneehlset, Kp, Ki, Kd, DIRECT);
PID kneehrPID (&kneehrinp, &kneehroutp, &kneehrset, Kp, Ki, Kd, DIRECT);
PID hipvlPID (&hipvlinp, &hipvloutp, &hipvlset, Kp, Ki, Kd, DIRECT);
PID hipvrPID (&hipvrinp, &hipvroutp, &hipvrset, Kp, Ki, Kd, DIRECT);
PID hiphlPID (&hiphlinp, &hiphloutp, &hiphlset, Kp, Ki, Kd, DIRECT);
PID hiphrPID (&hiphrinp, &hiphroutp, &hiphrset, Kp, Ki, Kd, DIRECT);
void setup() {
  pinMode(hipvl1,OUTPUT);
  pinMode(hipvl2,OUTPUT);
  pinMode(hipvr1,OUTPUT);
  pinMode(hipvr2,OUTPUT);
  pinMode(hiphl1,OUTPUT);
  pinMode(hiphl2,OUTPUT);
  pinMode(hiphr1,OUTPUT);
  pinMode(hiphr2,OUTPUT);
  pinMode(kneevl1,OUTPUT);
  pinMode(kneevl2,OUTPUT);
  pinMode(kneevr1,OUTPUT);
  pinMode(kneevr2,OUTPUT);
  pinMode(kneehl1,OUTPUT);
  pinMode(kneehl2,OUTPUT);
  pinMode(kneehr1,OUTPUT);
  pinMode(kneehr2,OUTPUT);
  pinMode(potkvl,INPUT);
  pinMode(potkvr,INPUT);
  pinMode(potkhl,INPUT);
  pinMode(potkhr,INPUT);
  pinMode(pothvl,INPUT);
  pinMode(pothvr,INPUT);
  pinMode(pothhl,INPUT);
  pinMode(pothhr,INPUT);
  pinMode(remote1,INPUT);
  pinMode(remote2,INPUT);
  kneevlPID.SetMode(AUTOMATIC);
  kneevlPID.SetOutputLimits(0,100);
  kneevlPID.SetTunings(Kp, Ki, Kd);
  kneevlPID.SetSampleTime(100);
  kneevrPID.SetMode(AUTOMATIC);
  kneevrPID.SetOutputLimits(0,100);
  kneevrPID.SetTunings(Kp, Ki, Kd);
  kneevrPID.SetSampleTime(100);
  kneehlPID.SetMode(AUTOMATIC);
  kneehlPID.SetOutputLimits(0,100);
  kneehlPID.SetTunings(Kp, Ki, Kd);
  kneehlPID.SetSampleTime(100);
  kneehrPID.SetMode(AUTOMATIC);
  kneehrPID.SetOutputLimits(0,100);
  kneehrPID.SetTunings(Kp, Ki, Kd);
  kneehrPID.SetSampleTime(100);
  hipvlPID.SetMode(AUTOMATIC);
  hipvlPID.SetOutputLimits(0,100);
  hipvlPID.SetTunings(Kp, Ki, Kd);
  hipvlPID.SetSampleTime(100);
  hipvrPID.SetMode(AUTOMATIC);
  hipvrPID.SetOutputLimits(0,100);
  hipvrPID.SetTunings(Kp, Ki, Kd);
  hipvrPID.SetSampleTime(100);
  hiphlPID.SetMode(AUTOMATIC);
  hiphlPID.SetOutputLimits(0,100);
  hiphlPID.SetTunings(Kp, Ki, Kd);
  hiphlPID.SetSampleTime(100);
  hiphrPID.SetMode(AUTOMATIC);
  hiphrPID.SetOutputLimits(0,100);
  hiphrPID.SetTunings(Kp, Ki, Kd);
  hiphrPID.SetSampleTime(100);
  
  Serial.begin(9600);
  Wire.begin();  
  HOME();
}
void loop() {
}
  void hipbend() {
    int hipvl_ = analogRead(pothvl);
    int hipvr_ = analogRead(pothvr);
    int hiphl_ = analogRead(pothhl);
    int hiphr_ = analogRead(pothhr);
    int hipleanvl = hipvl_; // aktueller Wert der Hftgelenke
    int hipleanvr = hipvr_;
    int hipleanhl = hiphl_;
    int hipleanhr = hiphr_;
    vl = 1;
    vr = 1;
    hl = 1;
    hr = 1;
if (neigung == 1)
  {
    hipvl_ = hipvl_ - hiplean; //Sollwert fr die Hftneigung
    hiphl_ = hiphl_ - hiplean;
    hipvr_ = hipvr_ + hiplean;
    hiphr_ = hiphr_ + hiplean;
  }
else if (neigung == 0)
  {
    hipvl_ = hipvl_ + hiplean; //Sollwert fr die Hftneigung
    hiphl_ = hiphl_ + hiplean;
    hipvr_ = hipvr_ - hiplean;
    hiphr_ = hiphr_ - hiplean;
  }
      if (hipvl_ > hipleanvl)
      {
        hipvlout();
        //hipvl nach innen 
      }
      else if (hipvl_ < hipleanvl)
      {
        hipvlin();
        //hipvl nach auen
      }
       else if (hipvl_ = hipleanvl)
      {
      }
 
      if (hipvr_ > hipleanvr)
        {
          hipvrin(); 
           //hipvr nach innen
        }
        else if (hipvr_ < hipleanvr)
        {
          hipvrout();
          //hipvr nach auen
        }
        else if (hipvr_ = hipleanvr)
        {
        }
          if (hiphl_ > hipleanhl)
          {
            hiphlout();
            //hiphl nach innen
          }
          else if (hiphl_ < hipleanhl)
          {
            hiphlin();
            //hiphl nach auen
          }
           else if (hiphl_ = hipleanhl)
          {
          }
            if (hiphr_ > hipleanhr)   
            {
              hiphrout();
              //hiphr nach innen
            }
            else if (hiphr_ < hipleanhr)   
            {
              hiphrin();
              //hiphr nach auen
            }
            
            else if (hiphr_ = hipleanhr)   
            {
            }
if (neigung == 1)
  {  
    while(vl==1 || vr==1 || hl==1 || hr==1)
    {
      if(analogRead(pothvl) <= hipvl_)
      {
        vl = 0;
        hipvllow();
        //hipvl low
      }
      if(analogRead(pothvr) >= hipvr_)
      {
        vr = 0;
        hipvrlow();
        //hipvr low
      }
      if(analogRead(pothhl) <= hiphl_)
      {
        hl = 0;
        hiphllow();
        //hiphl low 
      }
      if(analogRead(pothhr) >= hiphr_)
      {
        hr = 0;
        hiphrlow();
        //hiphr low
      }
       delay(500);   
    }
  }
              if (neigung == 0)
                {  
                  while(vl==1 || vr==1 || hl==1 || hr==1)
                  {
                    if(analogRead(pothvl) >= hipvl_)
                    {
                      vl = 0;
                      hipvllow();
                      //hipvl low
                    }
              
                    if(analogRead(pothvr) <= hipvr_)
                    {
                      vr = 0;
                      hipvrlow();
                      //hipvr low
                    }
              
                    if(analogRead(pothhl) >= hiphl_)
                    {
                      hl = 0;
                      hiphllow();
                      //hiphl low 
                    }
              
                    if(analogRead(pothhr) <= hiphr_)
                    {
                      hr = 0;
                      hiphrlow();
                      //hiphr low
                    }
                    delay(500);    
                  }
              
                }
    
  }
  
   void hipvlin ()
  {
    digitalWrite(hipvl2,HIGH);
    digitalWrite(hipvl1,LOW);
    Serial.println("hipvlin");
  }
   
   void hipvlout ()
  {
    digitalWrite(hipvl1,HIGH);
    digitalWrite(hipvl2,LOW);
    Serial.println("hipvlout");
  }
   
   void hipvrin ()
  {
    digitalWrite(hipvr2,HIGH);
    digitalWrite(hipvr1,LOW);
    Serial.println("hipvrin");
  }
   
   void hipvrout ()
  {
    digitalWrite(hipvr1,HIGH);
    digitalWrite(hipvr2,LOW);
    Serial.println("hipvrout");
  }
   
   void hiphlin ()
  {
    digitalWrite(hiphl2,HIGH);
    digitalWrite(hiphl1,LOW);
    Serial.println("hiphlin");
  }
   
   void hiphlout ()
  {
    digitalWrite(hiphl1,HIGH);
    digitalWrite(hiphl2,LOW);
    Serial.println("hiphlout");
  }
   
   void hiphrin ()
  {
    digitalWrite(hiphr2,HIGH);
    digitalWrite(hiphr1,LOW);
    Serial.println("hiphrin");
  }
   
   void hiphrout ()
  {
    digitalWrite(hiphr1,HIGH);
    digitalWrite(hiphr2,LOW);
    Serial.println("hiphrout");
  }
   
   void kneevlin ()
  {
    digitalWrite(kneevl2,HIGH);
    digitalWrite(kneevl1,LOW);
    Serial.println("kneevlin");
  }
   void kneevlout ()
  {
    digitalWrite(kneevl1,HIGH);
    digitalWrite(kneevl2,LOW);
    Serial.println("kneevlout");
  }
   
   void kneevrin ()
  {
    digitalWrite(kneevr2,HIGH);
    digitalWrite(kneevr1,LOW);
    Serial.println("kneevrin");
  }
   
   void kneevrout ()
  {
    digitalWrite(kneevr1,HIGH);
    digitalWrite(kneevr2,LOW);
    Serial.println("kneevrout");
  }
   
   void kneehlin ()
  {
    digitalWrite(kneehl2,HIGH);
    digitalWrite(kneehl1,LOW);
    Serial.println("kneehlin");
  }
   
   void kneehlout ()
  {
    digitalWrite(kneehl1,HIGH);
    digitalWrite(kneehl2,LOW);
    Serial.println("kneehlout");
  }
   
   void kneehrout ()
  {
    digitalWrite(kneehr2,HIGH);
    digitalWrite(kneehr1,LOW);
    Serial.println("kneehrin");
  }
   
   void kneehrin ()
  {
    digitalWrite(kneehr1,HIGH);
    digitalWrite(kneehr2,LOW);
    Serial.println("kneehrout");
  }
   
   void  hipvllow()
  {
    digitalWrite(hipvl1,LOW);
    digitalWrite(hipvl2,LOW);
    Serial.println("hipvllow");
  }
   
   void  hipvrlow()
  {
    digitalWrite(hipvr1,LOW);
    digitalWrite(hipvr2,LOW);
    Serial.println("hipvrlow");
  }
   
   void  hiphllow()
  {
    digitalWrite(hiphl1,LOW);
    digitalWrite(hiphl2,LOW);
    Serial.println("hiphllow");
  }
   
   void  hiphrlow()
  {
    digitalWrite(hiphr1,LOW);
    digitalWrite(hiphr2,LOW);
    Serial.println("hiphrlow");
  }
   
   void  kneevllow()
  {
    digitalWrite(kneevl1,LOW);
    digitalWrite(kneevl2,LOW);
    Serial.println("kneevllow");
  }
   
   void  kneevrlow()
  {
    digitalWrite(kneevr1,LOW);
    digitalWrite(kneevr2,LOW);
    Serial.println("kneevrlow");
  }
   
   void  kneehllow()
  {
    digitalWrite(kneehl1,LOW);
    digitalWrite(kneehl2,LOW);
    Serial.println("kneehllow");
  }
   
   void  kneehrlow()
  {
    digitalWrite(kneehr1,LOW);
    digitalWrite(kneehr2,LOW);
    Serial.println("kneehrlow");
  }
         
   void kneevl()
  {
kneevl_ = 0;    
while(kneevl_ == 0)
  {
    kneevlinp = map(analogRead(potkvl),235,453,0,10);
    kneevlPID.Compute();
      if(map(analogRead(potkvl),235,453,0,10) > kneevlset)
      {
        kneevlout();
        delay(kneevloutp);
      }
    
      if(map(analogRead(potkvl),235,453,0,10) < kneevlset)
      {
        kneevlin();
        delay(kneevloutp);
      }
    
      
      if(map(analogRead(potkvl),235,453,0,10) == kneevlset)
      {        
        kneevllow();
        kneevl_ = 1;
      }
  }
  }
//_______________________________________________________________________________________
   void kneevr()
  {
kneevr_ = 0;
while(kneevr_ == 0)
 {
    kneevrinp = map(analogRead(potkvr),400,680,0,10);
    kneevrPID.Compute();
      if(map(analogRead(potkvr),400,680,0,10) > kneevrset)
      {
        kneevrout();
        delay(kneevroutp);
      }
    
      if(map(analogRead(potkvr),400,680,0,10) < kneevrset)
      {
        kneevrin();
        delay(kneevroutp);
      }
    
      
      if(map(analogRead(potkvr),400,680,0,10) == kneevrset)
      {        
        kneevrlow();
        kneevr_ = 1;
      }
  }
  }
//_______________________________________________________________________________________
   void kneehl()
  {
kneehl_ = 0;
while(kneehl_ == 0)
 {
    kneehlinp = map(analogRead(potkhl),171,383,0,10);
    kneehlPID.Compute();
      if(map(analogRead(potkhl),171,383,0,10) > kneehlset)
      {
        kneehlout();
        delay(kneehloutp);
      }
    
      if(map(analogRead(potkhl),171,383,0,10) < kneehlset)
      {
        kneehlin();
        delay(kneehloutp);
      }
    
      
      if(map(analogRead(potkhl),171,383,0,10) == kneehlset)
      {        
        kneehllow();
        kneehl_ = 1;
      }
  }
  }
  
//_______________________________________________________________________________________
   void kneehr()
  {
kneehr_ = 0;    
while(kneehr_ == 0)
  {  
    kneehrinp = map(analogRead(potkhr),418,671,0,10);
    kneehrPID.Compute();
      if(map(analogRead(potkhr),418,671,0,10) > kneehrset)
      {
        kneehrout();
        delay(kneehroutp);
      }
    
      if(map(analogRead(potkhr),418,671,0,10) < kneehrset)
      {
        kneehrin();
        delay(kneehroutp);
      }
    
      
      if(map(analogRead(potkhr),418,671,0,10) == kneehrset)
      {        
        kneehrlow();
        kneehr_ = 1; 
      }
  }
  }
//_______________________________________________________________________________________
   void hipvl()
  {
hipvl_ = 0;    
while(hipvl_ == 0)
    {
    hipvlinp = map(analogRead(pothvl),95,247,0,10);
    hipvlPID.Compute();
Serial.print(hipvlinp);
      if(map(analogRead(pothvl),95,247,0,10) < hipvlset)
      {
        hipvlout();
        delay(hipvloutp);
      }
    
      else if(map(analogRead(pothvl),95,247,0,10) > hipvlset)
      {
        hipvlin();
        delay(hipvloutp);
      }
    
      
      else if(map(analogRead(pothvl),95,247,0,10) == hipvlset)
      {        
        hipvllow();
        hipvl_ = 1;
      }
  }
  }
//_______________________________________________________________________________________
   void hipvr()
  {
hipvr_ = 0;    
while(hipvr_ == 0)
{
    hipvrinp = map(analogRead(pothvr),825,967,0,10);
    hipvrPID.Compute();
      if(map(analogRead(pothvr),825,967,0,10) < hipvrset)
      {
        hipvrout();
        delay(hipvroutp);
      }
    
      if(map(analogRead(pothvr),825,967,0,10) > hipvrset)
      {
        hipvrin();
        delay(hipvroutp);
      }
    
      
      if(map(analogRead(pothvr),825,967,0,10) == hipvrset)
      {        
        hipvrlow();
        hipvr_ = 1; 
      }
  }
  }
//_______________________________________________________________________________________
   void hiphl()
  {
hiphl_ = 0;    
while(hiphl_ == 0)
    {
    hiphlinp = map(analogRead(pothhl),800,932,0,10);
    hiphlPID.Compute();
      if(map(analogRead(pothhl),800,932,0,10) < hiphlset)
      {
        hiphlout();
        delay(hiphloutp);
      }
    
      if(map(analogRead(pothhl),800,932,0,10) > hiphlset)
      {
        hiphlin();
        delay(hiphloutp);
      }
    
      
      if(map(analogRead(pothhl),800,932,0,10) == hiphlset)
      {        
        hiphllow();
        hiphl_ = 1;
      }
  }
  }
//_______________________________________________________________________________________
   void hiphr()
  {
hiphr_ = 0;    
while(hiphr_ == 0)
   { 
    hiphrinp = map(analogRead(pothhr),94,204,0,10);
   hiphrPID.Compute();
      if(map(analogRead(pothhr),94,204,0,10) < hiphrset)
      {
        hiphrout();
        delay(hiphroutp);
      }
    
      if(map(analogRead(pothhr),94,204,0,10) > hiphrset)
      {
        hiphrin();
        delay(hiphroutp);
      }
    
      
      if(map(analogRead(pothhr),94,204,0,10) == hiphrset)
      {        
        hiphrlow();
        hiphr_ = 1;
      }
  }
  }
//_______________________________________________________________________________________
void HOME()
{
kneevlset = 6;
kneevrset = 7;
kneehlset = 6;
kneehrset = 6;
hipvlset = 9;
hiphlset = 9;
hipvrset = 9;
hiphrset = 9;
     
     hipvl();
     delay(500);
     
     hiphl();
     delay(500);
     
     hiphr();
     delay(500);
     
     hipvr();
     delay(500);
     kneevl();
     delay(500);
     
     //kneehl();
     //delay(500);
     
     kneehr();
     delay(500);
     
     kneevr();
     delay(500);
     
     
  int servovl = 80;
  int servovr = 80;
  int servohl = 110;
  int servohr = 30;
  Wire.beginTransmission(8);
  Wire.write(servovl);
  Wire.write(servovr);
  Wire.write(servohl);
  Wire.write(servohr);
  Wire.endTransmission();
}
 
Comments