Skip to content

Commit be56223

Browse files
anigokhaleanigokhale
committed
SYSID TEST #1 DONE!
1 parent 45f478c commit be56223

11 files changed

+22945
-30
lines changed

‎SYSID_code/SYSID_code.ino‎

Lines changed: 56 additions & 30 deletions
Original file line numberDiff line numberDiff line change
@@ -18,8 +18,8 @@ int betaPin = 36;
1818
int gammaPin = 29;
1919
int EDFPin = 8;
2020

21-
float vane_min = -12;
22-
float vane_max = 12;
21+
float vane_min = -15;
22+
float vane_max = 15;
2323
float alpha1_0 = 140; // Initial Vane setting in degrees
2424
float alpha2_0 = 145;
2525

@@ -29,7 +29,7 @@ float throttle_min = 0;
2929
float throttle_max = 100;
3030

3131
int delta = high_endpoint - low_endpoint;
32-
int arm_tries = 10;
32+
int arm_tries = 100;
3333
bool startFlag = false;
3434

3535
int segment = 0;
@@ -45,44 +45,49 @@ float forcenm1 = 1e6;
4545
float forcenm2;
4646
float currentAvg;
4747
float lastAvg;
48-
float forceConvergenceThreshold = 0.01;
48+
float forceConvergenceThreshold = 0.03;
4949

5050
float currentTorque = 1e3;
5151
float torquenm1 = 1e6;
5252
float torquenm2;
5353
float currentTorqueAvg;
5454
float lastTorqueAvg;
55-
float torqueConvergenceThreshold = 0.01;
55+
float torqueConvergenceThreshold = 0.03;
5656

5757
double sum = 0;
5858
int count = 0;
5959

