Team 7 - iClean

#include <Servo.h>
#include <FastLED.h>

#define LED_PIN     10
#define NUM_LEDS    10
#define BRIGHTNESS  64
#define LED_TYPE    WS2811
#define COLOR_ORDER GRB
CRGB leds[NUM_LEDS];

#define UPDATES_PER_SECOND 10

//Sensoren und Lichter
int lightsensor = A2;

//Servos
Servo myservo1;
Servo myservo2;
Servo myservo3;

//Motor
int motor = 2;

//LED
int led1 = 6;
int led2 = 7;
int led3 = 8;

//PIEZO
int piezosensor = 9;

//Variablen
int pos = 0;
int platziert = 0;
int blinkspeed = 200;
int counter = 0;

//MELODY
#define NOTE_B0  31
#define NOTE_C1  33
#define NOTE_CS1 35
#define NOTE_D1  37
#define NOTE_DS1 39
#define NOTE_E1  41
#define NOTE_F1  44
#define NOTE_FS1 46
#define NOTE_G1  49
#define NOTE_GS1 52
#define NOTE_A1  55
#define NOTE_AS1 58
#define NOTE_B1  62
#define NOTE_C2  65
#define NOTE_CS2 69
#define NOTE_D2  73
#define NOTE_DS2 78
#define NOTE_E2  82
#define NOTE_F2  87
#define NOTE_FS2 93
#define NOTE_G2  98
#define NOTE_GS2 104
#define NOTE_A2  110
#define NOTE_AS2 117
#define NOTE_B2  123
#define NOTE_C3  131
#define NOTE_CS3 139
#define NOTE_D3  147
#define NOTE_DS3 156
#define NOTE_E3  165
#define NOTE_F3  175
#define NOTE_FS3 185
#define NOTE_G3  196
#define NOTE_GS3 208
#define NOTE_A3  220
#define NOTE_AS3 233
#define NOTE_B3  247
#define NOTE_C4  262
#define NOTE_CS4 277
#define NOTE_D4  294
#define NOTE_DS4 311
#define NOTE_E4  330
#define NOTE_F4  349
#define NOTE_FS4 370
#define NOTE_G4  392
#define NOTE_GS4 415
#define NOTE_A4  440
#define NOTE_AS4 466
#define NOTE_B4  494
#define NOTE_C5  523
#define NOTE_CS5 554
#define NOTE_D5  587
#define NOTE_DS5 622
#define NOTE_E5  659
#define NOTE_F5  698
#define NOTE_FS5 740
#define NOTE_G5  784
#define NOTE_GS5 831
#define NOTE_A5  880
#define NOTE_AS5 932
#define NOTE_B5  988
#define NOTE_C6  1047
#define NOTE_CS6 1109
#define NOTE_D6  1175
#define NOTE_DS6 1245
#define NOTE_E6  1319
#define NOTE_F6  1397
#define NOTE_FS6 1480
#define NOTE_G6  1568
#define NOTE_GS6 1661
#define NOTE_A6  1760
#define NOTE_AS6 1865
#define NOTE_B6  1976
#define NOTE_C7  2093
#define NOTE_CS7 2217
#define NOTE_D7  2349
#define NOTE_DS7 2489
#define NOTE_E7  2637
#define NOTE_F7  2794
#define NOTE_FS7 2960
#define NOTE_G7  3136
#define NOTE_GS7 3322
#define NOTE_A7  3520
#define NOTE_AS7 3729
#define NOTE_B7  3951
#define NOTE_C8  4186
#define NOTE_CS8 4435
#define NOTE_D8  4699
#define NOTE_DS8 4978

