Skip to content

Commit 84722a0

Browse files
micahpearlmanmicahpearlman
committed
first pass altitude hold control
1 parent 961c22f commit 84722a0

File tree

7 files changed

+277
-11
lines changed

7 files changed

+277
-11
lines changed

‎README.md‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -25,6 +25,7 @@ ZeroSim provides a multitude of tools for building robots and environments in Un
2525
* IMU -> ROS [Imu](http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/Imu.html) message
2626
* Magnetometer -> ROS [MagneticField](http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/MagneticField.html) message.
2727
* Contact switch
28+
* Altimeter
2829

2930
* Ready to use ROS standard controllers and plugins:
3031
* Differential drive. Controlled via standard ROS [Twist](https://docs.ros.org/en/api/geometry_msgs/html/msg/Twist.html) message.

‎Runtime/Scripts/Controllers/VehicleControllers/Drone/ZOQuadCopterController.cs‎

Lines changed: 132 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -2,24 +2,151 @@
22
using System.Collections.Generic;
33
using UnityEngine;
44
using ZO.Util;
5+
using ZO.Physics;
6+
using ZO.Sensors;
57

68
namespace ZO.Controllers {
79

810
public class ZOQuadCopterController : ZOGameObjectBase {
9-
11+
1012
public enum ZOQuadCopterMotorConfiguration {
1113
XConfiguration,
1214
CrossConfiguration
1315
}
1416

1517
public ZOQuadCopterMotorConfiguration _quadCopterConfiguration = ZOQuadCopterMotorConfiguration.XConfiguration;
16-
// Start is called before the first frame update
17-
void Start() {
18+
public ZOQuadCopterMotorConfiguration QuadCopterConfiguration {
19+
get => _quadCopterConfiguration;
20+
}
21+
public float _rotorDistanceFromCenter = 0.2f;
22+
public float RotorDistanceFromCenter {
23+
get => _rotorDistanceFromCenter;
24+
}
25+
public float _maxPerRotorForce = 50.0f;
26+
27+
public Rigidbody _baseRigidBody = null;
28+
public Rigidbody BaseRigidBody {
29+
get => _baseRigidBody;
30+
}
31+
32+
[Header("Altitude Control")]
33+
public ZOAltimeter _altimeter;
34+
public ZOAltimeter Altimeter {
35+
get => _altimeter;
36+
}
37+
public ZOPIDController _altitudePID = new ZOPIDController {
38+
Kp = 10,
39+
Ki = 1,
40+
Kd = 1,
41+
MaximumOutputValue = 100.0f,
42+
DeadBandEpsilon = 0.01f
43+
};
44+
public ZOPIDController AltitudePID {
45+
get => _altitudePID;
46+
}
1847

48+
private float AltitudeSetPoint {
49+
get {
50+
return AltitudePID.SetPoint;
51+
}
52+
set {
53+
AltitudePID.SetPoint = value;
54+
}
1955
}
2056

21-
// Update is called once per frame
22-
void Update() {
57+
private float _currentThrottle = 0.0f;
58+
private float _throttleIncrement = 0.1f;
59+
60+
protected class Motor {
61+
public Vector3 localPosition = new Vector3(0,0,0);
62+
public Vector3 globalPosition = new Vector3(0,0,0);
63+
64+
public float force = 0;
65+
public Vector3 globalForceVector = new Vector3(0,0,0);
66+
}
67+
68+
protected Motor[] _motors = new Motor[4];
69+
protected Motor[] Motors {
70+
get => _motors;
71+
}
72+
73+
protected override void ZOStart() {
74+
base.ZOStart();
75+
76+
// initialize the motors
77+
for (int i = 0; i < Motors.Length; i++) {
78+
Motors[i] = new Motor();
79+
}
80+
81+
// set the altitude setpoint
82+
if (Altimeter != null) {
83+
AltitudeSetPoint = Altimeter.AltitudeMeters;
84+
}
85+
}
86+
87+
88+
protected override void ZOUpdate() {
89+
base.ZOUpdate();
90+
91+
if (Input.GetKeyDown("i")) {
92+
AltitudeSetPoint += _throttleIncrement;
93+
}
94+
if (Input.GetKeyDown("k")) {
95+
AltitudeSetPoint -= _throttleIncrement;
96+
}
97+
98+
}
99+
100+
protected override void ZOFixedUpdate() {
101+
base.ZOFixedUpdate();
102+
if (QuadCopterConfiguration == ZOQuadCopterMotorConfiguration.XConfiguration) {
103+
104+
// calculate altitude control
105+
_currentThrottle = AltitudePID.Update(Altimeter.AltitudeMeters, Time.deltaTime);
106+
// forward right motor
107+
Vector3 forwardRightPosition = BaseRigidBody.transform.TransformPoint(new Vector3(RotorDistanceFromCenter, 0, RotorDistanceFromCenter));
108+
Vector3 forwardRightForce = BaseRigidBody.transform.up * _currentThrottle;
109+
BaseRigidBody.AddForceAtPosition(forwardRightForce, forwardRightPosition);
110+
Motors[0].globalPosition = forwardRightPosition;
111+
Motors[0].globalForceVector = forwardRightForce;
112+
113+
// forward left motor
114+
Vector3 forwardLeftPosition = BaseRigidBody.transform.TransformPoint(new Vector3(-RotorDistanceFromCenter, 0, RotorDistanceFromCenter));
115+
Vector3 forwardLeftForce = BaseRigidBody.transform.up * _currentThrottle;
116+
BaseRigidBody.AddForceAtPosition(forwardLeftForce, forwardLeftPosition);
117+
Motors[1].globalPosition = forwardLeftPosition;
118+
Motors[1].globalForceVector = forwardLeftForce;
119+
120+
// back right motor
121+
Vector3 backRightPosition = BaseRigidBody.transform.TransformPoint(new Vector3(RotorDistanceFromCenter, 0, -RotorDistanceFromCenter));
122+
Vector3 backRightForce = BaseRigidBody.transform.up * _currentThrottle;
123+
BaseRigidBody.AddForceAtPosition(backRightForce, backRightPosition);
124+
Motors[2].globalPosition = backRightPosition;
125+
Motors[2].globalForceVector = backRightForce;
126+
127+
// back left motor
128+
Vector3 backLeftPosition = BaseRigidBody.transform.TransformPoint(new Vector3(-RotorDistanceFromCenter, 0, -RotorDistanceFromCenter));
129+
Vector3 backLeftForce = BaseRigidBody.transform.up * _currentThrottle;
130+
BaseRigidBody.AddForceAtPosition(backLeftForce, backLeftPosition);
131+
Motors[3].globalPosition = backLeftPosition;
132+
Motors[3].globalForceVector = backLeftForce;
133+
134+
} else if (QuadCopterConfiguration == ZOQuadCopterMotorConfiguration.CrossConfiguration) {
135+
//TODO:
136+
}
137+
138+
}
139+
140+
141+
142+
private void OnGUI() {
143+
GUI.TextField(new Rect(10, 10, 300, 22), $"Altitude Set Point: {AltitudeSetPoint.ToString("R2")}");
144+
GUI.TextField(new Rect(10, 25, 300, 22), $"Altitude: {Altimeter.AltitudeMeters.ToString("R2")}");
145+
GUI.TextField(new Rect(10, 40, 300, 22), $"Altitude Throttle: {_currentThrottle.ToString("R2")}");
146+
147+
foreach (Motor motor in Motors) {
148+
Debug.DrawRay(motor.globalPosition, motor.globalForceVector, Color.green, 0.1f);
149+
}
23150

24151
}
25152
}

‎Runtime/Scripts/Document/ZOSimOccurrence.cs‎

Lines changed: 6 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -52,6 +52,11 @@ public ZOSimDocumentRoot DocumentRoot {
5252
}
5353
}
5454

55+
private void OnValidate() {
56+
// update root component
57+
ZOSimDocumentRoot rootComponent = DocumentRoot;
58+
}
59+
5560
private void Start() {
5661
if (Application.IsPlaying(gameObject) == false) { // In Editor Mode
5762
// update root component
@@ -721,7 +726,7 @@ public JObject Serialize(ZOSimDocumentRoot documentRoot, UnityEngine.Object pare
721726
Debug.LogWarning("WARNING: Trying to serialize visual mesh asset but does not have a mesh: " + visualsChild.name);
722727
}
723728

724-
729+
725730
} else {
726731
Debug.LogWarning("WARNING: asset: " + visualsChild.name + "does not exist in bundle: " + DocumentRoot.AssetBundle.name + " try to add the asset to the asset bundle");
727732
}

‎Runtime/Scripts/ROS/Unity/Publishers/ZOROSIMUPublisher.cs‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -94,7 +94,7 @@ private void Initialize() {
9494
ROSBridgeConnection.Advertise(ROSTopic, _imuMessage.MessageType);
9595

9696
// hookup to the sensor update delegate
97-
IMUSensor.OnPublishDelegate = OnPublishImuDelegate;
97+
IMUSensor.OnPublishDelegate += OnPublishImuDelegate;
9898

9999
}
100100

‎Runtime/Scripts/Sensors/Altimeter.meta‎

Lines changed: 8 additions & 0 deletions
Some generated files are not rendered by default. Learn more about customizing how changed files appear on GitHub.

‎Runtime/Scripts/Sensors/IMU/ZOIMU.cs‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,10 @@
1212

1313
namespace ZO.Sensors {
1414

15+
16+
/// <summary>
17+
/// IMU Sensor: Acceleration + Rotational Velocity.
18+
/// </summary>
1519
[RequireComponent(typeof(Rigidbody))]
1620
public class ZOIMU : ZOGameObjectBase, ZOSerializationInterface {
1721

0 commit comments

Comments
 (0)