-
Notifications
You must be signed in to change notification settings - Fork 0
/
main.cpp
141 lines (106 loc) · 3.34 KB
/
main.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
//
// main.cpp
// QuadcopterControl
//
// Created by Michael Chen on 9/9/15.
// Copyright (c) 2015 Michael Chen. All rights reserved.
//
#include <iostream>
#include "quadcopter_sim.h"
#include "quadcopter.h"
using namespace std;
int main ()
{
float Height_Kp = 3;
float Height_Ki = 1;
float Height_Kd = 1;
float Pitch_Kp = 0.3;
float Pitch_Ki = 0.1;
float Pitch_Kd = 0.1;
float Roll_Kp = 0.3;
float Roll_Ki = 0.1;
float Roll_Kd = 0.1;
float initialPitch = 10;
float initialRoll = 10;
float initialHeight = 0;
qc_init(initialPitch, initialRoll, initialHeight);
//--------------------------
int desiredHeight = 100;
int desiredPitch = 0;
int desiredRoll = 0;
//--------------------------
int cumulativeErrorHeight = 0;
int errorHeightChange = 0;
int previousErrorHeight = 0;
int cumulativeErrorPitch = 0;
int errorPitchChange = 0;
int previousErrorPitch = 0;
int cumulativeErrorRoll = 0;
int errorRollChange = 0;
int previousErrorRoll = 0;
cout << "trial | height Pitch Roll | Height_Change P_Change R_Change | rotorFL rotorFR rotorBL rotorBR\n\n";
for (int loop = 0; loop < 100; loop++)
{
int currentHeight = getHeight();
int errorHeight = desiredHeight - currentHeight;
cumulativeErrorHeight += errorHeight;
errorHeightChange = errorHeight - previousErrorHeight;
float rotorSpeedHeight = Height_Kp * errorHeight
+ Height_Ki * cumulativeErrorHeight
+ Height_Kd * errorHeightChange;
//----------------------------------------------
int currentPitch = getPitch();
int errorPitch = desiredPitch - currentPitch;
cumulativeErrorPitch += errorPitch;
errorPitchChange = errorPitch - previousErrorPitch;
float rotorSpeedPitch = Pitch_Kp * errorPitch
+ Pitch_Ki * cumulativeErrorPitch
+ Pitch_Kd * errorPitchChange;
//----------------------------------------------
int currentRoll = getRoll();
int errorRoll = desiredRoll - currentRoll;
cumulativeErrorRoll += errorRoll;
errorRollChange = errorRoll - previousErrorRoll;
float rotorSpeedRoll = Roll_Kp * errorRoll
+ Roll_Ki * cumulativeErrorRoll
+ Roll_Kd * errorRollChange;
//--------------------------
float rotorFR = rotorSpeedHeight - rotorSpeedPitch - rotorSpeedRoll;
float rotorFL = rotorSpeedHeight - rotorSpeedPitch + rotorSpeedRoll;
float rotorBR = rotorSpeedHeight + rotorSpeedPitch - rotorSpeedRoll;
float rotorBL = rotorSpeedHeight + rotorSpeedPitch + rotorSpeedRoll;
if (rotorFR > 255)
rotorFR = 255;
if (rotorFR < 0)
rotorFR = 0;
if (rotorFL > 255)
rotorFL = 255;
if (rotorFL < 0)
rotorFL = 0;
if (rotorBR > 255)
rotorBR = 255;
if (rotorBR < 0)
rotorBR = 0;
if (rotorBL > 255)
rotorBL = 255;
if (rotorBL < 0)
rotorBL = 0;
setFR(rotorFR);
setFL(rotorFL);
setBR(rotorBR);
setBL(rotorBL);
previousErrorHeight = errorHeight;
previousErrorPitch = errorPitch;
previousErrorRoll = errorRoll;
cout << loop << " | " << currentHeight << " " << currentPitch << " " << currentRoll << " | "
<< rotorSpeedHeight << " "
<< rotorSpeedPitch << " "
<< rotorSpeedRoll << " | "
<< rotorFL << " "
<< rotorFR << " "
<< rotorBL << " "
<< rotorBR <<"\n";
qc_RunSimulationCycle();
}
return 1;
}