#define melodyPin 3
//Mario main theme melody
int melody[] = {
  NOTE_E7, NOTE_E7, 0, NOTE_E7,
  0, NOTE_C7, NOTE_E7, 0,
  NOTE_G7, 0, 0,  0,
  NOTE_G6, 0, 0, 0,

  NOTE_C7, 0, 0, NOTE_G6,
  0, 0, NOTE_E6, 0,
  0, NOTE_A6, 0, NOTE_B6,
  0, NOTE_AS6, NOTE_A6, 0,

  NOTE_G6, NOTE_E7, NOTE_G7,
  NOTE_A7, 0, NOTE_F7, NOTE_G7,
  0, NOTE_E7, 0, NOTE_C7,
  NOTE_D7, NOTE_B6, 0, 0,

  NOTE_C7, 0, 0, NOTE_G6,
  0, 0, NOTE_E6, 0,
  0, NOTE_A6, 0, NOTE_B6,
  0, NOTE_AS6, NOTE_A6, 0,

  NOTE_G6, NOTE_E7, NOTE_G7,
  NOTE_A7, 0, NOTE_F7, NOTE_G7,
  0, NOTE_E7, 0, NOTE_C7,
  NOTE_D7, NOTE_B6, 0, 0
};
//Mario main them tempo
int tempo[] = {
  12, 12, 12, 12,
  12, 12, 12, 12,
  12, 12, 12, 12,
  12, 12, 12, 12,

  12, 12, 12, 12,
  12, 12, 12, 12,
  12, 12, 12, 12,
  12, 12, 12, 12,

  9, 9, 9,
  12, 12, 12, 12,
  12, 12, 12, 12,
  12, 12, 12, 12,

  12, 12, 12, 12,
  12, 12, 12, 12,
  12, 12, 12, 12,
  12, 12, 12, 12,

  9, 9, 9,
  12, 12, 12, 12,
  12, 12, 12, 12,
  12, 12, 12, 12,
};
//Underworld melody
int underworld_melody[] = {
  NOTE_C4, NOTE_C5, NOTE_A3, NOTE_A4,
  NOTE_AS3, NOTE_AS4, 0,
  0,
  NOTE_C4, NOTE_C5, NOTE_A3, NOTE_A4,
  NOTE_AS3, NOTE_AS4, 0,
  0,
  NOTE_F3, NOTE_F4, NOTE_D3, NOTE_D4,
  NOTE_DS3, NOTE_DS4, 0,
  0,
  NOTE_F3, NOTE_F4, NOTE_D3, NOTE_D4,
  NOTE_DS3, NOTE_DS4, 0,
  0, NOTE_DS4, NOTE_CS4, NOTE_D4,
  NOTE_CS4, NOTE_DS4,
  NOTE_DS4, NOTE_GS3,
  NOTE_G3, NOTE_CS4,
  NOTE_C4, NOTE_FS4, NOTE_F4, NOTE_E3, NOTE_AS4, NOTE_A4,
  NOTE_GS4, NOTE_DS4, NOTE_B3,
  NOTE_AS3, NOTE_A3, NOTE_GS3,
  0, 0, 0
};
//Underwolrd tempo
int underworld_tempo[] = {
  12, 12, 12, 12,
  12, 12, 6,
  3,
  12, 12, 12, 12,
  12, 12, 6,
  3,
  12, 12, 12, 12,
  12, 12, 6,
  3,
  12, 12, 12, 12,
  12, 12, 6,
  6, 18, 18, 18,
  6, 6,
  6, 6,
  6, 6,
  18, 18, 18, 18, 18, 18,
  10, 10, 10,
  10, 10, 10,
  3, 3, 3
};

//Timer
unsigned long previousMillis = 0;
unsigned long startMillis = 0;
unsigned long timerMillis1 = 0;
unsigned long end1 = 0;
const long interval = 1000;
long delta;


CRGBPalette16 currentPalette;
TBlendType    currentBlending;

extern CRGBPalette16 myRedWhiteBluePalette;
extern const TProgmemPalette16 myRedWhiteBluePalette_p PROGMEM;


void setup() {
  // put your setup code here, to run once:
pinMode(lightsensor, INPUT);
//2
pinMode(motor, OUTPUT);
//Servos definieren 3-5
myservo1.attach(3);
myservo2.attach(4);
myservo3.attach(5);
//6-8
pinMode(led1, OUTPUT);
pinMode(led2, OUTPUT);
pinMode(led3, OUTPUT);
//9
pinMode(piezosensor, OUTPUT);

Lights(1);

delay( 3000 ); // power-up safety delay
FastLED.addLeds<LED_TYPE, LED_PIN, COLOR_ORDER>(leds, NUM_LEDS).setCorrection( TypicalLEDStrip );
FastLED.setBrightness(  BRIGHTNESS );

 currentPalette = RainbowColors_p;
 currentBlending = LINEARBLEND;

 //ServoReset

  myservo1.write(pos);
  myservo2.write(pos);
  myservo3.write(pos);

//Debug
Serial.begin(115200);
}

