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

final code used for VR

Browse Source
This commit is contained in:
Anson Biggs 2021-11-30 17:42:31 -07:00
parent 9893658935
commit 9e8bac67cb
1 changed files with 20 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
src/main.cpp
View File

@ -14,8 +14,7 @@ float calc_height(float yaw, float pitch);
float yaw_conv(float theta);
float pitch_conv(float theta);
void setup()
{
void setup() {
delay(5000);
yaw.attach(29); // CHANGE MEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEE
pitch.attach(33); // CHANGE MEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEE
@ -31,8 +30,7 @@ void setup()
Serial.println("Testing 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(".");
double yaw_pos = (pct / 100) * YAW_MAX;
double pitch_pos = (pct / 100) * PITCH_MAX;
@ -50,12 +48,10 @@ void setup()
delay(1000);
}
void loop()
{
void loop() {
Serial.print("Enter Yaw Coord: ");
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");
yaw_pos = YAW_MAX;
}
@ -64,8 +60,7 @@ void loop()
Serial.print("Enter Pitch Coord: ");
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");
pitch_pos = PITCH_MAX;
}
@ -79,14 +74,15 @@ void loop()
pitch.write(90 + pitch_conv(pitch_pos));
Serial.print("Calculating current height");
for (int i = 0; i < 5; i++)
{
for (int i = 0; i < 5; i++) {
Serial.print(".");
delay(i * 100);
}
Serial.println("");
float height = 235 * (sin(yaw_pos * 3.14 / 180) + sin(pitch_pos * 3.14 / 180)) + 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");
@ -94,43 +90,37 @@ void loop()
read_float();
}
float read_float()
{
float read_float() {
char line[80];
int count = 0;
while (1)
{
if (Serial.available() > 0)
{
while (1) {
if (Serial.available() > 0) {
line[count] = (char)Serial.read(); // store the char
Serial.print(line[count]);
if (line[count++] == '\r')
{ // if its a CR,
line[count] = '\0'; // zero-terminate it
if (line[count++] == '\r') { // if its a CR,
line[count] = '\0'; // zero-terminate it
return String(line).toFloat();
}
}
}
}
float yaw_conv(float thetad)
{
//Serial.println(thetad);
float yaw_conv(float thetad) {
// Serial.println(thetad);
float h = .2875;
float H = 1.6;
float theta = thetad * 3.14 / 180;
return (2 * asin((H * sin(theta / 2)) / h)) * 180.0 / 3.14;
return -(2 * asin((H * sin(theta / 2)) / h)) * 180.0 / 3.14;
}
float pitch_conv(float thetad)
{
//Serial.println(thetad);
float pitch_conv(float thetad) {
// Serial.println(thetad);
float h = .2875;
float H = 1.2;
float theta = thetad * 3.14 / 180;
return (2 * asin((H * sin(theta / 2)) / h)) * 180.0 / 3.14;
return -(2 * asin((H * sin(theta / 2)) / h)) * 180.0 / 3.14;
}