From 9893658935da75b55a257cbe3ffead9b48a76ab5 Mon Sep 17 00:00:00 2001 From: Brendan McGeeney Date: Mon, 22 Nov 2021 10:52:13 -0700 Subject: [PATCH] maths stuffs --- src/main.cpp | 15 ++++++--------- 1 file changed, 6 insertions(+), 9 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index 8d87ee0..0e890f9 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -37,12 +37,9 @@ void setup() double yaw_pos = (pct / 100) * YAW_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)); pitch.write(90 + pitch_conv(pitch_pos)); - delay(50); + delay(25); } yaw.write(90); @@ -78,8 +75,8 @@ void loop() Serial.println("Moving servos to [" + String(yaw_pos) + ", " + String(pitch_pos) + "]"); - yaw.write(yaw_conv(yaw_pos)); - pitch.write(pitch_conv(pitch_pos)); + yaw.write(90 + yaw_conv(yaw_pos)); + pitch.write(90 + pitch_conv(pitch_pos)); Serial.print("Calculating current height"); for (int i = 0; i < 5; i++) @@ -89,7 +86,7 @@ void loop() } 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) + "\n"); @@ -121,7 +118,7 @@ float yaw_conv(float thetad) { //Serial.println(thetad); float h = .2875; - float H = 1.5; + float H = 1.6; float theta = thetad * 3.14 / 180; @@ -131,7 +128,7 @@ float pitch_conv(float thetad) { //Serial.println(thetad); float h = .2875; - float H = 1.6; + float H = 1.2; float theta = thetad * 3.14 / 180;