void loop() {

ChangePalettePeriodically();

 static uint8_t startIndex = 0;
 startIndex = startIndex + 1; /* motion speed */

 FillLEDsFromPaletteColors( startIndex);

 FastLED.show();
 FastLED.delay(20 / UPDATES_PER_SECOND);

  // Start time
  previousMillis = millis();

  myservo1.write(pos);
  myservo2.write(pos);
  myservo3.write(pos);
  Serial.println(myservo2.read());

  //LICHTSENSORWERT
 int lightvalue = analogRead(lightsensor);
 Serial.println(lightvalue);

  //LICHTSENSOR REGISTRIERT HAND
if(lightvalue<10){
   Lights(0);



 //LED LEUCHTEN//
 //warte nach dem ersten platzieren
  if(platziert==0){
      LightsBlink(led1);
    platziert = 1;
    }

Piezo1();



//ERSTER WASCHGANG
for(int i = 0; i<10; i++){
  digitalWrite(led1,1);
  int randomNum = random(45,150);
  ServoMove1(true, randomNum);
  delay(500);
 // Serial.println(i);
 // Serial.println("ERSTER WASCHGANG");
}
 myservo1.write(0);
 digitalWrite(led1,0);
  delay(2000);
  tone(piezosensor, 500);
 delay(500);
 noTone(piezosensor);


//ZWEITER WASCHGANG
for(int i = 0;i<3;i++){
  digitalWrite(led2,1);
  for (int j = 60; j <= 150; j += 1) { // goes from 0 degrees to 180 degrees
    // in steps of 1 degree
    myservo2.write(j);              // tell servo to go to position in variable 'pos'
    delay(10);                       // waits 15ms for the servo to reach the position
  }
  for (int j = 150; j >= 60; j -= 1) { // goes from 180 degrees to 0 degrees
    myservo2.write(j);              // tell servo to go to position in variable 'pos'
    delay(10);                       // waits 15ms for the servo to reach the position
  }
}
 myservo2.write(0);
 digitalWrite(led2,0);
 delay(2000);
 tone(piezosensor, 500);
 delay(500);
 noTone(piezosensor);


 //DRITTER WASCHGANG
 for(int i = 0;i<2;i++){
 digitalWrite(led3,1);
 digitalWrite(motor,HIGH);
  delay(2000);
  digitalWrite(motor,LOW);
  delay(500);
}

//VIERTER WASCHGANG
delay(1000);
myservo3.write(180);
delay(500);
myservo3.write(0);
delay(1000);

digitalWrite(led3,0);
delay(2000);
tone(piezosensor, 500);
 delay(500);
 noTone(piezosensor);

 Piezo2();


}else {

  //RESET
  delay(1000);
  myservo1.write(0);
  myservo2.write(0);
  Serial.println(myservo1.read());
  Lights(1);
  platziert = 0;
  counter = 0;
}

}

void ServoMove1(bool a, int x){
  if(a == true){
     myservo1.write(x);
  }
}

