HDD Disk + Arduino – не очень то работает.

int phase1 = 9;
int phase2 = 6;
int phase3 = 5;

int A = 9;
int B = 6;
int C = 5;

int led = 13;

unsigned long stepLength = 40000;
int minStepLength = 7000; //1400
int steps = 50;

void setup() {
  pinMode(led, OUTPUT);  
  pinMode(phase1, OUTPUT);
  pinMode(phase2, OUTPUT);
  pinMode(phase3, OUTPUT);
  analogWrite(led, LOW);
}

void loop() {
  switchStep(1);
  switchStep(2);
  switchStep(3);
 
  if(stepLength > minStepLength)
  {
    stepLength = stepLength - steps;
  } else {
    // set the minimum pulse length
    stepLength=minStepLength;
  }
 
  if (stepLength < 39950) {
    analogWrite(led, HIGH);   // second gear
    steps = 300;
  }
   
  if (stepLength < 20000) {
    analogWrite(led, LOW);      // third gear
    steps = 50;
  }

  if (stepLength < 3000) {
    analogWrite(led, HIGH);      // fourth gear
    steps = 2;
  }
}


// CB, AB, AC, BC, BA, CA

void switchStep(int stage)
{
  int PWM = 64;
 
  switch(stage)
  {
  case 1:
    analogWrite(phase1, PWM); // 100 ok
    analogWrite(phase2, 0);
    analogWrite(phase3, 0);
    myDelay(stepLength);
    break;
   
  case 2:
    analogWrite(phase1, 0);
    analogWrite(phase2, PWM);
    analogWrite(phase3, 0);
    myDelay(stepLength);
    break;
   
  default:
    analogWrite(phase1, 0);
    analogWrite(phase2, 0);
    analogWrite(phase3, PWM);
    myDelay(stepLength);
    break;
  }  


}

void myDelay(unsigned long p) {
if (p > 16380) {
  delay (p/1000);
  } else {
  delayMicroseconds(p);
  }
}