Download Gimbal Mount Detection Response Mechanism

Gimbal Mount Detection /Response Mechanism
Starting from scratch with the Arduino Mega 2560
The gimbal mount is the stabilization system which the LiDAR and vision system would ideally be mounted on.
The general concept is that the unstable motion of the boat due to thrust, braking, turning, and waves will
have a direct effect on the boats main detection systems (LiDAR and vision). The Gimbal mount is designed to
be an automated orientation correcting system, on which the detection systems should be mounted.
This section will focus only on the sensing mechanism, the logical operations involved with interpreting input
and the servo response. Mounting layout, design and frame of the gimbal mount are part of a different
assignment.
Accelerometer Sensor
The sensor made available to the ME ASV team was the ADXL335 by SPARKFUN.COM. This sensor is powered
by 3.3 V and produces a 3-axis analog output signal ranging from 0 to 3.3 V.
Raw Sensor Data
In order to get a meaningful reading from this accelerometer, the output must first be converted to a voltage.
The sensor outputs an analog voltage reading. The analog signal coming directly has characteristics which will
not be discussed in this paper. By the time the signal reaches the Arduino 2560 analog ports, it is a 10 bit (210 =
1024 step) analog signal. The raw data takes on the form of any number between 0 and 1024 depending on
the magnitude of acceleration it is measuring. The following is a sample of raw analog sensor data from the
accelerometer in a nearly stable condition:
X
332
331
331
333
332
332
331
332
331
332
332
333
332
332
333
331
333
331
334
332
Y
331
331
332
334
331
330
330
330
331
332
333
333
332
331
330
329
333
332
333
332
Z
393
394
391
398
396
394
394
394
394
395
398
396
394
394
394
391
396
393
396
395
Notice that the X and Y axis readings are approximately 330. This value represents the datum, which means
that at 330, 0 acceleration is being detected. Also notice that the Z axis reading is approximately 400. This is
the measure of acceleration due to gravity. This value is approximately 70 higher than the datum, which
means that 1g (9.81 m/s2) is about 70 on this scale.
It is inconvenient to make calculations based on the raw data, because this scale does not distinguish between
the directions of acceleration. The following chart illustrates what the values of raw data mean.
Reading
0
< 330
330
> 330
660
660 - 1023
Meaning
max measurement in positive direction
positive acceleration
no acceleration
negative acceleration
max measurement in negative direction
not used*
*The analog ports on the Arduino Mega 2560 by default give 10-bit (0-1023) readings based on a 5 V scale. Since the accelerometer
is being powered by 3.3 V, the maximum it can output is 3.3 V. Hence, the maximimum reading corresponding to 3.3 V is 660. Values
in the 660 - 1023 range will not be seen with this voltage.
We could take it even further and convert the voltage values to their corresponding acceleration expressed in
m/s2. It is simply a matter of multiplying by a constant. However, since it does not affect the calculation
process and nobody will be reading the values, we will leave accelerations in terms of voltage.
Stabilization - Compensating for rotational motion
During boat navigation there are two types of rotational motion that will be encountered. This first is pitch,
which causes the bow of the boat to rise up and dip down. The second is roll, which causes the boat to sway
left and right.
With the current dual pontoon design, in addition to the low wave speed conditions that the boat will be
encountering, we are anticipating a very small amount of rolling rotation. Therefore, we have decided that it
will not be necessary for the gimbal to compensate for roll. Compensation will only be necessary for the
pitching due to forward thrust and deceleration. With a simple free body diagram, the following formula can
be derived, which expresses the pitch angle (in radians) as a function of the gravitational force (voltage
reading) measured on the x-axis and the z-axis.
NOTE: This function is designed to work by measuring the static acceleration due to gravity only. Dynamic acceleration
caused by abrupt movements, shock or vibrations of the accelerometer will register a reading, which will be factored
into the stabilization calculations, which the equation is not prepared to encounter. This is a potential source of error
and can be expected to increase over extended durations of use.
Instantaneous response Vs. Arrayed Average Response
Making servo control adjustments based on instantaneous measurements was tested. It has been found that
high frequency of adjustments based on the current state of the accelerometer can give an unstable response.
The accelerometer operates at frequency of 1600 Hz. Making adjustments at each instant causes the servo to
be jittery. This may be due to servo responses being made based on noise that occurs for short instances.
There are several control methods that can reduce the appearance of an unstable response. The methods take
varying approaches, but what they have in common is the idea of formulating a response based on multiple
input readings rather than just a single reading.
We decided to take several output readings at timed intervals. It is convenient to store the values to be used
in the calculation to an array variable. For purposes of the gimbal code, the array variables were gravitational
force readings (measured in Voltage) on the x, y, and z axes spaced out by 5 milliseconds. Each array contained
15 data readings.
In our averaging method, we simply average the values contained in the array to produce a single value which
will be used to control the servo. With this method, servo responses are less frequent, but more stable.
Other Array Averaging Techniques
There are better methods available for controlling the servo. More testing and experimentation needs to be
done. However, some of the ideas we had in mind might include giving greater weight to the more recent
readings and lesser weight to the less recent readings. By doing this the averaging technique will be taking into
account the fact that the current physical state of the accelerometer is more similar to the most current
reading than the older readings.
There are also many control system stabilizing techniques which were not considered or implemented during
this project simply due to lack of time.
Servo Output
For testing purposes, the gimbal control system has utilized the Tower Pro SG5010 airplane servo. It operates
in the range of 4.8 – 6.0 V with the following operating speeds and torques:
4.8 V: speed 0.2sec / 60degrees
torque 5.5 kg / cm
6.0 V: speed 0.16sec / 60degrees
torque 6.5 kg / cm
For purposes of manipulating the current gimbal mount design, the servo is insufficient. Future designers
should look into selecting a servo which has ratings that exceed the maximum torque conditions that will be
encountered due to the weight of the gimbal mount.
The SG5010 servo has a 180 degree range of motion. ARDUINO has a predefined function for controlling a
standard servo.
#include "Servo.h" First you must call the servo controller function
Servo myservo; Then you must declare the servo with a name
myservo.attach(2); Assign the servo control signal to an output port
myservo.write(theta); Finally, you must embed this into a loop, where theta is the angle (in degrees from 0180) that the servo should be orienting itself towards.
The diagram below illustrates the degree positions of the servo as interpreted by ARDUINO. The neutral
position as shown in the illustration is at 90 degrees.
Increased precision for servo output
With the function call myservo.write(theta); the precision of motion is limited. With it there are only 180
different degree positions that the servo is able to assume. For extra precision, there is an alternative function
available:
myservo.writeMicroseconds(theta);
where theta is the angle (expressed as a numerical value from 1000 to 2000) that the servo should be
orienting itself towards. With writeMicroseconds, there are 1000 different positions that the servo can
assume. However, since these positions are not in degrees, any theta values found by trigonometric
calculations must be further converted to the writeMicroseconds angular format.
*NOTE: Depending on the servo manufacturer, writeMicroseconds may range either from 1000-2000 or from 17002300. When testing to find out which range your servo has, you should not allow the servo to push the maximum
turning limit too long (you will hear the servo roaring), otherwise you may damage the servo.
Future Gimbal Control Considerations
The current gimbal was designed around the Arduino Mega 2560 architecture. It utilized an external analog
accelerometer. A future design will utilize the ArduPilot which has several built-in digital sensors, including a
gyro and accelerometer. It is hoped that with the combination of input from the gyro and the accelerometer, a
more accurate and complete measurement process can be designed for determining the orientation of the
boat that is not based solely on static acceleration. The downfall of the accelerometer method used this
semester is the accumulated error that is generated as a result of dynamic accelerations.
Deliverable
The deliverable for this assignment is the actual real-time motion of the servo based on accelerometer
motion. This is viewable at the following link:
http://dasp.mem.odu.edu:8080/~asv_fa11/reports/gimbal_Deliverable_Demonstration.mp4
Current ARDUINO Code
#include "math.h"
#include "Servo.h"
const int xPin = 1; // analog: X axis output from accelerometer
const int yPin = 2; // analog: Y axis output from accelerometer
const int zPin = 3; // analog: Z axis output from accelerometer
const int dataPoints = 15;
double xV[dataPoints] = {0};
double yV[dataPoints] = {0};
double zV[dataPoints] = {0};
//Not currently used weighting multipliers
//double C[dataPoints] = {0};
//double c[dataPoints] = {0};
int val[3];
Servo myservo;
void setup() {
Serial.begin (9600);
for (int i = 0; i < dataPoints; i++) // this loop takes the raw readings from the accelerometer and converts
them to voltage readings.
{
xV[i] = analogRead(xPin)/1023.0*5.0-1.65;
yV[i] = analogRead(yPin)/1023.0*5.0-1.65;
zV[i] = analogRead(zPin)/1023.0*5.0-1.65;
}
myservo.attach(2);
}
void loop() {
// this loop shifts the contents of the arrays down so that the new value can be stored in slot 0. It also
//discards the value in the final slot (the oldest reading).
for(int i = dataPoints - 2; i >= 0; i--)
{
xV[i+1] = xV[i];
yV[i+1] = yV[i];
zV[i+1] = zV[i];
}
// the new value is placed into slot 0 of the array.
xV[0] = analogRead(xPin)/1023.0*5.0-1.65;
yV[0] = analogRead(yPin)/1023.0*5.0-1.65;
zV[0] = analogRead(zPin)/1023.0*5.0-1.65;
double sumx = 0;
double sumy = 0;
double sumz = 0;
/* this loop creates the multiplying weighting factors for weighted averaging
for(int i = dataPoints-1; i = 0; i--)
{
C[i] = (2/dataPoints)*i;
}
//Reverse the order of the multiplier array
int j = 0;
int k = dataPoints-1;
while(k){
C[j] = c[k];
j++;
k--;
}
*/
// this loop sums the values of the array and stores them to a variable.
for(int i = 0; i < dataPoints; i++)
{
sumx = sumx + xV[i];
sumy = sumy + yV[i];
sumz = sumz + zV[i];
/* Experimental weighted averaging technique; not currently used
sumx = sumx + xV[i]*c[i];
sumy = sumy + yV[i]*c[i];
sumz = sumz + zV[i]*c[i];
*/
}
// the variable is now divided by the number of data points (to produce an average value)
double avxV = sumx/dataPoints;
double avyV = sumy/dataPoints;
double avzV = sumz/dataPoints;
// the x and z state of acceleration due to gravity are used to calculate the angle of tilt.
double theta = 180/3.14159*asin(avxV/sqrt(avxV*avxV+avzV*avzV));
Serial.print(avxV); Serial.print("\t");
Serial.print(avyV); Serial.print("\t");
Serial.print(avzV); Serial.print("\t\t");
Serial.print(theta); Serial.print("\n");
myservo.writeMicroseconds(5.556*(theta + 90) + 1000); //
//Tested with 700-2300; did not work on this scale
//myservo.writeMicroseconds(8.889*(theta + 90) + 700); //
//Low precision servo range 0-90 degree
//myservo.write(theta + 90);
//delay(5);
}