void ServoMove2(bool a){
  if(a == true){
    if(myservo2.read()==180){
    myservo2.write(45);
    }
    if(myservo2.read()==45){
      myservo2.write(180);
    }
  }

 }

 void LightsBlink(int x){
    for(int i = 0; i<4; i++){
    delay(blinkspeed);
    Lights(1);
    delay(blinkspeed);
    Lights(0);
    }
 }

 void Lights(int x){
   for(int i = 5; i<9;i++){
    digitalWrite(i,x);
    }
 }

 void Piezo1(){
  tone(piezosensor,660,100);
delay(150);
tone(piezosensor,660,100);
delay(300);
tone(piezosensor,660,100);
delay(300);
tone(piezosensor,510,100);
delay(100);
tone(piezosensor,660,100);
delay(300);
tone(piezosensor,770,100);
delay(550);
tone(piezosensor,380,100);
delay(575);
 }

 void Piezo2(){


tone(piezosensor,510,100);
delay(450);
tone(piezosensor,380,100);
delay(400);
tone(piezosensor,320,100);
delay(500);
tone(piezosensor,440,100);
delay(300);
tone(piezosensor,480,80);
delay(330);
tone(piezosensor,450,100);
delay(150);
tone(piezosensor,430,100);
delay(300);
tone(piezosensor,380,100);
delay(200);
tone(piezosensor,660,80);
delay(200);
tone(piezosensor,760,50);
delay(150);
tone(piezosensor,860,100);
delay(300);
tone(piezosensor,700,80);
delay(150);
tone(piezosensor,760,50);
delay(350);
tone(piezosensor,660,80);
delay(300);
tone(piezosensor,520,80);
delay(150);
tone(piezosensor,580,80);
delay(150);
tone(piezosensor,480,80);
delay(500);



tone(piezosensor,510,100);
delay(450);
tone(piezosensor,380,100);
delay(400);
tone(piezosensor,320,100);
delay(500);
tone(piezosensor,440,100);
delay(300);
tone(piezosensor,480,80);
delay(330);
tone(piezosensor,450,100);
delay(150);
tone(piezosensor,430,100);
delay(300);
tone(piezosensor,380,100);
delay(200);
tone(piezosensor,660,80);
delay(200);
tone(piezosensor,760,50);
delay(150);
tone(piezosensor,860,100);
delay(300);
tone(piezosensor,700,80);
delay(150);
tone(piezosensor,760,50);
delay(350);
tone(piezosensor,660,80);
delay(300);
tone(piezosensor,520,80);
delay(150);
tone(piezosensor,580,80);
delay(150);
tone(piezosensor,480,80);
delay(500);

/*
tone(piezosensor,500,100);
delay(300);

tone(piezosensor,760,100);
delay(100);
tone(piezosensor,720,100);
delay(150);
tone(piezosensor,680,100);
delay(150);
tone(piezosensor,620,150);
delay(300);

tone(piezosensor,650,150);
delay(300);
tone(piezosensor,380,100);
delay(150);
tone(piezosensor,430,100);
delay(150);

tone(piezosensor,500,100);
delay(300);
tone(piezosensor,430,100);
delay(150);
tone(piezosensor,500,100);
delay(100);
tone(piezosensor,570,100);
delay(220);

tone(piezosensor,500,100);
delay(300);

tone(piezosensor,760,100);
delay(100);
tone(piezosensor,720,100);
delay(150);
tone(piezosensor,680,100);
delay(150);
tone(piezosensor,620,150);
delay(300);

tone(piezosensor,650,200);
delay(300);

tone(piezosensor,1020,80);
delay(300);
tone(piezosensor,1020,80);
delay(150);
tone(piezosensor,1020,80);
delay(300);

tone(piezosensor,380,100);
delay(300);
tone(piezosensor,500,100);
delay(300);

tone(piezosensor,760,100);
delay(100);
tone(piezosensor,720,100);
delay(150);
tone(piezosensor,680,100);
delay(150);
tone(piezosensor,620,150);
delay(300);

tone(piezosensor,650,150);
delay(300);
tone(piezosensor,380,100);
delay(150);
tone(piezosensor,430,100);
delay(150);

tone(piezosensor,500,100);
delay(300);
tone(piezosensor,430,100);
delay(150);
tone(piezosensor,500,100);
delay(100);
tone(piezosensor,570,100);
delay(420);

tone(piezosensor,585,100);
delay(450);

tone(piezosensor,550,100);
delay(420);

tone(piezosensor,500,100);
delay(360);

tone(piezosensor,380,100);
delay(300);
tone(piezosensor,500,100);
delay(300);
tone(piezosensor,500,100);
delay(150);
tone(piezosensor,500,100);
delay(300);

tone(piezosensor,500,100);
delay(300);

tone(piezosensor,760,100);
delay(100);
tone(piezosensor,720,100);
delay(150);
tone(piezosensor,680,100);
delay(150);
tone(piezosensor,620,150);
delay(300);

tone(piezosensor,650,150);
delay(300);
tone(piezosensor,380,100);
delay(150);
tone(piezosensor,430,100);
delay(150);

tone(piezosensor,500,100);
delay(300);
tone(piezosensor,430,100);
delay(150);
tone(piezosensor,500,100);
delay(100);
tone(piezosensor,570,100);
delay(220);

tone(piezosensor,500,100);
delay(300);

tone(piezosensor,760,100);
delay(100);
tone(piezosensor,720,100);
delay(150);
tone(piezosensor,680,100);
delay(150);
tone(piezosensor,620,150);
delay(300);

tone(piezosensor,650,200);
delay(300);

tone(piezosensor,1020,80);
delay(300);
tone(piezosensor,1020,80);
delay(150);
tone(piezosensor,1020,80);
delay(300);

tone(piezosensor,380,100);
delay(300);
tone(piezosensor,500,100);
delay(300);

tone(piezosensor,760,100);
delay(100);
tone(piezosensor,720,100);
delay(150);
tone(piezosensor,680,100);
delay(150);
tone(piezosensor,620,150);
delay(300);

tone(piezosensor,650,150);
delay(300);
tone(piezosensor,380,100);
delay(150);
tone(piezosensor,430,100);
delay(150);

tone(piezosensor,500,100);
delay(300);
tone(piezosensor,430,100);
delay(150);
tone(piezosensor,500,100);
delay(100);
tone(piezosensor,570,100);
delay(420);

tone(piezosensor,585,100);
delay(450);

tone(piezosensor,550,100);
delay(420);

tone(piezosensor,500,100);
delay(360);

tone(piezosensor,380,100);
delay(300);
tone(piezosensor,500,100);
delay(300);
tone(piezosensor,500,100);
delay(150);
tone(piezosensor,500,100);
delay(300);

tone(piezosensor,500,60);
delay(150);
tone(piezosensor,500,80);
delay(300);
tone(piezosensor,500,60);
delay(350);
tone(piezosensor,500,80);
delay(150);
tone(piezosensor,580,80);
delay(350);
tone(piezosensor,660,80);
delay(150);
tone(piezosensor,500,80);
delay(300);
tone(piezosensor,430,80);
delay(150);
tone(piezosensor,380,80);
delay(600);

tone(piezosensor,500,60);
delay(150);
tone(piezosensor,500,80);
delay(300);
tone(piezosensor,500,60);
delay(350);
tone(piezosensor,500,80);
delay(150);
tone(piezosensor,580,80);
delay(150);
tone(piezosensor,660,80);
delay(550);

tone(piezosensor,870,80);
delay(325);
tone(piezosensor,760,80);
delay(600);

tone(piezosensor,500,60);
delay(150);
tone(piezosensor,500,80);
delay(300);
tone(piezosensor,500,60);
delay(350);
tone(piezosensor,500,80);
delay(150);
tone(piezosensor,580,80);
delay(350);
tone(piezosensor,660,80);
delay(150);
tone(piezosensor,500,80);
delay(300);
tone(piezosensor,430,80);
delay(150);
tone(piezosensor,380,80);
delay(600);

tone(piezosensor,660,100);
delay(150);
tone(piezosensor,660,100);
delay(300);
tone(piezosensor,660,100);
delay(300);
tone(piezosensor,510,100);
delay(100);
tone(piezosensor,660,100);
delay(300);
tone(piezosensor,770,100);
delay(550);
tone(piezosensor,380,100);
delay(575);
*/


  /*tone(piezosensor, 500);
 delay(200);
 noTone(piezosensor);
 delay(50);
 tone(piezosensor, 500);
 delay(100);
 noTone(piezosensor);
 delay(10);
 tone(piezosensor, 500);
 delay(100);
 noTone(piezosensor);
 delay(10);
  tone(piezosensor, 1000);
 delay(200);
 noTone(piezosensor);*/
 }


