Two mode Servo Control (Direct and Precise)

1//libraries
2#include <Servo.h>
3
4//GLOBAL  
5//servo pin
6const  int servoPin = 2;
7//creating servo motor object
8Servo sMot1; 
9//saved servo  postition
10float sSPos;
11
12//analog joistick pins
13const int xPin = A6;
14const  int yPin = A7;
15const int clickPin = 3;
16//values store for joystick
17int  xVal, yVal, clickVal;
18
19//built in LED pin
20const int ledPin = 13;
21
22//read  delay (ms)
23int dt = 0;
24
25//current control mode
26int currentState = 1;
27
28//GLOBAL  FUNCTIONS
29//set servo in position
30void sPos(int dg) {
31  sMot1.write(dg);
32  //delay(1000);
33}
34//MODE CHANGER
35//Switches to precise or direct mode  whenever the analog joystic button is clicked
36void changeMode(){
37if (currentState  == 1 && clickVal == 0) {
38  currentState = 0;
39  delay(500);
40} else if (currentState  == 0 && clickVal == 0) {
41  currentState = 1;
42  delay(500);
43}
44}
45
46void  setup() {
47  
48//configure serial port transmission rate
49Serial.begin(9600);
50//set  the servo library object to the right pin
51sMot1.attach(servoPin);
52//sets the  servo position in the middle
53sSPos = 85; 
54
55//configure joystick pins
56pinMode(xPin,  INPUT);
57pinMode(yPin, INPUT);
58pinMode(clickPin, INPUT);
59digitalWrite(clickPin,  HIGH);
60}
61
62
63void loop() {
64  xVal = analogRead(xPin);
65  yVal = analogRead(yPin);
66  clickVal = digitalRead(clickPin);
67  //delay(dt);
68  //DEBUG
69  //Serial.println("xVal:  "+String(xVal)+" | yVal: "+String(yVal)+" | clickPin state: "+String(clickVal));
70  //Serial.println("current operation mode: "+String(currentState));
71  
72  changeMode();
73  if (currentState == 0){
74      //operate servo direct mode
75      //DEBUG
76      //Serial.println("DIRECT MODE: Servo position: "+String(sSPos));
77      sSPos = (170./1023.)*yVal;
78      sPos(sSPos);
79  } else {
80      //operate  servo precise mode
81      //DEBUG
82      //Serial.println("PRECISE MODE: Servo  position: "+String(sSPos));
83      //Serial.println("Saved sero position: "+String(sSPos));
84      delay(10);
85      if (sSPos < 170 && yVal > 524) { 
86          sSPos =  sSPos + 1; 
87      } else if (sSPos > 0 && yVal < 518) {
88          sSPos =  sSPos - 1;
89  }
90
91  sPos(sSPos); 
92  }
93
94
95
96
97  //click to  blink built in LED, to indicate when the mode was switched
98  if(clickVal == 0){
99    digitalWrite(ledPin, HIGH);
100  }
101  else {
102    digitalWrite(ledPin, LOW);
103  }
104}