# 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
Please log in or sign up to comment.