smart steps (beta, test , kod z dupy, pisane bez kawy)

#include <SoftPWM.h>
#define trigPin 13
#define echoPin 12
#define led 11
#define led2 10
long distance1 = 0;
long calosc = 0;
int av = 0;
int n = 0;
int initoff = 1000;
int jasnosc = 100;
int nsteptime = 100;
void setup()
{
  Serial.begin (9600);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(led, OUTPUT);
  pinMode(led2, OUTPUT);
inicjalizacja();
}

void loop()
{
//  distance1 = 0;
//  distance1 = sensor1();
//  switch (distance1) {
//  case 150:
//    schodek1();
//    break;
//  case 100:
//    schodek1();
//    break;
//  default:
//   schodekoff();
//  break;
//}
//if(av < 100){
//calosc = calosc + distance1;

//av++;
//}else{
//calosc = calosc / 100;
//    Serial.print(distance1);
//    //Serial.print(av);
//    Serial.println(" cm");
//av = 0;
//}
test_walk();
//if (distance1 < 70)
//{
//schodekall();
//Serial.println(" zapalam");
//delay(20000);
//schodekoff();
//Serial.println(" gasze");}
}


  int sensor1(){
  long duration, distance;
  digitalWrite(trigPin, LOW);  // Added this line
 // delayMicroseconds(2); // Added this line
  digitalWrite(trigPin, HIGH);

  delayMicroseconds(10); // Added this line
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  distance = (duration/2) / 29.1;
//delay();
return distance;

}

void test_walk(){
// test walk
  delay(nsteptime);
  schodek1();
  delay(nsteptime);
  schodek2();
  delay(nsteptime);
  schodek3();
  delay(nsteptime);
  schodek4();
  delay(nsteptime);
  schodek5();
  delay(nsteptime);
  schodek6();
  delay(nsteptime);
  schodek7();
  delay(nsteptime);
  schodek8();
  delay(nsteptime);
  schodekoff();
  delay(nsteptime);
  schodekoff();
  delay(nsteptime);
  schodek8();
  delay(nsteptime);
  schodek7();
  delay(nsteptime);
  schodek6();
  delay(nsteptime);
  schodek5();
  delay(nsteptime);
  schodek4();
  delay(nsteptime);
  schodek3();
  delay(nsteptime);
  schodek2();
  delay(nsteptime);
  schodek1();
  delay(nsteptime);
  schodekoff();
}

void inicjalizacja(){
  delay(10000);
  // Initialize
  SoftPWMBegin();
  // Create and set pin 13 to 0 (off)
 // SoftPWMSet(13, 0);
  SoftPWMSet(2, 0);
  SoftPWMSet(3, 0);
  SoftPWMSet(4, 0);
  SoftPWMSet(5, 0);
  SoftPWMSet(6, 0);
  SoftPWMSet(7, 0);
  SoftPWMSet(8, 0);
  SoftPWMSet(9, 0);

  // Set fade time for pin 13 to 100 ms fade-up time, and 500 ms fade-down time
  SoftPWMSetFadeTime(13, 500, 1000);
  SoftPWMSetFadeTime(2, 500, 1000);
  SoftPWMSetFadeTime(3, 500, 1000);
  SoftPWMSetFadeTime(4, 500, 1000);
  SoftPWMSetFadeTime(5, 500, 1000);
  SoftPWMSetFadeTime(6, 500, 1000);
  SoftPWMSetFadeTime(7, 500, 1000);
  SoftPWMSetFadeTime(8, 500, 1000);
  SoftPWMSetFadeTime(9, 500, 1000);
  //init
  // Turn on - set to 100%
  SoftPWMSetPercent(2, 100);
  SoftPWMSetPercent(3, 100);
  SoftPWMSetPercent(4, 100);
  SoftPWMSetPercent(5, 100);
  SoftPWMSetPercent(6, 100);
  SoftPWMSetPercent(7, 100);
  SoftPWMSetPercent(8, 100);
  SoftPWMSetPercent(9, 100);
  delay(initoff);
  SoftPWMSetPercent(9, 0);
  delay(initoff);
  SoftPWMSetPercent(8, 0);
  delay(initoff);
  SoftPWMSetPercent(7, 0);
  delay(initoff);
  SoftPWMSetPercent(6, 0);
  delay(initoff);
  SoftPWMSetPercent(5, 0);
  delay(initoff);
  SoftPWMSetPercent(4, 0);
  delay(initoff);
  SoftPWMSetPercent(3, 0);
  delay(initoff);
  SoftPWMSetPercent(2, 0);
}



