Skip to content

Commit a5fefbb

Browse files
micahpearlmanmicahpearlman
committed
working roll pitch. no yaw yet.
1 parent 84722a0 commit a5fefbb

File tree

8 files changed

+466
-55
lines changed

8 files changed

+466
-55
lines changed

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

Lines changed: 166 additions & 49 deletions
Original file line numberDiff line numberDiff line change
@@ -29,40 +29,98 @@ public Rigidbody BaseRigidBody {
2929
get => _baseRigidBody;
3030
}
3131

32+
public ZOIMU _imu;
33+
public ZOIMU IMU {
34+
get => _imu;
35+
}
36+
3237
[Header("Altitude Control")]
3338
public ZOAltimeter _altimeter;
3439
public ZOAltimeter Altimeter {
3540
get => _altimeter;
3641
}
37-
public ZOPIDController _altitudePID = new ZOPIDController {
38-
Kp = 10,
39-
Ki = 1,
40-
Kd = 1,
42+
public ZOPIDController _altitudeHoldPID = new ZOPIDController {
43+
Kp = 50,
44+
Ki = 15,
45+
Kd = 3,
46+
MaximumOutputValue = 1000.0f,
47+
DeadBandEpsilon = 0.0f
48+
};
49+
50+
/// <summary>
51+
/// PID for altitude hold control
52+
/// </summary>
53+
/// <value></value>
54+
public ZOPIDController AltitudeHoldPID {
55+
get => _altitudeHoldPID;
56+
}
57+
58+
public ZOPIDController _altitudeClimbPID = new ZOPIDController {
59+
Kp = 50,
60+
Ki = 15,
61+
Kd = 3,
4162
MaximumOutputValue = 100.0f,
42-
DeadBandEpsilon = 0.01f
63+
DeadBandEpsilon = 0.0f
4364
};
44-
public ZOPIDController AltitudePID {
45-
get => _altitudePID;
65+
66+
public ZOPIDController AltitudeClimbPID {
67+
get => _altitudeClimbPID;
4668
}
4769

48-
private float AltitudeSetPoint {
49-
get {
50-
return AltitudePID.SetPoint;
51-
}
52-
set {
53-
AltitudePID.SetPoint = value;
54-
}
70+
public float _altitudeClimbVelocity = 1.0f;
71+
public float AltitudeClimbVelocity {
72+
get => _altitudeClimbVelocity;
73+
}
74+
75+
public enum AltitudeControlStateEnum {
76+
AltitudeHold,
77+
AltitudeClimb
78+
}
79+
80+
public AltitudeControlStateEnum AltitudeControlState {
81+
get; set;
82+
} = AltitudeControlStateEnum.AltitudeHold;
83+
84+
85+
[Header("Attitude Control")]
86+
public ZOPIDController _rollControlPID = new ZOPIDController {
87+
Kp = 1,
88+
Ki = 0,
89+
Kd = 0.1f,
90+
MaximumOutputValue = 100.0f,
91+
DeadBandEpsilon = 0.0f
92+
};
93+
94+
public ZOPIDController RollControllPID {
95+
get => _rollControlPID;
96+
}
97+
98+
99+
public ZOPIDController _pitchControlPID = new ZOPIDController {
100+
Kp = 1,
101+
Ki = 0,
102+
Kd = 0.1f,
103+
MaximumOutputValue = 100.0f,
104+
DeadBandEpsilon = 0.0f
105+
};
106+
107+
public ZOPIDController PitchControllPID {
108+
get => _pitchControlPID;
55109
}
56110

57-
private float _currentThrottle = 0.0f;
58-
private float _throttleIncrement = 0.1f;
111+
private float _pitchForce = 0.0f;
112+
private float _rollForce = 0.0f;
113+
114+
115+
private float _altitudeForce = 0.0f;
116+
private float _altitudeHeightIncrement = 0.1f;
59117

60118
protected class Motor {
61-
public Vector3 localPosition = new Vector3(0,0,0);
62-
public Vector3 globalPosition = new Vector3(0,0,0);
119+
public Vector3 localPosition = new Vector3(0, 0, 0);
120+
public Vector3 globalPosition = new Vector3(0, 0, 0);
63121

64122
public float force = 0;
65-
public Vector3 globalForceVector = new Vector3(0,0,0);
123+
public Vector3 globalForceVector = new Vector3(0, 0, 0);
66124
}
67125

68126
protected Motor[] _motors = new Motor[4];
@@ -72,77 +130,136 @@ protected Motor[] Motors {
72130

73131
protected override void ZOStart() {
74132
base.ZOStart();
75-
133+
76134
// initialize the motors
77135
for (int i = 0; i < Motors.Length; i++) {
78136
Motors[i] = new Motor();
79137
}
80138

81139
// set the altitude setpoint
82140
if (Altimeter != null) {
83-
AltitudeSetPoint = Altimeter.AltitudeMeters;
141+
AltitudeHoldPID.SetPoint = Altimeter.AltitudeMeters;
84142
}
143+
144+
PitchControllPID.SetPoint = 0;
145+
RollControllPID.SetPoint = 0;
85146
}
86147

87148

88149
protected override void ZOUpdate() {
89150
base.ZOUpdate();
90151

91152
if (Input.GetKeyDown("i")) {
92-
AltitudeSetPoint += _throttleIncrement;
153+
AltitudeHoldPID.SetPoint += _altitudeHeightIncrement;
93154
}
94155
if (Input.GetKeyDown("k")) {
95-
AltitudeSetPoint -= _throttleIncrement;
156+
AltitudeHoldPID.SetPoint -= _altitudeHeightIncrement;
157+
}
158+
if (Input.GetKeyDown("w")) {
159+
PitchControllPID.SetPoint = 10.0f;
160+
}
161+
if (Input.GetKeyDown("x")) {
162+
PitchControllPID.SetPoint = -10.0f;
163+
}
164+
if (Input.GetKeyDown("d")) {
165+
RollControllPID.SetPoint = -10.0f;
166+
}
167+
if (Input.GetKeyDown("a")) {
168+
RollControllPID.SetPoint = 10.0f;
169+
}
170+
171+
if (Input.GetKeyDown("s")) {
172+
PitchControllPID.SetPoint = 0.0f;
173+
RollControllPID.SetPoint = 0.0f;
96174
}
97175

98176
}
99177

100178
protected override void ZOFixedUpdate() {
101179
base.ZOFixedUpdate();
180+
181+
// zero out forces
182+
foreach (Motor motor in Motors) {
183+
motor.globalForceVector = Vector3.zero;
184+
}
185+
102186
if (QuadCopterConfiguration == ZOQuadCopterMotorConfiguration.XConfiguration) {
103187

104188
// calculate altitude control
105-
_currentThrottle = AltitudePID.Update(Altimeter.AltitudeMeters, Time.deltaTime);
189+
_altitudeForce = AltitudeHoldPID.Update(Altimeter.AltitudeMeters, Time.deltaTime);
190+
106191
// 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-
192+
Motors[0].globalPosition = BaseRigidBody.transform.TransformPoint(new Vector3(RotorDistanceFromCenter, 0, RotorDistanceFromCenter));
193+
Motors[1].globalPosition = BaseRigidBody.transform.TransformPoint(new Vector3(-RotorDistanceFromCenter, 0, RotorDistanceFromCenter));
194+
Motors[2].globalPosition = BaseRigidBody.transform.TransformPoint(new Vector3(RotorDistanceFromCenter, 0, -RotorDistanceFromCenter));
195+
Motors[3].globalPosition = BaseRigidBody.transform.TransformPoint(new Vector3(-RotorDistanceFromCenter, 0, -RotorDistanceFromCenter)); ;
196+
197+
198+
Vector3 forwardRightForce = BaseRigidBody.transform.up * _altitudeForce;
199+
113200
// 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;
201+
Vector3 forwardLeftForce = BaseRigidBody.transform.up * _altitudeForce;
119202

120203
// 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;
204+
Vector3 backRightForce = BaseRigidBody.transform.up * _altitudeForce;
126205

127206
// 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;
207+
Vector3 backLeftForce = BaseRigidBody.transform.up * _altitudeForce;
208+
209+
Motors[0].globalForceVector += forwardRightForce;
210+
Motors[1].globalForceVector += forwardLeftForce;
211+
Motors[2].globalForceVector += backRightForce;
212+
Motors[3].globalForceVector += backLeftForce;
213+
214+
// pitch control
215+
_pitchForce = PitchControllPID.Update(IMU.OrientationEulerDegrees.x, Time.deltaTime);
216+
forwardRightForce = BaseRigidBody.transform.up * -_pitchForce;
217+
forwardLeftForce = BaseRigidBody.transform.up * -_pitchForce;
218+
backRightForce = BaseRigidBody.transform.up * _pitchForce;
219+
backLeftForce = BaseRigidBody.transform.up * _pitchForce;
220+
221+
Motors[0].globalForceVector += forwardRightForce;
222+
Motors[1].globalForceVector += forwardLeftForce;
223+
Motors[2].globalForceVector += backRightForce;
224+
Motors[3].globalForceVector += backLeftForce;
225+
226+
227+
// roll control
228+
_rollForce = RollControllPID.Update(IMU.OrientationEulerDegrees.z, Time.deltaTime);
229+
forwardRightForce = BaseRigidBody.transform.up * _rollForce;
230+
forwardLeftForce = BaseRigidBody.transform.up * -_rollForce;
231+
backRightForce = BaseRigidBody.transform.up * _rollForce;
232+
backLeftForce = BaseRigidBody.transform.up * -_rollForce;
233+
234+
Motors[0].globalForceVector += forwardRightForce;
235+
Motors[1].globalForceVector += forwardLeftForce;
236+
Motors[2].globalForceVector += backRightForce;
237+
Motors[3].globalForceVector += backLeftForce;
238+
239+
133240

134241
} else if (QuadCopterConfiguration == ZOQuadCopterMotorConfiguration.CrossConfiguration) {
135242
//TODO:
136243
}
137244

245+
// apply all the forces
246+
foreach (Motor motor in Motors) {
247+
BaseRigidBody.AddForceAtPosition(motor.globalForceVector, motor.globalPosition);
248+
}
249+
138250
}
139251

140-
252+
141253

142254
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")}");
255+
int y = 10;
256+
int yInc = 21;
257+
GUI.TextField(new Rect(10, y += yInc, 300, 22), $"Altitude Set Point: {AltitudeHoldPID.SetPoint.ToString("n2")}");
258+
GUI.TextField(new Rect(10, y += yInc, 300, 22), $"Altitude: {Altimeter.AltitudeMeters.ToString("n2")}");
259+
GUI.TextField(new Rect(10, y += yInc, 300, 22), $"Altitude Throttle: {_altitudeForce.ToString("n2")}");
260+
261+
GUI.TextField(new Rect(10, y += yInc, 300, 22), $"Orientation: {IMU.OrientationEulerDegrees.x.ToString("n2")} {IMU.OrientationEulerDegrees.y.ToString("n2")} {IMU.OrientationEulerDegrees.z.ToString("n2")}");
262+
GUI.TextField(new Rect(10, y += yInc, 300, 22), $"Pitch Force: {_pitchForce.ToString("n2")}");
146263

147264
foreach (Motor motor in Motors) {
148265
Debug.DrawRay(motor.globalPosition, motor.globalForceVector, Color.green, 0.1f);

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

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -99,6 +99,14 @@ public Quaternion Orientation {
9999
get { return _orientation; }
100100
}
101101

102+
/// <summary>
103+
/// Get orientation in euler degrees.
104+
/// </summary>
105+
/// <value></value>
106+
public Vector3 OrientationEulerDegrees {
107+
get { return Orientation.ToEuler() * Mathf.Rad2Deg; }
108+
}
109+
102110

103111
#region ZOGameObjectBase
104112

‎Samples~/ZeroSimSamples/Assets/Materials/Prototype_512x512_Blue1_scaled100.mat‎

Lines changed: 83 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,83 @@
1+
%YAML 1.1
2+
%TAG !u! tag:unity3d.com,2011:
3+
--- !u!21 &2100000
4+
Material:
5+
serializedVersion: 6
6+
m_ObjectHideFlags: 0
7+
m_CorrespondingSourceObject: {fileID: 0}
8+
m_PrefabInstance: {fileID: 0}
9+
m_PrefabAsset: {fileID: 0}
10+
m_Name: Prototype_512x512_Blue1_scaled100
11+
m_Shader: {fileID: 45, guid: 0000000000000000f000000000000000, type: 0}
12+
m_ShaderKeywords:
13+
m_LightmapFlags: 4
14+
m_EnableInstancingVariants: 0
15+
m_DoubleSidedGI: 0
16+
m_CustomRenderQueue: -1
17+
stringTagMap: {}
18+
disabledShaderPasses: []
19+
m_SavedProperties:
20+
serializedVersion: 3
21+
m_TexEnvs:
22+
- _BumpMap:
23+
m_Texture: {fileID: 0}
24+
m_Scale: {x: 1, y: 1}
25+
m_Offset: {x: 0, y: 0}
26+
- _DetailAlbedoMap:
27+
m_Texture: {fileID: 0}
28+
m_Scale: {x: 0.1, y: 0.1}
29+
m_Offset: {x: 0, y: 0}
30+
- _DetailMask:
31+
m_Texture: {fileID: 0}
32+
m_Scale: {x: 1, y: 1}
33+
m_Offset: {x: 0, y: 0}
34+
- _DetailNormalMap:
35+
m_Texture: {fileID: 0}
36+
m_Scale: {x: 1, y: 1}
37+
m_Offset: {x: 0, y: 0}
38+
- _EmissionMap:
39+
m_Texture: {fileID: 0}
40+
m_Scale: {x: 1000, y: 1000}
41+
m_Offset: {x: 0, y: 0}
42+
- _MainTex:
43+
m_Texture: {fileID: 2800000, guid: c1be0c964cfb40142ad9401ffbd251b6, type: 3}
44+
m_Scale: {x: 1000, y: 1000}
45+
m_Offset: {x: 0, y: 0}
46+
- _MetallicGlossMap:
47+
m_Texture: {fileID: 0}
48+
m_Scale: {x: 1, y: 1}
49+
m_Offset: {x: 0, y: 0}
50+
- _OcclusionMap:
51+
m_Texture: {fileID: 0}
52+
m_Scale: {x: 1, y: 1}
53+
m_Offset: {x: 0, y: 0}
54+
- _ParallaxMap:
55+
m_Texture: {fileID: 0}
56+
m_Scale: {x: 1, y: 1}
57+
m_Offset: {x: 0, y: 0}
58+
- _SpecGlossMap:
59+
m_Texture: {fileID: 0}
60+
m_Scale: {x: 1, y: 1}
61+
m_Offset: {x: 0, y: 0}
62+
m_Floats:
63+
- _BumpScale: 1
64+
- _Cutoff: 0.5
65+
- _DetailNormalMapScale: 1
66+
- _DstBlend: 0
67+
- _GlossMapScale: 1
68+
- _Glossiness: 0.5
69+
- _GlossyReflections: 1
70+
- _Metallic: 0
71+
- _Mode: 0
72+
- _OcclusionStrength: 1
73+
- _Parallax: 0.02
74+
- _SmoothnessTextureChannel: 0
75+
- _SpecularHighlights: 1
76+
- _SrcBlend: 1
77+
- _UVSec: 0
78+
- _ZWrite: 1
79+
m_Colors:
80+
- _Color: {r: 1, g: 1, b: 1, a: 1}
81+
- _EmissionColor: {r: 0, g: 0, b: 0, a: 1}
82+
- _SpecColor: {r: 0.19999996, g: 0.19999996, b: 0.19999996, a: 1}
83+
m_BuildTextureStacks: []

‎Samples~/ZeroSimSamples/Assets/Materials/Prototype_512x512_Blue1_scaled100.mat.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.

0 commit comments

Comments
 (0)