Anson/Lander-Embedded
1
0
Fork 0
mirror of https://gitlab.com/lander-team/lander-cpp.git synced 2025-07-23 14:41:25 +00:00
Code Releases Activity

intrgrate servos

Browse Source
This commit is contained in:
Anson Biggs
2021-11-22 13:30:53 -07:00
parent 7a523bab6d
commit d4e2d0162e
2 changed files with 44 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files

64
include/teensy.h
View File

@@ -14,6 +14,8 @@ void write2CSV(struct Vehicle &);
void printSimResults(struct Vehicle &);
void teensyAbort();
double loadCellFilter(double current, double previous);
float yaw_conv(float thetad);
float pitch_conv(float thetad);
// Load cell stuff
HX711 lc0;
@@ -42,19 +44,19 @@ double lcGain3 = -118.65;
const int chipSelect = BUILTIN_SDCARD;
File dataFile;
void testGimbal(PWMServo &servo1, PWMServo &servo2) {
void testGimbal(PWMServo &yaw, PWMServo &pitch) {
int servoTest = 90;
servo1.write(servoTest);
servo2.write(servoTest);
yaw.write(yaw_conv(servoTest));
pitch.write(pitch_conv(servoTest));
// Servo 1 Test
for (servoTest = 0; servoTest < 7; servoTest += 1) {
servo1.write(90 + servoTest);
yaw.write(yaw_conv(90 + servoTest));
delay(30);
}
for (servoTest = 7; servoTest >= 1; servoTest -= 1) {
servo1.write(90 + servoTest);
yaw.write(yaw_conv(90 + servoTest));
delay(30);
}
@@ -62,11 +64,11 @@ void testGimbal(PWMServo &servo1, PWMServo &servo2) {
// Servo 2 Test
for (servoTest = 0; servoTest < 7; servoTest += 1) {
servo2.write(90 + servoTest);
pitch.write(pitch_conv(90 + servoTest));
delay(30);
}
for (servoTest = 7; servoTest >= 1; servoTest -= 1) {
servo2.write(90 + servoTest);
pitch.write(pitch_conv(90 + servoTest));
delay(30);
}
@@ -74,20 +76,20 @@ void testGimbal(PWMServo &servo1, PWMServo &servo2) {
// Servo 1 & 2 Test
for (servoTest = 0; servoTest < 7; servoTest += 1) {
servo1.write(90 + servoTest);
servo2.write(90 + servoTest);
yaw.write(yaw_conv(90 + servoTest));
pitch.write(pitch_conv(90 + servoTest));
delay(30);
}
for (servoTest = 7; servoTest >= 1; servoTest -= 1) {
servo1.write(90 + servoTest);
servo2.write(90 + servoTest);
yaw.write(yaw_conv(90 + servoTest));
pitch.write(pitch_conv(90 + servoTest));
delay(30);
}
delay(30);
// BRING IN YAW AND PITCH CONVERSION FROM TVC TEST
servo1.write(90);
servo2.write(90);
yaw.write(yaw_conv(90));
pitch.write(pitch_conv(90));
}
void initLoadCells(Vehicle &State) {
@@ -207,39 +209,32 @@ void thrustInfo(Vehicle &State) {
getThrust(State);
// Constants based on vehicle
double r = 3.0;
double R = 5.0;
// Vector math to aqcuire thrust vector components
// PLACEHOLDER PLACEHOLDERPLACEHOLDER PLACEHOLDERPLACEHOLDER
// PLACEHOLDERPLACEHOLDER PLACEHOLDERPLACEHOLDER PLACEHOLDER
State.thrust = State.lc0_processed + State.lc1_processed +
State.lc2_processed + State.lc3_processed;
State.Fx = (State.lc1_processed - State.lc2_processed) * r / R;
State.Fy = (State.lc0_processed - State.lc3_processed) * r / R;
State.Fz =
sqrt(pow(State.thrust, 2) - pow(State.Fx, 2) - pow(State.Fy, 2)) +
(state.mass * g);
(State.mass * g);
} else {
State.thrust = 0.0;
}
}
void processTVC(Vehicle &State, PWMServo &servo1, PWMServo &servo2) {
void processTVC(Vehicle &State, PWMServo &yaw, PWMServo &pitch) {
State.Fx = State.thrust * sin(State.xServoDegs * (M_PI / 180.0));
State.Fy = State.thrust * sin(State.yServoDegs * (M_PI / 180.0));
State.Fz = sqrt(pow(State.thrust, 2) - pow(State.Fx, 2) - pow(State.Fy, 2)) +
(State.mass * g);
// Calculate moment created by Fx and Fy
State.momentX = State.Fx * State.momentArm;
State.momentY = State.Fy * State.momentArm;
State.momentZ = 0.0;
State.F = sqrt(pow(State.Fx, 2) + pow(State.Fy, 2) + pow(State.Fz, 2));
servo1.write(90 + State.xServoDegs);
servo2.write(90 + State.yServoDegs);
yaw.write(yaw_conv(State.xServoDegs));
pitch.write(pitch_conv(State.yServoDegs));
}
void write2CSV(Vehicle &State) {
@@ -395,4 +390,21 @@ double loadCellFilter(double current, double previous) {
} else {
return current;
}
}
float yaw_conv(float thetad) {
float h = .2875;
float H = 1.6;
float theta = thetad * M_PI / 180.0;
return 90 + (2 * asin((H * sin(theta / 2)) / h)) * 180.0 / M_PI;
}
float pitch_conv(float thetad) {
float h = .2875;
float H = 1.2;
float theta = thetad * M_PI / 180.0;
return 90 + (2 * asin((H * sin(theta / 2)) / h)) * 180.0 / M_PI;
}