void schodekall(){
  SoftPWMSetPercent(9, jasnosc);
  SoftPWMSetPercent(8, jasnosc);
  SoftPWMSetPercent(7, jasnosc);
  SoftPWMSetPercent(6, jasnosc);
  SoftPWMSetPercent(5, jasnosc);
  SoftPWMSetPercent(4, jasnosc);
  SoftPWMSetPercent(3, jasnosc);
  SoftPWMSetPercent(2, jasnosc);
}

void schodek1(){
  SoftPWMSetPercent(9, 0);
  SoftPWMSetPercent(8, 0);
  SoftPWMSetPercent(7, 0);
  SoftPWMSetPercent(6, 0);
  SoftPWMSetPercent(5, 0);
  SoftPWMSetPercent(4, jasnosc);
  SoftPWMSetPercent(3, jasnosc);
  SoftPWMSetPercent(2, jasnosc);
}

void schodek0(){
  SoftPWMSetPercent(9, jasnosc);
  SoftPWMSetPercent(8, 0);
  SoftPWMSetPercent(7, 0);
  SoftPWMSetPercent(6, 0);
  SoftPWMSetPercent(5, 0);
  SoftPWMSetPercent(4, 0);
  SoftPWMSetPercent(3, 0);
  SoftPWMSetPercent(2, 0);
}
void schodek2(){
  SoftPWMSetPercent(9, 0);
  SoftPWMSetPercent(8, 0);
  SoftPWMSetPercent(7, 0);
  SoftPWMSetPercent(6, 0);
  SoftPWMSetPercent(5, jasnosc);
  SoftPWMSetPercent(4, jasnosc);
  SoftPWMSetPercent(3, jasnosc);
  SoftPWMSetPercent(2, jasnosc);
}
void schodek3(){
  SoftPWMSetPercent(9, 0);
  SoftPWMSetPercent(8, 0);
  SoftPWMSetPercent(7, 0);
  SoftPWMSetPercent(6, jasnosc);
  SoftPWMSetPercent(5, jasnosc);
  SoftPWMSetPercent(4, jasnosc);
  SoftPWMSetPercent(3, jasnosc);
  SoftPWMSetPercent(2, 0);
}
void schodek4(){
  SoftPWMSetPercent(9, 0);
  SoftPWMSetPercent(8, 0);
  SoftPWMSetPercent(7, jasnosc);
  SoftPWMSetPercent(6, jasnosc);
  SoftPWMSetPercent(5, jasnosc);
  SoftPWMSetPercent(4, jasnosc);
  SoftPWMSetPercent(3, 0);
  SoftPWMSetPercent(2, 0);
}
void schodek5(){
  SoftPWMSetPercent(9, 0);
  SoftPWMSetPercent(8, jasnosc);
  SoftPWMSetPercent(7, jasnosc);
  SoftPWMSetPercent(6, jasnosc);
  SoftPWMSetPercent(5, jasnosc);
  SoftPWMSetPercent(4, 0);
  SoftPWMSetPercent(3, 0);
  SoftPWMSetPercent(2, 0);
}
void schodek6(){
  SoftPWMSetPercent(9, jasnosc);
  SoftPWMSetPercent(8, jasnosc);
  SoftPWMSetPercent(7, jasnosc);
  SoftPWMSetPercent(6, jasnosc);
  SoftPWMSetPercent(5, 0);
  SoftPWMSetPercent(4, 0);
  SoftPWMSetPercent(3, 0);
  SoftPWMSetPercent(2, 0);
}
void schodek7(){
  SoftPWMSetPercent(9, jasnosc);
  SoftPWMSetPercent(8, jasnosc);
  SoftPWMSetPercent(7, jasnosc);
  SoftPWMSetPercent(6, 0);
  SoftPWMSetPercent(5, 0);
  SoftPWMSetPercent(4, 0);
  SoftPWMSetPercent(3, 0);
  SoftPWMSetPercent(2, 0);
}
void schodek8(){
  SoftPWMSetPercent(9, jasnosc);
  SoftPWMSetPercent(8, jasnosc);
  SoftPWMSetPercent(7, 0);
  SoftPWMSetPercent(6, 0);
  SoftPWMSetPercent(5, 0);
  SoftPWMSetPercent(4, 0);
  SoftPWMSetPercent(3, 0);
  SoftPWMSetPercent(2, 0);
}

void schodekoff(){
  SoftPWMSetPercent(9, 0);
  SoftPWMSetPercent(8, 0);
  SoftPWMSetPercent(7, 0);
  SoftPWMSetPercent(6, 0);
  SoftPWMSetPercent(5, 0);
  SoftPWMSetPercent(4, 0);
  SoftPWMSetPercent(3, 0);
  SoftPWMSetPercent(2, 0);
}