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

made sure we were writing to our servos in degrees

Browse Source
This commit is contained in:
Brendan McGeeney 2021-11-21 16:18:40 -07:00
parent ea7841e251
commit 0df5f1b39d
1 changed files with 39 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

64
src/main.cpp
View File

@ -14,15 +14,16 @@ float calc_height(float yaw, float pitch);
float yaw_conv(float theta); float yaw_conv(float theta);
float pitch_conv(float theta); float pitch_conv(float theta);
void setup() { void setup()
{
delay(5000); delay(5000);
yaw.attach(29); // CHANGE MEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEE yaw.attach(29); // CHANGE MEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEE
pitch.attach(33); // CHANGE MEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEE pitch.attach(33); // CHANGE MEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEE
Serial.println("Remove Servo Horns and press enter."); Serial.println("Remove Servo Horns and press enter.");
read_float(); read_float();
yaw.write(yaw_conv(0)); yaw.write(yaw_conv(90));
pitch.write(pitch_conv(0)); pitch.write(pitch_conv(90));
Serial.println("Replace horns, level the TVC and press enter."); Serial.println("Replace horns, level the TVC and press enter.");
read_float(); read_float();
@ -30,14 +31,18 @@ void setup() {
Serial.println("Testing Servos"); Serial.println("Testing Servos");
// Do a full range of movement of the servos // Do a full range of movement of the servos
for (double pct = -100.0; pct <= 100.0; pct += 10.0) { for (double pct = -100.0; pct <= 100.0; pct += 10.0)
{
Serial.print("."); Serial.print(".");
double yaw_pos = pct * YAW_MAX; double yaw_pos = (pct / 100) * YAW_MAX;
double pitch_pos = pct * PITCH_MAX; double pitch_pos = (pct / 100) * PITCH_MAX;
yaw.write(yaw_conv(yaw_pos)); Serial.println(90 + yaw_conv(yaw_pos));
pitch.write(pitch_conv(pitch_pos)); Serial.println(90 + pitch_conv(pitch_pos));
delay(100);
yaw.write(90 + yaw_conv(yaw_pos));
pitch.write(90 + pitch_conv(pitch_pos));
delay(500);
} }
Serial.print("Please input the point height in millimeters: "); Serial.print("Please input the point height in millimeters: ");
@ -45,10 +50,12 @@ void setup() {
delay(1000); delay(1000);
} }
void loop() { void loop()
{
Serial.print("Enter Yaw Coord: "); Serial.print("Enter Yaw Coord: ");
float yaw_pos = read_float(); float yaw_pos = read_float();
if (abs(yaw_pos) > YAW_MAX) { if (abs(yaw_pos) > YAW_MAX)
{
Serial.println("Value too large, defaulting to maximum deflection"); Serial.println("Value too large, defaulting to maximum deflection");
yaw_pos = YAW_MAX; yaw_pos = YAW_MAX;
} }
@ -57,7 +64,8 @@ void loop() {
Serial.print("Enter Pitch Coord: "); Serial.print("Enter Pitch Coord: ");
float pitch_pos = read_float(); float pitch_pos = read_float();
if (abs(pitch_pos) > PITCH_MAX) { if (abs(pitch_pos) > PITCH_MAX)
{
Serial.println("Value too large, defaulting to maximum deflection"); Serial.println("Value too large, defaulting to maximum deflection");
pitch_pos = PITCH_MAX; pitch_pos = PITCH_MAX;
} }
@ -71,7 +79,8 @@ void loop() {
pitch.write(pitch_conv(pitch_pos)); pitch.write(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++)
{
Serial.print("."); Serial.print(".");
delay(i * 100); delay(i * 100);
} }
@ -85,38 +94,43 @@ void loop() {
read_float(); read_float();
} }
float read_float() { float read_float()
{
char line[80]; char line[80];
int count = 0; int count = 0;
while (1) { while (1)
if (Serial.available() > 0) { {
if (Serial.available() > 0)
{
line[count] = (char)Serial.read(); // store the char line[count] = (char)Serial.read(); // store the char
Serial.print(line[count]); Serial.print(line[count]);
if (line[count++] == '\r') { // if its a CR, if (line[count++] == '\r')
line[count] = '\0'; // zero-terminate it { // if its a CR,
line[count] = '\0'; // zero-terminate it
return String(line).toFloat(); return String(line).toFloat();
} }
} }
} }
} }
float yaw_conv(float thetad) { float yaw_conv(float thetad)
Serial.println(thetad); {
//Serial.println(thetad);
float h = .2875; float h = .2875;
float H = 1.5; float H = 1.5;
float theta = thetad * 3.14 / 180; float theta = thetad * 3.14 / 180;
return 2 * asin((H * sin(theta / 2)) / h); return (2 * asin((H * sin(theta / 2)) / h)) * 180.0 / 3.14;
} }
float pitch_conv(float thetad) { float pitch_conv(float thetad)
Serial.println(thetad); {
//Serial.println(thetad);
float h = .2875; float h = .2875;
float H = 1.6; float H = 1.6;
float theta = thetad * 3.14 / 180; float theta = thetad * 3.14 / 180;
Serial.println(2 * asin((H * sin(theta / 2)) / h)); return (2 * asin((H * sin(theta / 2)) / h)) * 180.0 / 3.14;
return 2 * asin((H * sin(theta / 2)) / h);
} }