//*********************SERVO POSITION CODE*************************
//servo array positions (8 total since the encoder has 8 clicks)
//encoder has a resolution of about 6 clock cycles:
//156, 162, 168, 174, 180, 186 neutral pos, 190, 194, 200, 206, 212
//-servo moves about every 6

/*
//this code does not work for some reason
//arrays are not actually storing the initialized values
unsigned int servo1table[]={156,162,168,174,180,186,190,186};
unsigned int servo2table[]={162,174,186,194,200,194,186,174};
unsigned int servo3table[]={162,174,186,194,200,194,186,174};
unsigned int servo4table[]={162,174,186,194,200,194,186,174};
unsigned int servo5table[]={162,174,186,194,200,194,186,174};
unsigned int motor1table[]={2,3,4,5,6,5,4,3};*/

/* points from Peter
1	2	3	4	5
217	182	197	191	194
174	202	198	188	189
136	223	191	166	136
132	229	188	158	99
122	234	187	154	86
119	238	186	150	79
160	219	185	162	84
210	191	189	185	139
*/

/* angles from Ravi after rounding to nearest value
 npctl ntctl
     5     8
 time    angle(deg)
  0.000  -2  -1   0   0   0
  0.111   1   0   0   0   0
  0.222   5   2   0  -2  -4
  0.333   5   3   0  -3  -7
  0.444   6   3   1  -3  -9
  0.556   6   4   1  -3  -9
  0.667   5   3   1  -4  -10
  0.778   2   2   1  -2  -9
*/

//156, 162, 168, 174, 180, 186 neutral pos, 190, 194, 200, 206, 212


//194 neutral pos
unsigned int servo1table[8];//note: in reverse from others
servo1table[0]=196;//-2+
servo1table[1]=194;//1+
servo1table[2]=186;//5+
servo1table[3]=186;//5+
servo1table[4]=183;//6+
servo1table[5]=183;//6+
servo1table[6]=186;//5+
servo1table[7]=190;//2+

//186 neutral pos
unsigned int servo2table[8];//note: in reverse from others
servo2table[0]=186;//-1+
servo2table[1]=186;//0+
servo2table[2]=183;//2+
servo2table[3]=180;//3+
servo2table[4]=180;//3+
servo2table[5]=178;//4+
servo2table[6]=180;//3+
servo2table[7]=183;//2+

//190 neutral pos
unsigned int servo3table[8]; //note: 186->190=2 degrees
servo3table[0]=190;//0+
servo3table[1]=190;//0+
servo3table[2]=190;//0+
servo3table[3]=190;//0+
servo3table[4]=194;//1+
servo3table[5]=194;//1+
servo3table[6]=194;//1+
servo3table[7]=194;//1+

//186 neutral pos
unsigned int servo4table[8];//note: in reverse from others
servo4table[0]=186;//0+
servo4table[1]=186;//0+
servo4table[2]=188;//-2+
servo4table[3]=190;//-3+
servo4table[4]=190;//-3+
servo4table[5]=190;//-3+
servo4table[6]=191;//-4+
servo4table[7]=188;//-2+

//186 neutral pos
unsigned int servo5table[8];
servo5table[0]=186;//0+
servo5table[1]=186;//0+
servo5table[2]=178;//-4+
servo5table[3]=174;//-7+
servo5table[4]=174;//-9+
servo5table[5]=174;//-9+
servo5table[6]=170;//-10+
servo5table[7]=174;//-9+

/*unsigned int motor1table[8];
motor1table[0]=55;//
motor1table[1]=55;//
motor1table[2]=55;//
motor1table[3]=55;//
motor1table[4]=55;//
motor1table[5]=55;//
motor1table[6]=55;//
motor1table[7]=55;//*/