Anson/TVC-Test-Code
1
0
Fork 0
mirror of https://gitlab.com/lander-team/tvc-test-code.git synced 2025-08-02 11:31:31 +00:00
Code Releases Activity

maths stuffs

Browse Source
This commit is contained in:
Brendan McGeeney
2021-11-22 10:52:13 -07:00
parent ff8f4cfec3
commit 9893658935
1 changed files with 6 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
src/main.cpp
View File

@@ -37,12 +37,9 @@ void setup()
double yaw_pos = (pct / 100) * YAW_MAX; double yaw_pos = (pct / 100) * YAW_MAX;
double pitch_pos = (pct / 100) * PITCH_MAX; double pitch_pos = (pct / 100) * PITCH_MAX;
Serial.println(yaw_conv(yaw_pos));
Serial.println(pitch_conv(pitch_pos));
yaw.write(90 + yaw_conv(yaw_pos)); yaw.write(90 + yaw_conv(yaw_pos));
pitch.write(90 + pitch_conv(pitch_pos)); pitch.write(90 + pitch_conv(pitch_pos));
delay(50); delay(25);
} }
yaw.write(90); yaw.write(90);
@@ -78,8 +75,8 @@ void loop()
Serial.println("Moving servos to [" + String(yaw_pos) + ", " + Serial.println("Moving servos to [" + String(yaw_pos) + ", " +
String(pitch_pos) + "]"); String(pitch_pos) + "]");
yaw.write(yaw_conv(yaw_pos)); yaw.write(90 + yaw_conv(yaw_pos));
pitch.write(pitch_conv(pitch_pos)); pitch.write(90 + pitch_conv(pitch_pos));
Serial.print("Calculating current height"); Serial.print("Calculating current height");
for (int i = 0; i < 5; i++) for (int i = 0; i < 5; i++)
@@ -89,7 +86,7 @@ void loop()
} }
Serial.println(""); Serial.println("");
float height = sin(yaw_pos) + sin(pitch_pos) + zero_height; float height = 235 * (sin(yaw_pos * 3.14 / 180) + sin(pitch_pos * 3.14 / 180)) + zero_height;
Serial.println("Please verify current height is correct: " + String(height) + Serial.println("Please verify current height is correct: " + String(height) +
"\n"); "\n");
@@ -121,7 +118,7 @@ float yaw_conv(float thetad)
{ {
//Serial.println(thetad); //Serial.println(thetad);
float h = .2875; float h = .2875;
float H = 1.5; float H = 1.6;
float theta = thetad * 3.14 / 180; float theta = thetad * 3.14 / 180;
@@ -131,7 +128,7 @@ float pitch_conv(float thetad)
{ {
//Serial.println(thetad); //Serial.println(thetad);
float h = .2875; float h = .2875;
float H = 1.6; float H = 1.2;
float theta = thetad * 3.14 / 180; float theta = thetad * 3.14 / 180;