Senior Project - CubeSat Report
-
Upload
matt-bergman -
Category
Documents
-
view
300 -
download
5
Transcript of Senior Project - CubeSat Report
CubeSat Prototype: Development of 3-Axis Attitude
Determination and Control System and Frictionless Test
Bed
Aerospace Engineering Department
California Polytechnic University, Pomona
3801 West Temple Ave
Pomona, CA 91768, USA
Presented by: Team Lead, Wesley Eller
Omar Alhassan Matt Bergman
Kasbar Gulbenli Jeremiah Kho Kyle Teshiba ECE Team:
Lead, Duy Tran Jamison Berg
Chris Buen Additional assistance:
Shannon Acedillo Jake Greenwood E’Quacy Parker
1
Executive Summary
The objective of the 2015 CubeSat project was to refine a three axis attitude control
system built for the CPP CubeSat prototype during the 2014 CubeSat program, and develop a
one dimensional test-bed using air bearings in order to test the control system. These objectives
were each accomplished. A PID controller was implemented, and the performance of the control
loop was tested using both a breadboard simulator and the developed test-bed. The test-bed was
constructed using air bearings provided by Dr. Edberg and mounted to the CubeSat, along with a
tank of C02. The assembly was then placed on a leveled, clean glass plate, which was mounted
in a wooden frame. The resulting system allows for the CubeSat to translate in two directions
and rotate in one axis with very little resistance from friction. The data collected during runs
included position, acceleration, and magnetic flux from the compass vs time, which was logged
using a Matlab script and is presented here in three plots
2
Contents Executive Summary .................................................................................................................................. 1
1.0 Objective ............................................................................................................................................... 5
2.0 Analysis ................................................................................................................................................. 7
2.1 Governing Equations of CubeSat Attitude Dynamics ....................................................................... 7
2.2 PID Controller Theory...................................................................................................................... 10
3.0 Project Design ..................................................................................................................................... 12
3.1 CubeSat Design ............................................................................................................................... 12
3.2 Attitude Determination and Control System .................................................................................. 15
3.3 Power System.................................................................................................................................. 20
3.4 Test Bed Design ............................................................................................................................... 25
3.5 Pneumatic Piping System ................................................................................................................ 28
4.0 Results and Conclusion ....................................................................................................................... 34
5.0 Summary ............................................................................................................................................. 36
Appendix A: Control Loop Code ................................................................................................................ 37
Original Proportional Controller Code .................................................................................................. 37
PID Control code, final iteration ........................................................................................................... 44
Wesley
Jeremiah
Matt
Matt
Wesley
Wesley
Matt
Jamison
Kyle
Kasbar
Wesley
Wesley
Wesley &
Matt
Formatting,
tables,
contents,
misc: Omar
3
List of Symbols
kp Proportional gain
ki Integral gain
kd Derivative gain
t Time
τ Integration parameter
u(t) System response as a function of time
e(t) System error as a function of time
4
List of Figures
Figure 1-1 CubeSat Project Work Break Down Structure ............................................................................. 6
Figure 2-1 Cubesat Orientation and Axis Legend.......................................................................................... 7
Figure 3-1 Pololu MinIMU-9 IMU ................................................................................................................ 16
Figure 3-2 Reaction Wheel .......................................................................................................................... 18
Figure 3-3 Final Solar Array Layout on CubeSat .......................................................................................... 21
Figure 3-4 Sundance Solar Panels ............................................................................................................... 22
Figure 3-5 Solar Panel Array Configuration #1 ............................................................................................ 23
Figure 3-6 Solar Panel and Battery Switching Circuit Schematic ................................................................ 24
Figure 3-7 Completed Test Bed ................................................................................................................... 25
Figure 3-8 Test Bed Frame .......................................................................................................................... 26
Figure 3-9 Test Bed Base and Glass Sheet .................................................................................................. 26
Figure 3-10 Test Bed Border ....................................................................................................................... 27
Figure 3-11 CubeSat in Test Carriage .......................................................................................................... 28
Figure 3-12 Air puck bottom side with circular air passageway ................................................................. 29
Figure 3-13 Air puck top side with Velcro mounting points ....................................................................... 30
Figure 3-14 Silicone tubing to air regulator adaptor .................................................................................. 30
Figure 3-15 24 oz and 12oz Air tank ........................................................................................................... 31
Figure 3-16 Pneumatic Regulator ............................................................................................................... 32
Figure 3-17 CubeSat in test configuration .................................................................................................. 33
5
1 Objective The objectives of the project were to develop a PID controller to control the CubeSat’s
attitude to within ± 5º. Each team however also had individual objectives to meet the project’s
overall objective. The mechanical’s team’s objectives were to create a CAD model of the
CubeSat, build the test bed and air-bearing test system. The software team’s objectives were to
build the attitude determination system and the PID controller. The electrical team’s objectives
were to build and integrate a solar panel system to charge and discharge the CubeSat’s battery.
The systems team’s objective was to document and record the progress of the project and to
assist other teams as needed. The work breakdown structure for the project can be seen below in
figure 1-1.
6
Figure 1-1 CubeSat Project Work Break Down Structure
7
2 Analysis
2.1 Governing Equations of CubeSat Attitude Dynamics
Figure 2-1 Cubesat Orientation and Axis Legend
Let the components of angular velocity of the CubeSat in body-fixed coordinates X, Y, and
Z, as shown in Figure 2.1, be ω1, ω2, and ω3, respectively. The angular kinematics of the CubeSat,
in Euler angles, is therefore:
= (𝜔2 sin𝜙 + 𝜔3cos𝜙) sec 𝜃
= 𝜔2cos𝜙 − 𝜔3sin𝜙
= 𝜔1 + sin 𝜃
8
Where , , and are the Euler angles that determine the CubeSat’s position relative to an Earth-
fixed reference frame. The governing equations of the total system (CubeSat kinetic equations and
equations of reaction wheels) can be expressed as:
= 𝐺
= 𝑊
Where 𝐺 describes the angular momentum of the total system (CubeSat and reaction wheels)
about the center of mass, and 𝑊 represents the angular momentum of the reaction wheel relative
to the CubeSat body. The term 𝑊 is a representation of the known (or unknown) torques on the
CubeSat, and is the torque of the motor acting on the reaction wheels.
The complete, expanded angular kinetic equations of the system is:
𝑀1 = (𝐼𝑎 + 2𝐼𝑡 + 2𝑚𝑎2 + 𝐼𝑥𝑥)1 − 𝑚𝑎2 2 + 𝐼𝑎Ω1 + 𝐼𝑎Ω3𝜔2 +
(𝐼𝑧𝑧 + 𝐼𝑦𝑦 + 2𝑚𝑎2 )𝜔2𝜔3 − 𝐼𝑎Ω2 𝜔3 + 2𝑚𝑎2 𝜔1𝜔3
𝑀2 = (𝐼𝑎 + 2𝐼𝑡 + 2𝑚𝑎2 + 𝐼𝑦𝑦)2 − 𝑚𝑎2 1 + 𝐼𝑎Ω2 − 𝐼𝑎Ω3𝜔1 +
(𝐼𝑥𝑥 + 𝐼𝑧𝑧 + 2𝑚𝑎2 )𝜔1𝜔3 − 2𝑚𝑎2 𝜔2𝜔3 + 𝐼𝑎Ω1 𝜔3
𝑀3 = (𝐼𝑎 + 2𝐼𝑡 + 4𝑚𝑎2 + 𝐼𝑧𝑧)3 + 𝐼𝑎Ω3 + 𝐼𝑎Ω2𝜔1 − 𝐼𝑎Ω1𝜔2 +
(𝐼𝑦𝑦 + 𝐼𝑥𝑥)𝜔1𝜔2 + 𝑚𝑎2 (𝜔12 − 𝜔3
2)
Where M1, M2, and M3 are the unknown disturbance torques acting on the body of the CubeSat.
The principle moments of inertia of the CubeSat in the XYZ direction are given by Ixx, Iyy, and Izz,
respectively, whereas Ia and It are axial and transversal moments of inertia of the reaction wheels.
9
The angular velocity of the reaction wheels relative to the CubeSat are given by Ω1, Ω2, andΩ3,
and the term ma2 expresses the concentrated inertial moment of the reaction wheel due to the
CubeSat center of mass.
By applying angular momentum on the equation of the CubeSat actuators alone, the
governing equations of the actuators are obtained as:
𝑢1 = 𝐼𝑎(1 + Ω1)
𝑢2 = 𝐼𝑎(2 + Ω2)
𝑢3 = 𝐼𝑎(3 + Ω3)
Where u1, u2, and u3 and are the torques applied from the motor of the reaction wheels. In essence,
these 4 groups of equations are the governing equations of the CubeSat system, and indicate that
the system is coupled and highly nonlinear. The inputs to the system are torques, which in this
case are exerted by reaction wheels. The output of the system are the desired Euler angles.
10
2.2 PID Controller Theory Generally speaking, the CubeSat’s attitude dynamics equations are three second-
order nonlinear equation. Linearization of these equations are necessary because automatic control
theory does not provide exact analytical solutions for nonlinear dynamics. Many, if not most,
modern-day attitude controllers are very simple PID controllers. A proportional-integral-derivative
(PID) controller is a type of feedback controller that utilizes all three types of control gains:
proportional gain, integral gain, and derivative gain. By varying these gains, the controller can
change the response of the system to inputs. One of the great advantages to the PID controller is
that the transfer function of the plant, or CubeSat, can be unknown. The PID controller is described
by:
u(t) = kpe(t) + ki ∫ e(τ)dτ + kd
d
dt
t
0
e(t)
Where u(t) is the controller output in the time domain, e(t) is the delta signal (attitude
deviation), and kp, ki, and kd are the PID controller parameters. The equation above is the standard
“textbook” equation of the PID controller. The first term is the proportional term. Increasing the
proportional gain will decrease the rise time, increase the overshoot, decrease the steady-state
error, and degrade stability. The second term is the integral term. Increasing the integral gain will
decrease the rise time, increase the overshoot, increase the settling time, eliminate the steady-state
error, and degrade stability. The third, and final, term is the derivate term. Increasing the derivative
gain will decrease overshoot, decrease settling time, and improve stability if the gain is small.
However, in order to implement the PID control onto a microcontroller, a discrete (not
continuous) PID control law must be utilized. Due to sampling rates, microcontrollers are discrete
11
by nature. In order to derive the discrete-time PID algorithm, the Z-transform must be used. The
“velocity” form of this algorithm, which avoids computing the summation of the integral gain, is:
𝑢[𝑘] = 𝑢[𝑘 − 1] − 𝑘𝑝(𝑒[𝑘] − 𝑒[𝑘 − 1]) + 𝑘𝑖𝑒[𝑘] + 𝑘𝑑(𝑒[𝑘] − 2𝑒[𝑘 − 1] + 𝑒[𝑘 − 2])
Where u is the controller output, k is the time-step, and e is the error. This equation can
easily be implemented in a microcontroller with the C++ programming language. The pseudo-
code is presented below:
Error = Setpoint – Actual
Integral = Integral + (Error*dt);
Derivative = (Error – PreviousError)/dt
PID = (Error*Kp) + (Integral*Ki) + (Derivative*Kd)
PreviousError = Error
Tuning of the PID controller was done utilizing the Ziegler-Nichols method. It was
performed by settling the integral and derivative gains to zero, then tuning the proportional gain
until the control loop oscillated indefinitely at steady state. With the extracted gain value, and
period of oscillation, the controller’s integrator and derivative time constant can be looked up and
calculated from a Z-N tuning table.
12
3 Project Design
3.1 CubeSat Design
The CubeSat was designed collaboratively during the 2013-2014 CPP CubeSat project,
headed by Kyle Stovner. The primary design drivers were a 1 kg maximum mass, keeping all
electronics inside of the 10 cm3 frame volume. Several design compromises were therefore
required in order to fit all the necessary electronics into the frame. This included offsetting each
of the reaction wheels radially from the center of the frame walls by approximately 3 cm. This
complicated the requirements of the acs system, and created a physical inertial coupling and
torque coupling inside the system. This complexity is reflected in the design of the control loop,
which attempts to examine this coupling. The CubeSat was also designed with several safety
features, as are required by the NASA MUREP program for safe flight aboard the Weightless
Wonder for micro-gravity testing. These include a safety switch, with three settings and a
Master Kill Switch, for suddenly turning off the system. The safety switch allows the user to
choose to operate the CubeSat in one of three modes: off, operation, or programmable. During
flight testing, the switch was used to ensure that the system could not be accidentally powered
on, and could not be accidentally set to programmable and therefore suddenly cease
Figure 3.1-1 CubeSat internal layout diagram
13
operations. The CubeSat was designed to be highly modular in its electronic design, however, in
practice, the assembly was highly integrated. The layers of electronics shown in the diagram,
Figure 3.1-1, were integrated into the system from the bottom up. The relays were solidly
mounted to the frame of the CubeSat, along with the wheel motors and the superboard on
which the rest of the electronics were mounted. The wheel motors were also mounted to the
frame, after having the wheels installed and balanced. The 9 dof IMU, which included a rate
gyro, accelerometer, and a magnetic compass, were mounted as far away from the motors as
was possible. This was done to reduce ambient vibration from the electronics and wheels. The
gyro was also internally mounted to the superboard using Velcro, so that it could be
repositioned. This also allowed for easy replacement of the gyro when it needed to be replaced.
The gyro was replaced on two different occasions, first after flight testing when the gyro
appeared to be damaged, and a second time for testing purposes when the gyro drift
compensation was being tested.
14
Figure 3.1-2 Internal view of CubeSat
The batteries were finally placed in the fully integrated cube, and attached to the sidewall with
cabling and Velcro, in order to allow for easy replacement. The solar panels were ultimately
installed on the outside of three sides of the cube, leaving free the control side with the master
switches and the top bottom, for testing purposes. In a more practical design, all six sides could
have solar panels installed. The final weight of the CubeSat was approximately 962 g, just barely
short of the 1 kg mark. The outer dimensions were slightly more than the 10 cm cube allowed,
due to the extrusion of the solar panels.
15
3.2 Attitude Determination and Control System Attitude determination is the process of measuring a spacecraft’s orientation, which is done
by combining available sensor inputs with knowledge of spacecraft dynamics. Due to the power
of microcomputers available, attitude algorithms can now be programmed and processed onboard.
For this project, attitude determination of the CubeSat was provided using a 9-degree-of-freedom
(9-DOF) inertial measurement unit (IMU), which contains a 3-DOF accelerometer, magnetometer,
and gyroscope.
Attitude control is the process of commanding the spacecraft to desired orientation, and
requires the combination of the prediction of, and reaction to, a spacecraft’s rotational dynamics.
This control can be either passive or active. A Passive control system relies on the natural response
to an external environment to maintain the proper attitude, while an active control system relies on
computed actuator torques to achieve and maintain a desired orientation. Because of the use of
reaction wheels (actuators) in this project, the CubeSat’s control system was active.
Attitude systems include the sensor, actuators, avionics, algorithms, and software used to
determine and control the orientation of the system. Last year’s CubeSat team focused on the
avionics and actuators, while this year focused heavily on the software, algorithms, and sensors.
The following sections will go into more detail about the sensors, actuators, software, and power
system completed this year.
Rate sensors, such as a gyroscope, are used to measure the angular motion of a spacecraft,
independent of the spacecraft’s attitude (in an inertial reference frame). An important function of
a rate sensor is to provide angular stability to the spacecraft, which is done by feeding a position
error signal and a rate signal into the attitude control system. Although not done in this project,
16
rate sensors are also able to provide the angular attitude of the spacecraft via time integration of
their output. These rate sensors are usually classified as rate-integrating gyros (RIGs).
The specific rate sensor used in this project was the gyroscope. Gyroscopes on spacecraft
are almost exclusively used in strap down mode, which means that the gyro is rigidly attached to
the spacecraft (in this case, with Velcro). An unfortunate limitation of gyroscopes is that the
measured body rate has a small, but nearly constant offset. This results in a draft in the attitude,
but other attitude measurements can be incorporated to correct the drift.
The attitude control system is designed to return the CubeSat to a specific orientation after
it was disturbed. The orientation is determined by a Pololu MinIMU-9 inertial measurement unit.
The sensor utilizes a 3-axis gyro, a 3-axis accelerometer, and a 3-axis magnetometer to determine
the orientation and rotational speed of the CubeSat. This data is then converted through a feedback
control law, seen in section 3.2.1, to rotate the reaction wheels to return cube back to its original
orientation. The IMU can be seen in figure 3.3-1.
Figure 3-1 Pololu MinIMU-9 IMU
17
Momentum exchange devices, such as reaction wheels, are controllers that allow changing
the distribution of momentum inside the spacecraft, while keeping the total momentum of the
system constant. This is because a symmetrical rotating rigid body produces angular torque when
accelerated about its axis of rotation. It is important for the reaction wheel to provide as much
torque as possible in order to achieve fast attitude maneuvers.
The reaction wheel can be mounted in the satellite with its rotational axis in any direction
relative to the CubeSat’s axis frame. Reaction wheels are used as the primary attitude control
actuators on most spacecraft. A reaction wheel assembly contains a rotating flywheel, an internal
brushless DC electric motor, and associated electronics. For controlling the attitude in space, at
least three reaction wheels are required. In general, the required angular torques can be achieved
by accelerating the rotor of the electrical motors, whose axes of rotation are aligned with the body
axes. The primary task of the electrical motor is to provide the necessary angular torque to the
CubeSat. In order to apply a torque on the body about any axis, a torque in the opposite direction
must be produced by the rotor of the electrical motor.
The CubeSat uses three reaction wheels to control its orientation. A reaction wheel
is a device comprised of a flywheel attached to an electric motor. Each wheel is oriented in a
different axis of revolution and applies an angular velocity in that axis when spinning. As the
wheel’s angular speed changes, the CubeSat will counter-rotate based on the conservation of
angular momentum. These wheels can only apply a rotation about the spacecraft’s center of gravity
and are not capable of providing translational motion. These reaction wheels can be seen in Figure
3.3-2.
18
Figure 3-2 Reaction Wheel
3.3 CubeSat Attitude Determination and Control Software
The main objective for the CubeSat software team was to develop an attitude control
system which could meet pointing requirements within a few degrees of accuracies. The two main
problems which hindered the progress of completion was the drift of the gyroscope, which is an
accumulated false rate that off-sets the zero-rate value of the gyro, and a pointing algorithm issue
which would turn off the motors while the CubeSat was tumbling every time the CubeSat’s attitude
pointed towards a referenced pointing position.
Within seconds, gyroscope drift will cause the measurements of the IMU to stray from
their equilibrium position, and rise constantly as time increased. This would eventually cause the
referenced pointing direction to deviate, making the CubeSat’s attitude slowly off-set from the
requested pointing position. This drift was corrected using a Directional Cosine Matrix with a
proportional and integral (PI) controller. Gyro roll and pitch were corrected with accelerometer
measurements, and gyro yaw was corrected with magnetometer measurements. The gain values
used were derived from the manufacturer’s specification sheet, rather than through testing. Offset
19
values were also initialized when the program started, in order to further calibrate the sensor
measurements.
In order to correct for the tumbling issues, the CubeSat’s attitude control system used two
main modes: a de-tumbling mode to stop rotation, and a pointing mode to return the CubeSat to
an initial reference angle. The source for this initial issue was straight-forward. The pointing mode
required the CubeSat to turn off the motors when the referenced pointing attitude was reached.
However, if the CubeSat was tumbling quickly, the system would turn off the motors whenever
the desired orientation of the CubeSat was pointed at. The CubeSat was therefore unable to de-
tumble, or stop its excessive angular rotation, and point within the same control law.
The de-tumbling mode of the CubeSat initialized when the system’s gyroscope was reading
a rotation rate above three degrees per second. The de-tumbling mode’s purpose was to retard the
rotation rate of the CubeSat quickly and consistently. A motor-power algorithm was created which
increased the power to the motors depending on how fast the CubeSat was rotating. When the
CubeSat nearly stopped its motion (below 3 degrees per second), the pointing mode was initialized.
The pointing mode requested reference angles (yaw, pitch, and roll) for the CubeSat to
point towards. These angles were calculated based on the Direction Cosine Matrix of the system.
For testing purposes, the CubeSat always pointed to a heading angle of 0, which is in the direction
of true-North. The pointing mode used a motor-power algorithm which would increase the motor’s
power as the heading angle changed due to disturbances; at 0 degrees heading, the motors would
be off, while at 180 degrees heading the motors would be on maximum-power. Due to the motor
response, the pointing mode had a large overshoot, as well as a slight oscillation, before the system
settled.
20
The software team also wrote the power control algorithm. This was done using logic states
(binary 0 and 1). With logic 0 (not arbitrary, since the logic depends on the analog switch circuit),
the power system would charge the 1st battery and discharge the 2nd. With logic 1, the power system
would discharge battery 1 and charge battery 2. The power control system would continuously
check the battery voltages, and if one battery was drained below two voltages, the system would
switch to the other battery. If both batteries were at low power (less than two volts), a warning
error would pop-up.
3.4 Power System
The primary goal of the Electrical Team was to create a solar panel array in order to power the
CubeSat and increase operational time. The previous CubeSat did not have a method of power
generation and could only run for a limited amount of time. The methods that were initially
chosen for power generation were kinetic regression or using a solar array. To generate the
most power while reducing weight and increasing efficiency, solar power generation was the
best option. The final design of the solar array while mounted onto the CubeSat is shown below
in Figure 3.3 -1. Three sides were covered in order to allow for testing, while still providing
enough current to charge the batteries.
21
Figure 3-3 Final Solar Array Layout on CubeSat
The first step was to find a solar panel that would supply the proper amount of power
and efficiency while reducing weight and size. The initial solar panels that were tested did not
supply enough power to charge the batteries efficiently. These panels provided about 1V and
10mA, while taking less surface area, which did not meet the power requirements. More
research was conducted into more efficient and powerful solar panels. The solar panels that
were finally selected were the Sundance Solar Panels as displayed in Figure 3.3-2. These solar
panels were capable of producing 3V at 70mA.
22
Figure 3-4 Sundance Solar Panels
It was necessary to test the solar panels in order to ensure they met the necessary
requirements to supply adequate power. To do this a small test bed was created and the solar
panels were wired in both series and parallel to test which is more efficient. By using an voltage
meter, the series setup was decided upon. The panels were arranged in sets of four in series
with each side in parallel. Each set of panels is set to generate more than 12V with a maximum
of 210mA. At optimum conditions, with all panels in direct sunlight, the 800mAh batteries could
be charged in four hours.
Once testing of the solar panels was completed, the array could be designed. There
were two initial ideas of how the array would be designed. The first idea was that each side of
the CubeSat would have a set of solar panels on it. This would allow for energy generation
regardless of orientation of the CubeSat. This method is displayed in Figure 3.3-3. The second
23
idea was to have four unfolding panels which allowed for 5 panels on one side to be directed
towards the source of light. This would allow for more power generation due to the fact that all
panels would be faced in the ideal direction rather than the other configuration which would
have one panel in the direction of the source and the rest only receiving partial light. The final
selection was to have sets of solar panels on every side of the cube due to the fact that it would
add weight, complexity, and draw more power in order to overcome the momentum of the
folded out panels. Space restrictions were the main trade off in this decision, not allowing for a
spring mechanism and 4 panels to fit inside the cube.
Figure 3-5 Solar Panel Array Configuration #1
The configuration selection allowed for the design and creation of the schematic layout
of the circuitry. Based around the DG403 switching IC, a dual single-pole/double-throw function
was used. This has four input/outputs, which allows for a charge and discharge connection to
two batteries. Using the arduino microcontroller to control the switch, the two batteries
24
voltages are compared to determine which will charge. The other battery provides power to the
CubeSat system. A voltage regulator is used for the solar charging component to ensure no
overcharging of the batteries. The schematic design is seen below in Figure 3.3 -5.
Figure 3-6 Solar Panel and Battery Switching Circuit Schematic
D2
12V
D3
0.5V
R1
100
U2
DG403
I11
N.C.2
I33
O34
O45
O116
IN115
V-14
GND13
VL12
V+11
IN210
O29
I46
N.C.7
I28
Bat 1
12.5V
Bat 2
9V
U3
Arduino MCU
OUT13IN1
1
IN22
Solar Panel12-15V
Vcc
0
0
0
VccR2
330
R3
330
R4
330
R5
100
R6
330
R7
100
D4
D5
25
3.5 Test Bed Design
The test bed was initially designed out of need for a leveled, frictionless platform in order
to test the design of the CubeSat. Since no test platform was available at Cal Poly Pomona, the
test bed was constructed. The test bed consists of two parts, the test bed base and the test
carriage which holds the CubeSat. The requirements of the test bed were: (1) to provide a nearly
frictionless environment to test the CubeSat, (2) be big enough to allow the CubeSat to move
around, (3) enclose the CubeSat to ensure the safety of the system, and (4) be leveled to reduce
disturbances during testing.
Figure 3-7 Completed Test Bed
The first part of the design was for the frame of the test bed base, which was constructed out of
wood. The base of the test bed is made out of 1 x 2 inch beams that are put together in a square
measuring three feet by three feet. Then using the same size beams, cross members are put
across the base to reinforce the middle and the sides.
26
Figure 3-8 Test Bed Frame
The next part of the test bed base was the base platform. The base is comprised of a sheet
of laminated plywood and a ¼ inch sheet of glass separated by spacers. The laminated plywood
was screwed onto the beams of the frame in multiple points to ensure that it does not move or
shift. Then spacers made of felt with a single side of adhesive, were evenly spaced around the
board. Finally a piece of 3 x 3 ft ¼ inch glass is placed on top to make the frictionless surface
necessary for testing.
Figure 3-9 Test Bed Base and Glass Sheet
27
The last part of test bed is the border. The border was built with laminated plywood; cut
into lengths large enough that the edge of the sheets reached four inches above the surface of
the glass, and then screwed into place around the borders of the frame. This allows for adequate
protection from the test cube falling out.
Figure 3-10 Test Bed Border
The remaining part of the test bed was the carriage, which holds the CubeSat, air pucks,
and the air system that allows the CubeSat to move close to frictionless. This carriage was
designed in two parts that were modeled in SolidWorks and constructed created by 3D printing.
The other parts of the system were purchased. The basis of the system is an air tank that feeds
into a regulator which controls the rate of airflow during the test. The regulator then leads to an
adaptor which splits the flow into multiple streams and then through small flexible tubes into the
lower base. The lower base then houses four air pucks that provide the small cushion of air which
allows the base to float nearly frictionless.
28
Figure 3-11 CubeSat in Test Carriage
3.6 3.5 Pneumatic Piping System
Air pucks were used to create a layer of air to have the CubeSat hover on the test bed, to
minimize the amount of friction. The air pucks, made by Ameropean, shown in Figure 3.5-2,
created a smooth uniform flow of air to allow a leveled cushion for the CubeSat to move freely
on the test bed. The air pucks are measured at 40mm in diameter with an average air gap of 10
microns when air pressure is above 66psi. Three air pucks were used in an evenly spaced
triangular formation to allow the CubeSat to be balanced. The air pucks were mounted to the
bottom of CubeSat with velcro, shown in Figure 3.5-2, on the side opposite of the air flow.
29
Figure 3-12 Air puck bottom side with circular air passageway
The air pucks were connected to the air regulator by the use of Trelleborg SF Medical
Neutrasil Silicone tubing. The tubing, shown in Figure 3.5-2, was 10mm in diameter with a
thickness of 4mm to withstand the high pressure provided by the air tanks. Each air puck was
connected to one line of silicone tubing and was connected to the inlet by use of a notched hose
connector. The other side of the silicone tubing was connected to the air regulator with a 3-way
adaptor, shown in Figure 3.5-3. Each silicone tubing was 40 cm in length to allow for adequate
room to have the air pucks mounted and regulator mounted on top of the cube.
30
Figure 3-13 Air puck top side with Velcro mounting points
Figure 3-14 Silicone tubing to air regulator adaptor
31
The air storage devices were a set of Tippmann air tanks, shown in Figure 3.5-4, which
were readily available in the commercial world. The air tanks used, one 12 oz and one 24 oz,
have a pressure rating of 800psi which was connected to the air regulator to reduce the pressure
to an acceptable range for the air puck. The 24 oz tank was used for testing for leaks and
balancing issues because of its large volume allowing for more test runs. The 12 oz tank was used
for demonstration and testing for its smaller size and mass that allows for an easier time
balancing and smaller load on the air pucks.
Figure 3-15 24 oz and 12oz Air tank
The air regulator was used to drop the pressure of 800psi from the air tank to a safe range of 0-
120 psi for the air pucks. The air regulator, shown in Figure 3.5-5, was made by Interstate
32
Pneumatics which allows for the safe drop in pressure from tank to air puck. The air regulator
has a 3/8in inlet to a 1/2in outlet that allows the air tank to connect to the inlet and the
adaptor to the air pucks connected to the outlet. The regulator has a knob on the top to
change the outgoing pressure to any pressure between 0-120 psi.
Figure 3-16 Pneumatic Regulator
The fully assembled CubeSat in its test assembly shown in Figure 3.5-6, has all the air
pucks and air tank mounted on it. The test assembly has the air tank mounted to the top of the
CubeSat with velcro aligned with the c.g. as to not disrupt the Inertia of the cube. The air pucks
were evenly spaced in a triangular formation on the bottom of the cube and mounted with velcro.
The silicone tubes were mounted to the side of the cube with velcro as to not have the tubes
disturb any part of the cube while being tested.
33
Figure 3-17 CubeSat in test configuration
34
4 Results and Conclusion The CubeSat testing was conducted over the course of the 2014-2015 school year, though
the CubeSat was not operational until the spring quarter. Several critical problems were
encountered. The first was the difficulty of harnessing the air bearings to the CubeSat and
balancing the cg, so there was not tipping. This was solved using a simple suspenders-method for
finding the cg of the cube and CO2 tank with the plumbing, and balancing them over each other.
The resulting testbed was near frictionless, and accomplished its purpose excellently. This allwed
the team to complete several weeks of testing before the next problem was encountered. During
this time, the system produced excellent results, successfully detumbling from a rapid spin and
pointing with a few degrees of accuracy. Example recorded data is shown in previous sections of
the report. The system was also verified using a breadboard simulator with identical electronics
connected together without the reaction wheel system, and commanded response times were
measured. The CubeSat system was also subjected to the discussed one dimensional mechanical
bearing testing. The CubeSat shortly thereafter began to experience a critical electrical problem,
where the batteries appeared to be connecting directly to the wheels on any command speed. The
wheels therefore turned at maximum speed under any command signal. This was temporarily
circumvented by powering the cube via a hardline running from a computer to the board in
conjunction with the batteries. This was short lived, however, as this soon caused a critical fault
in the electrical system. This fault has still to be solved, as it results in an inability for the
computer to communicate with the CubeSat system, apparently as the data lines are constantly
occupied by some other signal. Several attempts were made to forcefully open communication
and find the fault point. These attempts included resetting the Arduino board, forcing open and
closed all the ports on both the communication computer and the Arduino, and electrically
isolating the microcontroller as much as possible. Finally, the microcontroller was removed and
35
a new one installed, which required cutting the old microcontroller out and soldering a new one
into the assembly. Even this, however, did not solve the problem, which appears to be some
substantial signal feeding back into the communication line and interrupting any attempt to
communicate with the device. Unfortunately, this renders the CubeSat currently unusable, as the
microcontroller appears unable to communicate either through xBee radio communication, over
a serial port, or with the motors in the ACS system. The project is therefore of limited success,
since a live demonstration was never conducted. However, the team that worked on the project
were able to see results, and met with some significant, if temporary success. The success of
previous years is may be greatly attributed to the involvement of members of several majors,
especially several Electrical Engineering students. While Aerospace students are very
multidisciplinary, the difficulties encountered by the CubeSat team in the final weeks of this
term were largely beyond the scope of the readily available knowledge. With more time, or more
EE involvement, these problems could likely be overcome. The air bearing testbed, however,
worked excellently, and will be a valuable addition to future years.
36
5 Summary The CubeSat program has endeavored to produce a functioning Cubeat prototype
Attitude Control System in three axis, and to test this system on a test-bed produced by the team.
This was accomplished successfully, although the system eventually failed due to unsolved
electrical problems. The test-bed which was developed worked well and provided near
frictionless testing in X and Y translations and one rotational axis. This facilitated response
testing in all three axis of rotation, since the CubeSat is intended to be controlled in any
orientation. The control laws written for the CubeSat worked as intended, and provided both
detumbling and pointing capabilities. Solar panels were also integrated into the design, allowing
for an indefinite recharge-use cycle, bringing the CubeSat one step closer to space readiness. In
future years, teams will need to first identify and resolve the electrical problems facing the
CubeSat, and then begin payload selection, integration, and implementation.
37
6 Appendix A: Control Loop Code
6.1 Original Proportional Controller Code #include <Wire.h>
#include <L3G.h>
#include <Servo.h>
L3G gyro;
Servo motor_1;
Servo motor_2;
Servo motor_3;
//Gyro stuff
int sampleNum=500;
int Xdc_offset=0;
int Ydc_offset=0;
int Zdc_offset=0;
double Xnoise=0;
double Ynoise=0;
double Znoise=0;
unsigned long time;
int sampleTime=10;
double Xrate=0;
double Yrate=0;
double Zrate=0;
double Xprev_rate=0;
double Yprev_rate=0;
double Zprev_rate=0;
double Xangle=0;
double Yangle=0;
double Zangle=0;
double divisor = 100; //92
//Control loop stuff
int aDirection = 10;
int bDirection = 11;
int gDirection = 12;
int aPower = 3;
int bPower = 5;
int gPower = 6;
int i = 0;
int val = 0;
double A = 2.1582;
double B = 2.2264;
double C = 2.3307;
double Ixx = 0.110038;
double Iyy = 0.121459;
double Izz = 0.112368;
double oldTime = 0;
double newTime = 0;
double dt = 0;
38
double oldXrate = 0;
double newXrate = 0;
double oldYrate = 0;
double newYrate = 0;
double oldZrate = 0;
double newZrate = 0;
double Xaccel = 0;
double Yaccel = 0;
double Zaccel = 0;
double Mx = 0;
double My = 0;
double Mz = 0;
signed long ox = 0;
signed long oy = 0;
signed long oz = 0;
int Xpower = 0;
int Ypower = 0;
int Zpower = 0;
int maxPow = 65;
int Xint = 1100;
int Yint = 1100;
int Zint = 1100;
void setup()
//Gyro stuff
Serial.begin(111111);
Wire.begin();
if (!gyro.init())
Serial.println("Failed to autodetect gyro
type!");
while(1);
gyro.enableDefault();
//Calculate initial DC offset and noise level of
gyro
for(int n=0;n<sampleNum;n++)
gyro.read();
Xdc_offset+=(int)gyro.g.x;
Ydc_offset+=(int)gyro.g.y;
Zdc_offset+=(int)gyro.g.z;
Xdc_offset=Xdc_offset/sampleNum;
Ydc_offset=Ydc_offset/sampleNum;
Zdc_offset=Zdc_offset/sampleNum;.
for(int n=0;n<sampleNum;n++)
gyro.read();
if((int)gyro.g.x-Xdc_offset>Xnoise)
39
Xnoise=(int)gyro.g.x-Xdc_offset;
else if((int)gyro.g.x-Xdc_offset<-Xnoise)
Xnoise=-(int)gyro.g.x-Xdc_offset;
if((int)gyro.g.y-Ydc_offset>Ynoise)
Ynoise=(int)gyro.g.y-Ydc_offset;
else if((int)gyro.g.y-Ydc_offset<-Ynoise)
Ynoise=-(int)gyro.g.y-Ydc_offset;
if((int)gyro.g.z-Zdc_offset>Znoise)
Znoise=(int)gyro.g.z-Zdc_offset;
else if((int)gyro.g.z-Zdc_offset<-Znoise)
Znoise=-(int)gyro.g.z-Zdc_offset;
Xnoise=Xnoise/divisor;
Ynoise=Ynoise/divisor;
Znoise=Znoise/divisor; //gyro returns
hundredths of degrees/sec
//Print DC offset and noise level
Serial.println("***************************
***************************************
***");
Serial.println();
Serial.print("X DC Offset: ");
Serial.print(Xdc_offset);
Serial.print(" \tX Noise Level: ");
Serial.print(Xnoise);
Serial.println();
Serial.print("Y DC Offset: ");
Serial.print(Ydc_offset);
Serial.print(" \tY Noise Level: ");
Serial.print(Ynoise);
Serial.println();
Serial.print("Z DC Offset: ");
Serial.print(Zdc_offset);
Serial.print(" \tZ Noise Level: ");
Serial.print("\t");
Serial.print(Znoise);
Serial.println();
//
Serial.println("Time\tXang\tYang\tZang\tXrat\t
Yrat\tZrat\tXacc\tYacc\tZacc\tXpow\tYpow\tZp
ow");
Serial.flush();
//Control loop stuff
pinMode(aDirection, OUTPUT);
pinMode(bDirection, OUTPUT);
pinMode(gDirection, OUTPUT);
motor_1.attach(aPower);
motor_2.attach(bPower);
motor_3.attach(gPower);
for(i;i<4000;i++)
motor_1.write(val);
motor_2.write(val);
motor_3.write(val);
delay(1);
40
void loop()
//Gyro stuff
// Every "sampleTime" ms take a sample from
the gyro
if(millis()-time>sampleTime)
time=millis(); // Update the time to get the
next sample
gyro.read();
Xrate=((int)gyro.g.x-Xdc_offset)/divisor;
Yrate=((int)gyro.g.y-Ydc_offset)/divisor;
Zrate=((int)gyro.g.z-Zdc_offset)/divisor;
Xrate -= 1.395;
Yrate -= 1.235;
Xangle+=((double)(Xprev_rate+Xrate)*sampleT
ime)/2000;
Yangle+=((double)(Yprev_rate+Yrate)*sampleT
ime)/2000;
Zangle+=((double)(Zprev_rate+Zrate)*sampleTi
me)/2000;
//Remember the current speed for the next
loop rate integration
Xprev_rate=Xrate;
Yprev_rate=Yrate;
Zprev_rate=Zrate;
//Keep our angle between 0-359 degrees
if(Xangle<0)
Xangle+=360;
else if(Xangle>=360)
Xangle-=360;
if(Yangle<0)
Yangle+=360;
else if(Yangle>=360)
Yangle-=360;
if(Zangle<0)
Zangle+=360;
else if(Zangle>=360)
Zangle-=360;
oldTime = newTime;
newTime = millis()/1000.0;
dt = newTime-oldTime;
oldXrate = newXrate;
newXrate = Xrate;
oldYrate = newYrate;
newYrate = Yrate;
oldZrate = newZrate;
newZrate = Zrate;
41
// units: deg/s^2
Xaccel = (newXrate-oldXrate)/dt;
Yaccel = (newYrate-oldYrate)/dt;
Zaccel = (newZrate-oldZrate)/dt;
// units: lb-in
Mx = A*Xaccel+(C-B)*Yrate*Zrate;
My = B*Yaccel+(A-C)*Yrate*Xrate;
Mz = C*Zaccel+(B-A)*Xrate*Yrate;
//units: deg/s
ox = (Mx*newTime/Ixx)+Xint;
oy = (My*newTime/Iyy)+Yint;
oz = (Mz*newTime/Izz)+Zint;
Xpower = 1*pow(10,-27)*pow(ox,6)-
2*pow(10,-22)*pow(ox,5)+1*pow(10,-
17)*pow(ox,4)+3*pow(10,-14)*pow(ox,3)-
2*pow(10,-8)*pow(ox,2)+0.0008*ox+4;
Ypower = 1*pow(10,-27)*pow(oy,6)-
2*pow(10,-22)*pow(oy,5)+1*pow(10,-
17)*pow(oy,4)+3*pow(10,-14)*pow(oy,3)-
2*pow(10,-8)*pow(oy,2)+0.0008*oy+4;
Zpower = 1*pow(10,-27)*pow(oz,6)-
2*pow(10,-22)*pow(oz,5)+1*pow(10,-
17)*pow(oz,4)+3*pow(10,-14)*pow(oz,3)-
2*pow(10,-8)*pow(oz,2)+0.0008*oz+4;
Xpower = abs(Xpower);
Ypower = abs(Ypower);
Zpower = abs(Zpower);
if (Xpower > maxPow)
Xpower = maxPow;
else if (Xpower < 7)
if(Xpower < 4)
Xpower = 4;
else
Xpower = 7;
if (Ypower > maxPow)
Ypower = maxPow;
else if (Ypower < 7)
if(Ypower < 4)
Ypower = 4;
else
Ypower = 7;
if (Zpower > maxPow)
Zpower = maxPow;
42
else if (Zpower < 7)
if(Zpower < 4)
Zpower = 4;
else
Zpower = 7;
// Serial.print(time);
// Serial.print("\t");
// Serial.print(Xangle);
// Serial.print("\t");
// Serial.print(Yangle);
// Serial.print("\t");
// Serial.print(Zangle);
// Serial.print("\t");
Serial.print(Xrate);
Serial.print("\t");
Serial.print(Yrate);
Serial.print("\t");
Serial.print(Zrate);
// Serial.print("\t");
// Serial.print(Xaccel);
// Serial.print("\t");
// Serial.print(Yaccel);
// Serial.print("\t");
// Serial.print(Zaccel);
// Serial.print("\t");
// Serial.print(Xpower);
// Serial.print("\t");
// Serial.print(Ypower);
// Serial.print("\t");
// Serial.print(Zpower);
// Serial.print("\t");
Serial.println();
Serial.flush();
if(abs(Xrate) > 3)
aChangeDirection(Xrate);
if(abs(Xrate) > 3)
bChangeDirection(Yrate);
if(abs(Xrate) > 3)
gChangeDirection(Zrate);
motor_1.write(Xpower);
motor_2.write(Ypower);
motor_3.write(Zpower);
43
void aChangeDirection(double a)
if (a<0)
digitalWrite(aDirection, HIGH);
else
digitalWrite(aDirection, LOW);
void bChangeDirection(double b)
if (b<0)
digitalWrite(bDirection, HIGH);
else
digitalWrite(bDirection, LOW);
void gChangeDirection(double g)
if (g<0)
digitalWrite(gDirection, HIGH);
else
digitalWrite(gDirection, LOW);
44
6.2 PID Control code, final iteration #include <Temboo.h>
int SENSOR_SIGN[9] = 1,1,1,-1,-1,-1,1,1,1;
//Correct directions x,y,z - gyro, accelerometer,
magnetometer
#include <Wire.h>
#include <L3G.h>
#include <LSM303.h>
#include <Servo.h>
Servo motor_1;
Servo motor_2;
Servo motor_3;
// LSM303 accelerometer: 8 g sensitivity
// 3.9 mg/digit; 1 g = 256
#define GRAVITY 256 //this equivalent to 1G
in the raw data coming from the accelerometer
#define ToRad(x) ((x)*0.01745329252) //
*pi/180
#define ToDeg(x) ((x)*57.2957795131) //
*180/pi
// L3G4200D gyro: 2000 dps full scale
// 70 mdps/digit; 1 dps = 0.07
#define Gyro_Gain_X 0.07 //X axis Gyro gain
#define Gyro_Gain_Y 0.07 //Y axis Gyro gain
#define Gyro_Gain_Z 0.07 //Z axis Gyro gain
#define Gyro_Scaled_X(x)
((x)*ToRad(Gyro_Gain_X)) //Return the scaled
ADC raw data of the gyro in radians for second
#define Gyro_Scaled_Y(x)
((x)*ToRad(Gyro_Gain_Y)) //Return the scaled
ADC raw data of the gyro in radians for second
#define Gyro_Scaled_Z(x)
((x)*ToRad(Gyro_Gain_Z)) //Return the scaled
ADC raw data of the gyro in radians for second
// LSM303 magnetometer calibration constants;
use the Calibrate example from
// the Pololu LSM303 library to find the right
values for your board
#define M_X_MIN -421
#define M_Y_MIN -639
#define M_Z_MIN -238
#define M_X_MAX 424
#define M_Y_MAX 295
#define M_Z_MAX 472
#define Kp_ROLLPITCH 0.02
#define Ki_ROLLPITCH 0.00002
#define Kp_YAW -.003
#define Ki_YAW 0.000002
#define Ki 0
#define Kp 0.2
#define Kd 0
#define N 1
/*For debugging purposes*/
//OUTPUTMODE=1 will print the corrected
data,
//OUTPUTMODE=0 will print uncorrected data
of the gyros (with drift)
#define OUTPUTMODE 1
//#define PRINT_DCM 0 //Will print the
whole direction cosine matrix
#define PRINT_ANALOGS 0 //Will print the
analog raw data
#define PRINT_EULER 1 //Will print the
Euler angles Roll, Pitch and Yaw
45
#define STATUS_LED 13
float G_Dt=0.02; // Integration time (DCM
algorithm) We will run the integration loop at
50Hz if possible
long timer=0; //general purpuse timer
long timer_old;
long timer24=0; //Second timer used to print
values
int AN[6]; //array that stores the gyro and
accelerometer data
int AN_OFFSET[6]=0,0,0,0,0,0; //Array that
stores the Offset of the sensors
int gyro_x;
int gyro_y;
int gyro_z;
int accel_x;
int accel_y;
int accel_z;
int magnetom_x;
int magnetom_y;
int magnetom_z;
float c_magnetom_x;
float c_magnetom_y;
float c_magnetom_z;
float MAG_Heading;
int oldroll = 0;
int oldpitch = 0;
int oldyaw = 0;
float Accel_Vector[3]= 0,0,0; //Store the
acceleration in a vector
float Gyro_Vector[3]= 0,0,0;//Store the gyros
turn rate in a vector
float Omega_Vector[3]= 0,0,0; //Corrected
Gyro_Vector data
float Omega_P[3]= 0,0,0;//Omega
Proportional correction
float Omega_I[3]= 0,0,0;//Omega Integrator
float Omega[3]= 0,0,0;
// Euler angles
float roll;
float pitch;
float yaw;
float Xrate;
float Yrate;
float Zrate;
float oldXrate;
float oldYrate;
float oldZrate;
float commandX = roll;
float commandY = pitch;
float commandZ = 0;
float errorRollPitch[3]= 0,0,0;
float errorYaw[3]= 0,0,0;
unsigned int counter=0;
byte gyro_sat=0;
float DCM_Matrix[3][3]=
1,0,0
,
0,1,0
,
0,0,1
46
;
float
Update_Matrix[3][3]=0,1,2,3,4,5,6,7,8;
//Gyros here
float Temporary_Matrix[3][3]=
0,0,0
,
0,0,0
,
0,0,0
;
int aDirection = 10;
int bDirection = 11;
int gDirection = 12;
float errorsum = 0;
int aPower = 3;
int bPower = 5;
int gPower = 6;
int i = 0;
int val = 0;
double A = 2.1582;
double B = 2.2264;
double C = 2.3307;
double Ixx = 0.110038;
double Iyy = 0.121459;
double Izz = 0.112368;
double Xaccel = 0;
double Yaccel = 0;
double Zaccel = 0;
double Mx = 0;
double My = 0;
double Mz = 0;
signed long ox = 0;
signed long oy = 0;
signed long oz = 0;
int Xpower = 0;
int Ypower = 0;
int Zpower = 0;
int maxPow = 65;
int Xint = 1100;
int Yint = 1100;
int Zint = 1100;
float Xerror = 0;
float Yerror = 0;
float Zerror = 0;
float oldXerror=0;
float oldYerror=0;
float oldZerror=0;
float errorsumX = 0;
float errorsumY = 0;
float errorsumZ = 0;
float PX =0;
float PY =0;
float PZ =0;
float IX =0;
float IY =0;
float IZ =0;
float Dx =0;
float Dy =0;
47
float Dz =0;
void setup()
Serial.begin(9600);
pinMode (STATUS_LED,OUTPUT); // Status
LED
I2C_Init();
// Serial.println("Pololu MinIMU-9 + Arduino
AHRS");
digitalWrite(STATUS_LED,LOW);
delay(1500);
Accel_Init();
Compass_Init();
Gyro_Init();
delay(20);
for(int i=0;i<32;i++) // We take some
readings...
Read_Gyro();
Read_Accel();
for(int y=0; y<6; y++) // Cumulate values
AN_OFFSET[y] += AN[y];
delay(20);
for(int y=0; y<6; y++)
AN_OFFSET[y] = AN_OFFSET[y]/32;
// AN_OFFSET[5]-
=GRAVITY*SENSOR_SIGN[5];
// AN_OFFSET[6]-=256;
for(int y=0; y<6; y++)
// Serial.println(AN_OFFSET[y]);
delay(2000);
digitalWrite(STATUS_LED,HIGH);
timer=millis();
delay(20);
counter=0;
pinMode(aDirection, OUTPUT);
pinMode(bDirection, OUTPUT);
pinMode(gDirection, OUTPUT);
motor_1.attach(aPower);
motor_2.attach(bPower);
motor_3.attach(gPower);
for(i;i<4000;i++)
motor_1.write(val);
motor_2.write(val);
motor_3.write(val);
delay(1);
void loop() //Main Loop
if((millis()-timer)>=25) // Main loop runs at
50Hz
counter++;
timer_old = timer;
timer=millis();
if (timer>timer_old)
G_Dt = (timer-timer_old)/1000.0; // Real
time of loop run. We use this on the DCM
algorithm (gyro integration time)
else
48
G_Dt = 0;
// *** DCM algorithm
// Data adquisition
Read_Gyro(); // This read gyro data
Read_Accel(); // Read I2C accelerometer
if (counter > 5) // Read compass data at
10Hz... (5 loop runs)
counter=0;
Read_Compass(); // Read I2C
magnetometer
Compass_Heading(); // Calculate magnetic
heading
// Calculations...
Matrix_update();
Normalize();
Drift_correction();
Euler_angles();
// ***
/*
Omega is rate
Accel_Vector is translational acceleration
Euler angles pitch roll and yaw are heading
*/
// printdata();
roll = ToDeg(roll);
pitch = ToDeg(pitch);
yaw = ToDeg(yaw);
// Xrate= (pitch - oldpitch)*.05;
// Yrate= (yaw - oldyaw)*.05;
// Zrate= (roll - oldroll)*.05;
commandX = roll;
commandY = pitch;
commandZ = 0;
Xrate= gyro_x*.07;
Yrate= gyro_y*.07;
Zrate= gyro_z*.07;
//de-tumble mode
if (abs(Xrate) > 3 || abs(Yrate) > 3 ||
abs(Zrate) > 3)
// Serial.print(Xrate);
// Serial.print(",");
// Serial.print(Yrate);
// Serial.print(",");
// Serial.print(Zrate);
// Serial.print(",");
// Serial.print("detumble");
// Serial.println();
motor_1.write(maxPow);
motor_2.write(maxPow);
motor_3.write(maxPow);
//pointing mode
if (abs(Xrate) < 3 && (Yrate) < 3 && (Zrate)
< 3)
// Serial.print();
49
// Serial.print(",");
// Serial.print(Yrate);
// Serial.print(",");
// Serial.print(Zrate);
// Serial.print(",");
// Serial.print("point");
// Serial.println();
Xerror = roll - commandX;
Yerror = pitch - commandY;
Zerror = yaw -commandZ;
PX = Xerror* Kp;
PY = Yerror*Kp;
PZ = Zerror*Kp;
errorsumX += Xerror;
errorsumY += Yerror;
errorsumZ += Zerror;
IX = (errorsumX*.02)*Ki;
IY = (errorsumY*.02)*Ki;
IZ = (errorsumZ*.02)*Ki;
Dx = (Xerror-oldXerror)*.02*Kd;
Dy = (Yerror-oldYerror)*.02*Kd;
Dz = (Zerror-oldZerror)*.02*Kd;
ox = IX*(1/Xerror) +
Dx*(N/(1+N*(1/Xerror))) + PX;
oy = IY*(1/Yerror) +
Dy*(N/(1+N*(1/Yerror))) + PY;
oz = IZ*(1/Zerror) +
Dy*(N/(1+N*(1/Zerror))) + PZ;
Xpower = ox;
Ypower = oy;
Zpower = oz;
Xpower = abs(Xpower);
Ypower = abs(Ypower);
Zpower = abs(Zpower);
if (Xpower > maxPow)
Xpower = maxPow;
if (Ypower > maxPow)
Ypower = maxPow;
if (Zpower > maxPow)
Zpower = maxPow;
if(abs(Xrate) > .5)
aChangeDirection(ToDeg(roll));
if(abs(Yrate) > 0.5)
bChangeDirection(ToDeg(pitch));
if(abs(Zrate) > 0.5)
gChangeDirection(ToDeg(yaw));
motor_1.write(Xpower);
motor_2.write(Ypower);
motor_3.write(Zpower);
Serial.print(Xpower);
Serial.print(',');
Serial.print(Ypower);
Serial.print(',');
50
Serial.print(Zpower);
Serial.print(',');
Serial.println();
oldroll = roll;
oldpitch = pitch;
oldyaw = yaw;
oldXrate = Xrate;
oldYrate = Yrate;
oldZrate = Zrate;
oldXerror = Xerror;
oldYerror = Yerror;
oldZerror = Zerror;
void aChangeDirection(double a)
if (a>0 && a < 180)
digitalWrite(aDirection, HIGH);
else
digitalWrite(aDirection, LOW);
void bChangeDirection(double b)
if (b>0 && b < 180)
digitalWrite(bDirection, HIGH);
else
digitalWrite(bDirection, LOW);
void gChangeDirection(double g)
if (g>0 && g< 180)
digitalWrite(gDirection, HIGH);
else
digitalWrite(gDirection, LOW);