kopia lustrzana https://github.com/YohanHadji/R2Home
54 wiersze
1.5 KiB
C++
54 wiersze
1.5 KiB
C++
#include <PID_v1.h>
|
|
#include "config.h"
|
|
#include "cmd_cmpt.hpp"
|
|
|
|
int SERVO_MAX_M_W = 0;
|
|
int SERVO_MAX_C_W = 0;
|
|
|
|
int SERVO_MAX_M = 0;
|
|
int SERVO_MAX_C = 0;
|
|
|
|
unsigned long time_gain = 0;
|
|
|
|
void navigation_setup() {
|
|
myPID.SetTunings(NKP, NKI, NKD);
|
|
myPID.SetOutputLimits(-180, 180);
|
|
myPID.SetMode(MANUAL);
|
|
if (DEBUG) { Serial.println("PIDs set correctly"); }
|
|
}
|
|
|
|
void cmpt_weight_gain() {
|
|
if (AUTO_GAIN_WEIGHT) {
|
|
int total_weight = SYSTEM_WEIGHT+PAYLOAD_WEIGHT;
|
|
total_weight = constrain(total_weight, 500, 1500);
|
|
SERVO_MAX_M_W = map(total_weight, 500, 1500, 2000, 1500);
|
|
SERVO_MAX_C_W = map(total_weight, 500, 1500, 1750, 1350);
|
|
}
|
|
else {
|
|
SERVO_MAX_M_W = SERVO_MAX_M_DEF;
|
|
SERVO_MAX_C_W = SERVO_MAX_C_DEF;
|
|
}
|
|
}
|
|
|
|
void cmpt_pressure_gain(float pressure_ratio) {
|
|
if (millis()-time_gain>1000) {
|
|
time_gain = millis();
|
|
if (AUTO_GAIN_PRESSURE) {
|
|
SERVO_MAX_M = constrain(map((SERVO_MAX_M_W-1000)/pressure_ratio, 0, 1000, 1000, 2000), 1250, 2000);
|
|
SERVO_MAX_C = constrain(map((SERVO_MAX_C_W-1000)/pressure_ratio, 0, 1000, 1000, 2000), 1250, 2000);
|
|
}
|
|
else {
|
|
SERVO_MAX_M = SERVO_MAX_M_W;
|
|
SERVO_MAX_C = SERVO_MAX_C_W;
|
|
}
|
|
}
|
|
}
|
|
|
|
String servo_max_cmd_text() {
|
|
String max_m_w_text = String(SERVO_MAX_M_W);
|
|
String max_c_w_text = String(SERVO_MAX_C_W);
|
|
String max_m_text = String(SERVO_MAX_M);
|
|
String max_c_text = String(SERVO_MAX_C);
|
|
return max_m_w_text+","+max_c_w_text+","+max_m_text+","+max_c_text ;
|
|
}
|