Hjälp med Arduino!

/********************************************************
 * Optical Encoder + Stepper Motor = Smart Follow Focus *
 *       Realtime, Lens Calibration, Variable Speed     *
 *                    Adi Soffer 2.9.12                 *
 *******************************************************/

#include <AccelStepper.h>
#include <OneButton.h>


//encoder/motor/driver setup
int easyDriverMicroSteps = 1; 
int rotaryEncoderSteps = 75; //setps per rev on your encoder
int motorStepsPerRev = 200; //steps per rev on the motor
int MinPulseWidth = 50; //too low and the motor will stall, too high and it will slow it down

//Encoder pins (cannot be changed 2/3 are special pins)
int encoderPin1 = 2;
int encoderPin2 = 3;

//Easy Driver pins
int easyDriverStepPin = 4;
int easyDriverDirPin = 5;

//Encoder Values
volatile int lastEncoded = 0;
volatile int encoderValue = 0;
int lastencoderValue = 0;
int lastMSB = 0;
int lastLSB = 0;

//1 motor driver with the library
AccelStepper stepper(1, easyDriverStepPin, easyDriverDirPin);

//Enable pin:high will put chip in disabled mode
#define enablePin 8

//LEDs
#define realTimeLED 12      //Real Time LED
#define playLED 9          //PLay LED
#define inLED 11            //In LED
#define outLED 10           //Out LED

// Setup OneButton 
OneButton realTimebutton(A4, true);
OneButton playButton (A1, true);
OneButton inButton (A3, true);
OneButton outButton (A2, true);

//Blink without delay 
int ledState = LOW;
long previousMillis = 0;
int ledInterval = 75;

//Modes
int mode;
#define RealTime 1
#define STOP 2
#define REWIND 3
#define PLAY 4
#define LENSCALIB 5

//Values for focus points
int inPoint;
int outPoint;

//Values for calibrating lens and motor
int lowEnd = -1500;
int highEnd = 1500;

//Speed Val
int remappedVal;
int encoderIn;
int encoderOut;

boolean rClickedOnce = false;
boolean rLongPress = false;

void setup(){
  Serial.begin(38400);
  Serial.flush();

  stepper.setMinPulseWidth(MinPulseWidth); //may need to play with this, but I think this is good.
  stepper.setMaxSpeed(1000);
  stepper.setAcceleration(1000000000); //try 100, or 1000 and see if it works
  stepper.setSpeed(200); //play with this to see if it makes a diff (1 to 1000)

  //cut current to stepper until in action
  pinMode(enablePin, OUTPUT);
  digitalWrite(enablePin,HIGH);

  //Attach Click to Buttons
  realTimebutton.attachClick(Click);
  playButton.attachClick(ClickPlay);
  inButton.attachClick(ClickIn);
  outButton.attachClick(ClickOut);

  //Attach DoubleClick to Real Time for calibrating lens and motor
  realTimebutton.attachPress(rPress);

  //Pinmodes
  pinMode (realTimeLED, OUTPUT);
  pinMode (playLED, OUTPUT);
  pinMode (inLED, OUTPUT);
  pinMode (outLED, OUTPUT);
  pinMode(encoderPin1, INPUT);
  pinMode(encoderPin2, INPUT);

  digitalWrite(encoderPin1, HIGH); //turn pullup resistor on
  digitalWrite(encoderPin2, HIGH); //turn pullup resistor on

  //call updateEncoder() when any high/low changed seen
  //on interrupt 0 (pin 2), or interrupt 1 (pin 3) 
  attachInterrupt(0, updateEncoder, CHANGE);
  attachInterrupt(1, updateEncoder, CHANGE);
}

void loop(){

  // keep watching the push buttons:
  realTimebutton.tick();
  playButton.tick();
  inButton.tick();
  outButton.tick();

  // Defining Cases
  switch (mode)
  {

  case RealTime:
    { 
      digitalWrite(realTimeLED, HIGH);
      digitalWrite(enablePin, LOW);
      stepperMove();
    } 
    break;

  case STOP:
    digitalWrite(realTimeLED, LOW);
    digitalWrite(playLED, LOW);
    rClickedOnce = false;
    rLongPress = false;
    digitalWrite(enablePin, HIGH);      //cut current to stepper
   
    break;

  case REWIND:
    {
      //terminate realtime in case it wasn't stopped
      rClickedOnce = false;  
      digitalWrite(realTimeLED, LOW);
      blinkFunction (playLED);
      digitalWrite(enablePin, LOW);      //resume current to stepper

      //Variable Speed control
      remappedVal = focusSpeed(encoderValue);
      stepper.setMaxSpeed(remappedVal);

      stepper.moveTo(inPoint);
      stepper.run();

      if (stepper.currentPosition() == inPoint)
        mode = STOP;
    }  
    break;

  case PLAY:
    //terminate realtime in case it wasn't stopped
    rClickedOnce = false;  
    digitalWrite(realTimeLED, LOW);
    digitalWrite (playLED, HIGH);
    digitalWrite(enablePin, LOW);      //resume current to stepper

    //Variable Speed control
    remappedVal = focusSpeed(encoderValue);
    stepper.setMaxSpeed(remappedVal);

    stepper.moveTo(outPoint);
    stepper.run();
    if (stepper.currentPosition() == outPoint)
      mode = STOP;

    break;

  case LENSCALIB:                  //Calibrating Lens and Stepper
    digitalWrite(enablePin, LOW);  //resume current to stepper    
    stepperMove();
    blinkFunction(realTimeLED);
    break;
  }

}

