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);