 double Tp = 0;              // torque of the motor pulley
 double duty = 0;            // duty cylce (between 0 and 255)
 unsigned int output = 0;    // output command to the motor
+float set_position = 0.0f;
 
 
 /*

	return result;
}

/*
    Loop function
*/
bool dir;
int32_t nextcall = 1000;
void loop() {
  // read the position in count
  readPosCount();
  calPosMeter();

  if(millis() > nextcall) {
    nextcall += 200;
    analogWrite(pwmPin, 100);
    dir = !dir;
    digitalWrite(dirPin, dir);
	
	Serial.print(readJoystick()); // return joystick state
    Serial.print(" ");
    Serial.println(xh);
  }
  
}


  // Set up serial communication
  Serial.begin(57600);

  // Set PWM frequency
  setPwmFrequency(pwmPin, 1);
  // Input pins
  pinMode(sensorPosPin, INPUT); // set MR sensor pin to be an
  // set joystick pins as input with pullup 
  pinMode(joyFwdPin, INPUT_PULLUP);
  pinMode(joyBackPin, INPUT_PULLUP);
  pinMode(joyLeftPin, INPUT_PULLUP);