void FillLEDsFromPaletteColors( uint8_t colorIndex)
{
    uint8_t brightness = 255;

    for( int i = 0; i < NUM_LEDS; i++) {
        leds[i] = ColorFromPalette( currentPalette, colorIndex, brightness, currentBlending);
        colorIndex += 3;
    }
}


// There are several different palettes of colors demonstrated here.
//
// FastLED provides several 'preset' palettes: RainbowColors_p, RainbowStripeColors_p,
// OceanColors_p, CloudColors_p, LavaColors_p, ForestColors_p, and PartyColors_p.
//
// Additionally, you can manually define your own color palettes, or you can write
// code that creates color palettes on the fly.  All are shown here.

void ChangePalettePeriodically()
{
    uint8_t secondHand = (millis() / 1000) % 60;
    static uint8_t lastSecond = 99;

    if( lastSecond != secondHand) {
        lastSecond = secondHand;
        if( secondHand ==  0)  { currentPalette = RainbowColors_p;         currentBlending = LINEARBLEND; }
        if( secondHand == 10)  { currentPalette = RainbowStripeColors_p;   currentBlending = NOBLEND;  }
        if( secondHand == 15)  { currentPalette = RainbowStripeColors_p;   currentBlending = LINEARBLEND; }
        if( secondHand == 20)  { SetupPurpleAndGreenPalette();             currentBlending = LINEARBLEND; }
        if( secondHand == 25)  { SetupTotallyRandomPalette();              currentBlending = LINEARBLEND; }
        if( secondHand == 30)  { SetupBlackAndWhiteStripedPalette();       currentBlending = NOBLEND; }
        if( secondHand == 35)  { SetupBlackAndWhiteStripedPalette();       currentBlending = LINEARBLEND; }
        if( secondHand == 40)  { currentPalette = CloudColors_p;           currentBlending = LINEARBLEND; }
        if( secondHand == 45)  { currentPalette = PartyColors_p;           currentBlending = LINEARBLEND; }
        if( secondHand == 50)  { currentPalette = myRedWhiteBluePalette_p; currentBlending = NOBLEND;  }
        if( secondHand == 55)  { currentPalette = myRedWhiteBluePalette_p; currentBlending = LINEARBLEND; }
    }
}