6060
void setup() {
6161
// put your setup code here, to run once:
62-
while (!Serial.available()) {
63-
}
6462
FreqMeasure.begin();
6563
Serial.begin(9600);
6664
Serial1.begin(115200);
65+
66+
while (!Serial.available()) {
67+
}
68+
6769
vane1.attach(vane1Pin);
6870
vane2.attach(vane2Pin);
6971
betaServo.attach(betaPin);
7072
gammaServo.attach(gammaPin);
7173
EDF.attach(EDFPin);
7274
vane1.write(alpha1_0);
7375
vane2.write(alpha2_0);
74-
betaServo.write(100);
76+
betaServo.write(90);
7577
gammaServo.write(100);
7678

7779
newestForce = 0;
7880
newestTorque = 0;
7981

82+
delay(15);
8083
EDF.writeMicroseconds(1020);
8184
for (byte i = 0; i < arm_tries; i++) {
8285
EDF.writeMicroseconds(1000);
8386
}
8487

88+
delay(15);
8589
EDF.writeMicroseconds(1020);
90+
delay(5000);
8691

8792
lastSegmentTime = millis() / 1000.0;
8893

@@ -102,35 +107,44 @@ void loop() {
102107
switch (segment) {
103108
case 0:
104109

105-
unitStep(lastSegmentTime, 20, 't');
110+
unitStep(lastSegmentTime, 40, 't');
106111
unitStep(lastSegmentTime, 0, '1');
107112
unitStep(lastSegmentTime, 0, '2');
108113

109-
if (forceConvergence() && torqueConvergence()) {
114+
currentForce = newestForce;
115+
currentTorque = newestTorque;
116+
117+
if ((millis() / 1000.0) > lastSegmentTime + 10) {
110118
segment++;
111119
lastSegmentTime = millis() / 1000.0;
112120
}
113121

114122
break;
115123
case 1:
116124

117-
unitStep(lastSegmentTime, 50, 't');
125+
unitStep(lastSegmentTime, 70, 't');
118126
unitStep(lastSegmentTime, 0, '1');
119127
unitStep(lastSegmentTime, 0, '2');
120128

121-
if (forceConvergence() && torqueConvergence()) {
129+
currentForce = newestForce;
130+
currentTorque = newestTorque;
131+
132+
if ((millis() / 1000.0) > lastSegmentTime + 10) {
122133
segment++;
123134
lastSegmentTime = millis() / 1000.0;
124135
}
125136

126137
break;
127138
case 2:
128139

129-
unitStep(lastSegmentTime, 70, 't');
140+
unitStep(lastSegmentTime, 100, 't');
130141
unitStep(lastSegmentTime, 0, '1');
131142
unitStep(lastSegmentTime, 0, '2');
132143

133-
if (forceConvergence() && torqueConvergence()) {
144+
currentForce = newestForce;
145+
currentTorque = newestTorque;
146+
147+
if ((millis() / 1000.0) > lastSegmentTime + 10) {
134148
segment++;
135149
lastSegmentTime = millis() / 1000.0;
136150
}
@@ -142,7 +156,10 @@ void loop() {
142156
unitStep(lastSegmentTime, 0, '1');
143157
unitStep(lastSegmentTime, 0, '2');
144158

145-
if (forceConvergence() && torqueConvergence()) {
159+
currentForce = newestForce;
160+
currentTorque = newestTorque;
161+
162+
if ((millis() / 1000.0) > lastSegmentTime + 10) {
146163
segment++;
147164
lastSegmentTime = millis() / 1000.0;
148165
}
@@ -153,7 +170,10 @@ void loop() {
153170
unitStep(lastSegmentTime, vane_max, '1');
154171
unitStep(lastSegmentTime, vane_max, '2');
155172

156-
if (forceConvergence() && torqueConvergence()) {
173+
currentForce = newestForce;
174+
currentTorque = newestTorque;
175+
176+
if ((millis() / 1000.0) > lastSegmentTime + 10) {
157177
segment++;
158178
lastSegmentTime = millis() / 1000.0;
159179
}
@@ -163,7 +183,10 @@ void loop() {
163183
unitStep(lastSegmentTime, vane_min, '1');
164184
unitStep(lastSegmentTime, vane_min, '2');
165185

166-
if (forceConvergence() && torqueConvergence()) {
186+
currentForce = newestForce;
187+
currentTorque = newestTorque;
188+
189+
if ((millis() / 1000.0) > lastSegmentTime + 10) {
167190
segment++;
168191
lastSegmentTime = millis() / 1000.0;
169192
}
@@ -173,7 +196,10 @@ void loop() {
173196
unitStep(lastSegmentTime, 0, '1');
174197
unitStep(lastSegmentTime, 0, '2');
175198

176-
if (forceConvergence() && torqueConvergence()) {
199+
currentForce = newestForce;
200+
currentTorque = newestTorque;
201+
202+
if ((millis() / 1000.0) > lastSegmentTime + 10) {
177203
segment++;
178204
lastSegmentTime = millis() / 1000.0;
179205
}
@@ -275,23 +301,23 @@ void loop() {
275301
}
276302

277303
Serial.print(segment);
278-
Serial.print("\t");
304+
Serial.print(",\t");
279305
Serial.print(millis() / 1000.0);
280-
Serial.print("\t");
306+
Serial.print(",\t");
281307
Serial.print(throttle_command);
282-
Serial.print("\t");
308+
Serial.print(",\t");
283309
Serial.print(vane_command);
284-
Serial.print("\t");
285-
Serial.print(currentForce);
286-
Serial.print("\t");
287-
Serial.print(currentTorque);
288-
Serial.print("\t");
310+
Serial.print(",\t");
311+
Serial.print(currentForce, 5);
312+
Serial.print(",\t");
313+
Serial.print(currentTorque, 5);
314+
Serial.print(",\t");
289315

290316
if (FreqMeasure.available()) {
291317
// average several reading together
292318
sum = sum + FreqMeasure.read();
293319
count = count + 1;
294-
if (count > 30) {
320+
if (count > 10) {
295321
float frequency = 60 / 2 * FreqMeasure.countToFrequency(sum / count);
296322
Serial.print(frequency);
297323
sum = 0;
@@ -312,7 +338,7 @@ void loop() {
312338
}
313339
}
314340

315-
bool forceConvergence() {
341+
bool forceConvergence(char id) {
316342
forcenm2 = forcenm1;
317343
forcenm1 = currentForce;
318344
currentForce = newestForce;
@@ -344,8 +370,8 @@ void unitStep(float t, float end, char identifier) {
344370

345371
void sineInput(float t, float angularRate, char identifier) {
346372
if (identifier == 't') {
347-
int pwm_out = (int)(((25 + 25 * sin((t - lastSegmentTime) * angularRate) - throttle_min) / (throttle_max - throttle_min)) * (float)(high_endpoint - low_endpoint) + low_endpoint);
348-
throttle_command = 25 + 25 * sin((t - lastSegmentTime) * angularRate);
373+
int pwm_out = (int)(((50 + 50 * sin((t - lastSegmentTime) * angularRate) - throttle_min) / (throttle_max - throttle_min)) * (float)(high_endpoint - low_endpoint) + low_endpoint);
374+
throttle_command = 50 + 50 * sin((t - lastSegmentTime) * angularRate);
349375
EDF.writeMicroseconds(pwm_out);
350376
} else if (identifier == '1') {
351377
vane1.write(alpha1_0 + vane_max * sin((t - lastSegmentTime) * angularRate));

0 commit comments

Comments
 (0)