Anson/TVC-Test-Code
1
0
Fork 0
mirror of https://gitlab.com/lander-team/tvc-test-code.git synced 2025-06-16 15:06:56 +00:00
Code Releases Activity

code where it was left in class

Browse Source
This commit is contained in:
Anson Biggs 2021-11-19 20:00:05 -07:00
parent c06234ef28
commit c544fc97e2
1 changed files with 30 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
src/main.cpp
View File

@ -11,16 +11,18 @@ double zero_height;
float read_float();
float calc_height(float yaw, float pitch);
float yaw_conv(float theta);
float pitch_conv(float theta);
void setup() {
delay(5000);
yaw.attach(9); // CHANGE MEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEE
pitch.attach(10); // CHANGE MEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEE
yaw.attach(29); // CHANGE MEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEE
pitch.attach(33); // CHANGE MEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEE
Serial.println("Remove Servo Horns and press enter.");
read_float();
yaw.write(0);
pitch.write(0);
yaw.write(yaw_conv(0));
pitch.write(pitch_conv(0));
Serial.println("Replace horns, level the TVC and press enter.");
read_float();
@ -33,8 +35,8 @@ void setup() {
double yaw_pos = pct * YAW_MAX;
double pitch_pos = pct * PITCH_MAX;
yaw.write(yaw_pos);
pitch.write(pitch_pos);
yaw.write(yaw_conv(yaw_pos));
pitch.write(pitch_conv(pitch_pos));
delay(100);
}
@ -65,8 +67,8 @@ void loop() {
Serial.println("Moving servos to [" + String(yaw_pos) + ", " +
String(pitch_pos) + "]");
yaw.write(yaw_pos);
pitch.write(pitch_pos);
yaw.write(yaw_conv(yaw_pos));
pitch.write(pitch_conv(pitch_pos));
Serial.print("Calculating current height");
for (int i = 0; i < 5; i++) {
@ -98,3 +100,23 @@ float read_float() {
}
}
}
float yaw_conv(float thetad) {
Serial.println(thetad);
float h = .2875;
float H = 1.5;
float theta = thetad * 3.14 / 180;
return 2 * asin((H * sin(theta / 2)) / h);
}
float pitch_conv(float thetad) {
Serial.println(thetad);
float h = .2875;
float H = 1.6;
float theta = thetad * 3.14 / 180;
Serial.println(2 * asin((H * sin(theta / 2)) / h));
return 2 * asin((H * sin(theta / 2)) / h);
}