# Closing the Loop on Hobby Servos with MRL

This is really a page to talk about how to measure the actual angle that a servo has reached, rather than the last position the servo was told to move to.  The aim is to be able to allow the servos to automatically configure their actual max/min angles.  In addition, it should allow for quicker response to the stop() method.

The stop() method stops the sweeper that is currently running for the servo.  However, this only tells it to stop the sweeper, the servo will continue to move to the last position it was told to go to.

The idea is to center tap the potentiometer and run that to an arduino pin.  Also, tap the positive voltage at the potentiometer.  (2 analog pins!)  (Why? because the supply voltage of the servo is likely going to be different than the supply voltage for the arduino boards, and we'll need to account for that in our calibration routines.)

So, the idea is the following  (Example using analog pins Vref and A0)

Vref = supply voltage of (V+ as measured at the potentiometer) of the servo  (hopefully this is lower than the supply voltage of arduino!!)

A0 = signal at the center tap of the potentiometer.

On Servo startup for calibration

1. move to rest position ( 90 degrees) , measure V0(90) = 550
2. move to rest position + 10 degrees , measure V0 (value between 0-1023)
3. This is a 10 degree difference, compute the change in voltage per degree of movement. (V0(110) - V0(90))/10 = 4.3
4. TODO: MAX/MIN value auto compute
5. TODO: Move servo to 0 degrees  (wait for servo to finish moving)
6. TODO: measure V2, use calibrated scale to determine actual angle that servo is at  ( this computed angle is the servo min.
7. TODO: move servo to 180 degrees, and repeat step 7 to compute the max angle.

Now, in addition to this, when we call stop on the servo, we'll need to compute the current angle of the servo based on the voltage read on the A0 pin.  When stop is called, measure that voltage, and tell the servo to move to that position so it stops moving.

#file : home/kwatters/ServoFeedback.py edit raw
```###################################################
# ServoFeedback.py
# This script shows how to use analog feedback
# from the potentiometer of a hobby servo
# to determine the servos actual angle
# In this example we're using an Arduino Uno
# The Servo is connected to digital pin 8 of the Arduino
# The supply voltage (red wire on potentiometer) is connected to Vref
# The center pin on the potentiometer is connected to A0
# Note: ground was not connected from the potentiometer
# as it was observed that the potentiometer 3rd pin is not actually at ground.

###################################################
# Read the pin data in a callback from the arduino
# if it's pin 0, this is
###################################################
def pinData(pin):
centerValue = 550
resolution = 4.3
if pin.pin == 0:
#  print str(pin.value) + " " +str(pin.type) + " " + str(pin.pin)
print str(pin.value) + " " + str(pin.pin)
# the pin value will be between 0-1023 center is at 550 (usually)
# 90 degrees is measured as 550 and 100 degrees is 593.
# get the difference from 90 degrees
diff = pin.value - centerValue
# how big is that difference in degrees
delta = diff/resolution
angle = 90 + delta
print "Measured angle is " + str(angle)
# TODO: servo.moveTo(angle)

# Create the arduino
arduino = Runtime.createAndStart("arduino", "Arduino")
arduino.connect("COM30")
# create and attach the servo
servo = Runtime.createAndStart("servo", "Servo")
servo.attach("arduino", 8)

# add the python call back for the publishPin method to invok pinData in python.

# start reading the value from analog pin 0.
arduino.arduino.enablePin(0)

# move the servo to center position  (here, I used the GUI and measured 550)
# this computes the center value
# use the gui to move the angle +/- 10 degrees to another angle and compute the resolution.
# update the constants in the pinData method
servo.moveTo(90)

# TODO: programmatically implement the calibration methods.

```

In my local testing, I've tried a few configuration to center tap the servo.  In all cases the servo is powered with an external power supply.  The ground of the arduino is connected to the ground of the external power supply.  For simplicity I'll refer to the potentiometer as pin 1, 2 and 3, for the plus, center, and minus ends of the potentiometer respectively.  In all tests, pin 2 was connected to A0 of the arduino.  These numbers below represent manually turning the potentiometer connected to the servo and the arduino board until the servo motor stopped turning.

 Value @ 0 Degrees 90 Degrees 180 degrees Pin 1 Pin 2 Pin 3 167 554 839* Vref A0 Ground 168 554 837* Vref A0 Open 68 223 377 Open A0 Open 68 223 377 Open A0 Ground

Note: * These were not very stable values.  The motor did eventually actually stop, however it was very sentisitive to noise as the motor tended to jitter in this configuration.  Turning the angle down to about 170 degrees made it less sensitive and it was easier to make the servo stop.

It's clear from the data that the Vref pin is scaling the input.  This provides much better resolution for reading as is seen by the first two connection scenarios.  A few things to be concerned with.

1. The Vref must be lower than the supply voltage of the arduino. (a higher ref voltage will damage the arduino.
2. Noise should be cleaned up (if possible from PIN2 on the pot to isolate it better from arduino noise.
3. The real range of values is not from 0-1024, but rather from about 167 - 839.  This is interesting because the potentiometers actually rotate though 270 degrees of rotation, but the servo only responds to the center 180 degrees of it.  This gives about 3.7 steps per degree for the servo motor.  This mapping will vary from servo to servo.
4. Variation on the servo supply voltage will properly be scaled on the A0 pin so long as the Vref pin is connected.

### References:

Here's a video about using the analog reference voltage pin to properly scale reading of the arduino analog input pins.  It was possible to get the servo to stop, but it was very sensitive in this range to noise.

## Comment viewing options

### ...my center tapped Servo reads.......

Indeed the Center_Tap mod works well.

On my 5 V supplied MG90S I get :-

0.605 V to 2.45V for full servo movement on the center tapping of the feedback potentiometer.

You just have to be careful that the new positional wire does not pick up "Noise" (maybe screened cable on long runs), else the Servo will get the "Jitters".

If your ADC chip has a reference voltage the you can offset this to give full range bit conversions.

ie the Arduino has an ARef pin if I recall.

A neat reason for doing the mod to your servos, particularly with a Multi Servoed robot like InMoov is during master power switch turn on ... you could scan all the Servos to get their starting positions and "Soft Reset" InMoovs posture to stop any any big twitching taking place (for example my spider robot does this and Its quite disconcerting when it almost jumps off the table when it first switches on and the servos become active from random positions.

Regards G

### Jitters and worries

Hi Gareth!   I didn't even think to look to see if an analog reference pin was already on the arduino.   That's very handy.  A little reading shows that the input of that pin is limited between 0-5V, so if you're servos are powered with a greater voltage, you'll risk damanging the arduino.  Maybe having both connected will give a better scale of resolution.  The jitters , electrical noise, and loading of the circuit were some of my concerns..

We could probably design a voltage divider to scale the both the ref voltage and the center tap voltage to values acceptable, and maybe it could help isolate some of the noise...

### Caps off to you

I was thinking about the noise problem.  I think perhaps adding a small capacitor between analog in 1 and ground in parallel will sink the noise.

The idea is the noise is going to be and analog (AC) signal.  Capacitors will act as a short circuit for a DC value.  So the AC noise should ground out where as the DC value should pass through to the A1 pin..

I'll be away a few days, but maybe when I get back I'll have some time to test it ou.

### Joystick to servo control using computer based sweep

Below is a python script that will use the thread based sweeping to incrementally move the servo based on the joystick.

the direction of the joystick controls the direction of the sweep and the magnitude of the joystick push is inversely porportional to the delay for the sweep between steps.

import time
import math

leftPort = "COM15"
rightPort = "COM19"

############################
# Start the InMoov Service
############################
i01 = Runtime.createAndStart("i01", "InMoov")
i01.startAll(leftPort, rightPort)

############################
# Start the Joystick service
############################
uberjoy = Runtime.createAndStart("uberjoy", "Joystick")
# Update this to be your controller
uberjoy.setController(2)
uberjoy.startPolling()

############################
# Attach the joystick to
# the inmoov service servos
############################

def AListener(value):
i01.rightHand.close()

def XListener(value):
i01.rightHand.open()

i01.rightArm.shoulder.setSpeedControlOnUC(False)

def StickXListener(value):
print "Stick X :" + str(value) + " Current pos: " + str(i01.rightArm.shoulder.pos)

absValue = math.fabs(value)
if (absValue < 0.175):
print "Stop sweep"
i01.rightArm.shoulder.stop()
return
absValue = absValue-0.01
print "Set Speed " + str(absValue)
i01.rightArm.shoulder.setSpeed(absValue)
delay = int((1-absValue) * 200)
if (value > 0.0):
if (i01.rightArm.shoulder.isSweeping()):
i01.rightArm.shoulder.setDelay(delay)
else:
i01.rightArm.shoulder.sweep(i01.rightArm.shoulder.pos, i01.rightArm.shoulder.max, delay, 1, True)
else:
if (i01.rightArm.shoulder.isSweeping()):
i01.rightArm.shoulder.setDelay(delay)
else:
i01.rightArm.shoulder.sweep(i01.rightArm.shoulder.min, i01.rightArm.shoulder.pos, delay, -1, True)

########################################################
# Left Arm Control  (left joystick for rotate and shoulder)
########################################################
uberjoy.map("x", -1, 1, -1, 1)