//Functions

void updateEncoder(){
  int MSB = digitalRead(encoderPin1); //MSB = most significant bit
  int LSB = digitalRead(encoderPin2); //LSB = least significant bit

  int encoded = (MSB << 1) |LSB; //converting the 2 pin value to single number
  int sum  = (lastEncoded << 2) | encoded; //adding it to the previous encoded value

  if(sum == 0b1101 || sum == 0b0100 || sum == 0b0010 || sum == 0b1011) encoderValue ++;
  if(sum == 0b1110 || sum == 0b0111 || sum == 0b0001 || sum == 0b1000) encoderValue --;

  //Limiting value range
  if (encoderValue>highEnd){
    encoderValue = highEnd; 
  }
  if (encoderValue< lowEnd){
    encoderValue = lowEnd; 
  }
  lastEncoded = encoded; //store this value for next time
}

//4 Buttons Click Functions
void Click() {
  digitalWrite(playLED, LOW);
  if (rClickedOnce == false && rLongPress == false)
  {
    rClickedOnce = !rClickedOnce; 
    mode = RealTime;
  }
  else
    mode = STOP;
}

void ClickPlay () {
  //terminate realtime in case it wasn't stopped
  rClickedOnce = false;  
  digitalWrite(realTimeLED, LOW);
  if (stepper.currentPosition()>inPoint || stepper.currentPosition()<inPoint)
  {
    mode = REWIND;
    Serial.println ("REWIND");
  }
  else if (stepper.currentPosition() == inPoint)
  {
    mode = PLAY;
    Serial.println ("PLAY");
  }
}

void ClickIn () {
  // Saving In
  if (rClickedOnce == true) {                      
    inPoint = stepper.currentPosition();
    encoderIn = encoderValue;            //saved for variable speed 
    //Serial.print ("In Point = ");
    //Serial.println (encoderIn);
  }
  //Saving Low End of encoderVal
  else if (rLongPress == true) {
    lowEnd = encoderValue; 
    //Serial.print ("lowEnd = ");
    //Serial.println (lowEnd); 
  }
  blinkMark (inLED);
}

void ClickOut() {
  //Saving Out
  if (rClickedOnce == true) {
    outPoint = stepper.currentPosition();
    encoderOut = encoderValue;          //saved for variable speed
    
  }
  else if (rLongPress ==true) 
     highEnd = encoderValue;
 
 blinkMark (outLED);
}

// Press Function - calibrating lens and stepper
void rPress() {
  if (rLongPress == false) {
    Serial.println ("Lens Limit");
    rLongPress = true;
    highEnd = 4000;
    lowEnd = 0;
    mode = LENSCALIB;
  }
  else 
    mode = STOP;
  }

void stepperMove () {
  stepper.setMaxSpeed(1000);
  int stepsPerRotaryStep = (motorStepsPerRev * easyDriverMicroSteps) / rotaryEncoderSteps;
  stepper.moveTo(encoderValue * stepsPerRotaryStep);
  stepper.run();
 }

int blinkFunction (int y)   // Blink without delay
{

  unsigned long currentMillis = millis ();
  if (currentMillis - previousMillis>ledInterval)
  {
    previousMillis = currentMillis;
    if (ledState == LOW)
      ledState = HIGH;
    else
      ledState = LOW;
    digitalWrite (y, ledState);
  }
}

int blinkMark (int y)
{
  boolean b = HIGH;
  for (int i=0;i<6;i++)
  {
    digitalWrite(y, b);
    delay(75);
    b=!b;
  }
}

int focusSpeed (int valToRemap)
{
  valToRemap = map(valToRemap, encoderIn, encoderOut, 10, 1000);
  return valToRemap;
}