Anson/Lander-Embedded
1
0
Fork 0
mirror of https://gitlab.com/lander-team/lander-cpp.git synced 2025-06-16 07:06:51 +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
Show Stats 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;
}

16
src/main.cpp
View File

@ -26,12 +26,8 @@ unsigned long last, initTime;
#include <HX711.h>
#include <PWMServo.h>
PWMServo servo1;
PWMServo servo2;
const int pin_servo1 = 33;
const int pin_servo2 = 29;
const int pin_igniter = 7;
PWMServo yaw;
PWMServo pitch;
int servoPos = 90; // variable to store the servo position;
@ -48,9 +44,9 @@ void setup() {
initLoadCells(State);
init_Vehicle(State);
servo1.attach(pin_servo1);
servo2.attach(pin_servo2);
testGimbal(servo1, servo2);
yaw.attach(29);
pitch.attach(33);
testGimbal(yaw, pitch);
Serial.println("Servos Tested");
delay(3000);
@ -75,7 +71,7 @@ void loop() {
thrustInfo(State);
pidController(State, PrevState);
TVC(State, PrevState);
processTVC(State, servo1, servo2);
processTVC(State, yaw, pitch);
write2CSV(State);