// This function fills the palette with totally random colors.
void SetupTotallyRandomPalette()
{
    for( int i = 0; i < 16; i++) {
        currentPalette[i] = CHSV( random8(), 255, random8());
    }
}

// This function sets up a palette of black and white stripes,
// using code.  Since the palette is effectively an array of
// sixteen CRGB colors, the various fill_* functions can be used
// to set them up.
void SetupBlackAndWhiteStripedPalette()
{
    // 'black out' all 16 palette entries...
    fill_solid( currentPalette, 16, CRGB::Black);
    // and set every fourth one to white.
    currentPalette[0] = CRGB::White;
    currentPalette[4] = CRGB::White;
    currentPalette[8] = CRGB::White;
    currentPalette[12] = CRGB::White;

}

// This function sets up a palette of purple and green stripes.
void SetupPurpleAndGreenPalette()
{
    CRGB purple = CHSV( HUE_PURPLE, 255, 255);
    CRGB green  = CHSV( HUE_GREEN, 255, 255);
    CRGB black  = CRGB::Black;

    currentPalette = CRGBPalette16(
                                   green,  green,  black,  black,
                                   purple, purple, black,  black,
                                   green,  green,  black,  black,
                                   purple, purple, black,  black );
}


// This example shows how to set up a static color palette
// which is stored in PROGMEM (flash), which is almost always more
// plentiful than RAM.  A static PROGMEM palette like this
// takes up 64 bytes of flash.
const TProgmemPalette16 myRedWhiteBluePalette_p PROGMEM =
{
    CRGB::Red,
    CRGB::Gray, // 'white' is too bright compared to red and blue
    CRGB::Blue,
    CRGB::Black,

    CRGB::Red,
    CRGB::Gray,
    CRGB::Blue,
    CRGB::Black,

    CRGB::Red,
    CRGB::Red,
    CRGB::Gray,
    CRGB::Gray,
    CRGB::Blue,
    CRGB::Blue,
    CRGB::Black,
    CRGB::Black
};