Verbesserte 3D MC3 Master v1.1 und Ohrautomatisierung

#include <Servo.h>

Servo LeftVer;
Servo LeftAng;

Servo RightVer;
Servo RightAng;

const byte LeftVerPin = 2;
const byte LeftAngPin = 3;
const byte RightVerPin = 4;
const byte RightAngPin = 5;
const int SensorPin = 9; //
int ledPin = 13; //

const byte Button_1 = 6;
const byte Button_2 = 7;
const byte Button_3 = 8;
const byte Button_4 = 11;
const byte Button_5 = 10;

int i=0;
int pos = 0;
int SensorState = 0;
boolean BowFlag = 0;
boolean RearBowFlag = 0;

void setup()
{
pinMode(ledPin, OUTPUT); // initialize the Sensor pin as an input:
pinMode(SensorPin, INPUT); // ,

LeftVer.attach(LeftVerPin);
LeftAng.attach(LeftAngPin);

RightVer.attach(RightVerPin);
RightAng.attach(RightAngPin);

pinMode(Button_1, INPUT);
pinMode(Button_2, INPUT);
pinMode(Button_3, INPUT);
pinMode(Button_4, INPUT);
pinMode(Button_5, INPUT);

digitalWrite(Button_1, HIGH);
digitalWrite(Button_2, HIGH);
digitalWrite(Button_3, HIGH);
digitalWrite(Button_4, HIGH);
digitalWrite(Button_5, HIGH);

InitState();
delay(1000);
}

//------------------------------------------------------
void loop()
{
SensorState = digitalRead(SensorPin); // check if the Sensor is close. // if it is, the SensorState is HIGH:
if (SensorState == HIGH)
{
digitalWrite(ledPin, HIGH);
i=i++;
}

if (i==6)
{
i=1;
}

if (i=1)
{
if (!BowFlag)
{
FrontBow();
BowFlag = 1;
}

else
{
InitState();
BowFlag = 0;
delay(200);
}
}

if (i=2)
{
LeftEarBow();
InitState();
}

if (i=3)
{
RightEarBow();
InitState();
}

if (i=4)
{
TwoEarsBow();
InitState();
}

if (i=5)
{
if (!RearBowFlag)
{
RearBow();
RearBowFlag = 1;
}

else
{
InitState();
RearBowFlag = 0;
}

delay(200);
}

}
//------------------------------------------------------
void FrontBow()
{
byte Max = 90;
byte Min = 70;
unsigned int MoveDelay = 5;
unsigned int PosDelay = 500;

for(byte i=0; i <= (Max — Min) + 15; i++)
{
LeftAng.write(Min + i);
RightAng.write(Max — i);
delay(MoveDelay);
}

// delay(PosDelay);

for(byte i=0; i <= 3*(Max — Min) + 17; i++)
{
LeftAng.write(Max — i);
RightAng.write(Min + i);
delay(3*MoveDelay);
}

delay(PosDelay);
}
//------------------------------------------------------
void InitState()
{
LeftVer.write(90);
LeftAng.write(90);
RightVer.write(90);
RightAng.write(90);
}
//------------------------------------------------------
void RearBow()
{
byte Max = 90;
byte Min = 70;
unsigned int MoveDelay = 5;
unsigned int PosDelay = 500;

/*
for(byte i=0; i <= (Max — Min) + 10; i++)
{
LeftAng.write(Max — i);
RightAng.write(Min + i);
delay(MoveDelay);
}

delay(PosDelay);
*/
for(byte i=0; i <= (Max — Min) + 15; i++)
{
LeftAng.write(Min + i);
RightAng.write(Max — i);
delay(MoveDelay);
}

for(byte i=0; i <= 10; i++)
{
LeftVer.write(Max + i);
RightVer.write(Max — i);
delay(1);
}

delay(PosDelay);
}
//------------------------------------------------------
void LeftEarBow()
{
byte Max = 90;
unsigned int MoveDelay = 7;
byte AnglePos = Max;

for(byte i=0; i <= 80; i++)
{
LeftVer.write(Max + i);
delay(1);
}

for(byte i=0; i <= 75; i++)
{
LeftAng.write(AnglePos);
AnglePos = Max — i;
delay(MoveDelay);
}

for(byte i=0; i <= 10; i++)
{
LeftAng.write(AnglePos);
AnglePos += i;
delay(MoveDelay);
}

for(byte j=0; j<5; j++)
{
for(byte i=0; i <= 10; i++)
{
LeftAng.write(AnglePos);
AnglePos -= i;
delay(MoveDelay);
}

for(byte i=0; i <= 10; i++)
{
LeftAng.write(AnglePos);
AnglePos += i;
delay(MoveDelay);
}
}
}
//------------------------------------------------------
void RightEarBow()
{
byte Max = 90;
unsigned int MoveDelay = 7;
byte AnglePos = Max;

for(byte i=0; i <= 80; i++)
{
RightVer.write(Max — i);
delay(1);
}

for(byte i=0; i <= 75; i++)
{
RightAng.write(AnglePos);
AnglePos = Max + i;
delay(MoveDelay);
}

for(byte i=0; i <= 10; i++)
{
RightAng.write(AnglePos);
AnglePos -= i;
delay(MoveDelay);
}

for(byte j=0; j<5; j++)
{
for(byte i=0; i <= 10; i++)
{
RightAng.write(AnglePos);
AnglePos += i;
delay(MoveDelay);
}

for(byte i=0; i <= 10; i++)
{
RightAng.write(AnglePos);
AnglePos -= i;
delay(MoveDelay);
}
}
}
//------------------------------------------------------
void TwoEarsBow()
{
byte Max = 90;
unsigned int MoveDelay = 7;
byte AnglePosLeft = Max;
byte AnglePosRight = Max;

for(byte i=0; i <= 80; i++)
{
LeftVer.write(Max + i);
RightVer.write(Max — i);
delay(1);
}

for(byte i=0; i <= 75; i++)
{
LeftAng.write(AnglePosLeft);
RightAng.write(AnglePosRight);
AnglePosLeft = Max — i;
AnglePosRight = Max + i;
delay(MoveDelay);
}

for(byte i=0; i <= 10; i++)
{
LeftAng.write(AnglePosLeft);
RightAng.write(AnglePosRight);
AnglePosLeft += i;
AnglePosRight -= i;
delay(MoveDelay);
}

for(byte j=0; j<5; j++)
{
for(byte i=0; i <= 10; i++)
{
LeftAng.write(AnglePosLeft);
RightAng.write(AnglePosRight);
AnglePosLeft -= i;
AnglePosRight += i;
delay(MoveDelay);
}

for(byte i=0; i <= 10; i++)
{
LeftAng.write(AnglePosLeft);
RightAng.write(AnglePosRight);
AnglePosLeft += i;
AnglePosRight -= i;
delay(MoveDelay);
}
}
}