VSAG/amadeus_26_fb
3
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix bug

Browse Source
This commit is contained in:
xinyang
2019-08-07 05:18:25 +08:00
parent c8b13159a2
commit 4fc5c55f84
2 changed files with 13 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
energy/src/energy/send/send.cpp
View File

@@ -32,11 +32,11 @@ void Energy::sendEnergy() {
double tmp_yaw = yaw_rotation; double tmp_yaw = yaw_rotation;
double tmp_pitch = pitch_rotation; double tmp_pitch = pitch_rotation;
if(mcu_data.mark == 1){ if (mcu_data.mark == 1) {
yaw_rotation = TRY_BIG_YAW_AIM_KP * yaw_rotation + TRY_BIG_YAW_AIM_KI * sum_yaw + yaw_rotation = TRY_BIG_YAW_AIM_KP * yaw_rotation + TRY_BIG_YAW_AIM_KI * sum_yaw +
TRY_BIG_YAW_AIM_KD * (yaw_rotation - last_yaw); TRY_BIG_YAW_AIM_KD * (yaw_rotation - last_yaw);
pitch_rotation = TRY_BIG_PITCH_AIM_KP * pitch_rotation + TRY_BIG_PITCH_AIM_KI * sum_pitch + pitch_rotation = TRY_BIG_PITCH_AIM_KP * pitch_rotation + TRY_BIG_PITCH_AIM_KI * sum_pitch +
TRY_BIG_PITCH_AIM_KD * (pitch_rotation - last_pitch); TRY_BIG_PITCH_AIM_KD * (pitch_rotation - last_pitch);
} else { } else {
yaw_rotation = BIG_YAW_AIM_KP * yaw_rotation + BIG_YAW_AIM_KI * sum_yaw + yaw_rotation = BIG_YAW_AIM_KP * yaw_rotation + BIG_YAW_AIM_KI * sum_yaw +
BIG_YAW_AIM_KD * (yaw_rotation - last_yaw); BIG_YAW_AIM_KD * (yaw_rotation - last_yaw);
@@ -67,8 +67,14 @@ void Energy::sendEnergy() {
} else if (is_small) { } else if (is_small) {
double tmp_yaw = yaw_rotation; double tmp_yaw = yaw_rotation;
double tmp_pitch = pitch_rotation; double tmp_pitch = pitch_rotation;
yaw_rotation = SMALL_YAW_AIM_KP * yaw_rotation + SMALL_YAW_AIM_KD * (yaw_rotation - last_yaw); if (mcu_data.mark == 1) {
pitch_rotation = SMALL_PITCH_AIM_KP * pitch_rotation + SMALL_PITCH_AIM_KD * (pitch_rotation - last_pitch); yaw_rotation = TRY_SMALL_YAW_AIM_KP * yaw_rotation + TRY_SMALL_YAW_AIM_KD * (yaw_rotation - last_yaw);
pitch_rotation =
TRY_SMALL_PITCH_AIM_KP * pitch_rotation + TRY_SMALL_PITCH_AIM_KD * (pitch_rotation - last_pitch);
} else {
yaw_rotation = SMALL_YAW_AIM_KP * yaw_rotation + SMALL_YAW_AIM_KD * (yaw_rotation - last_yaw);
pitch_rotation = SMALL_PITCH_AIM_KP * pitch_rotation + SMALL_PITCH_AIM_KD * (pitch_rotation - last_pitch);
}
if (ROBOT_ID == 7) { if (ROBOT_ID == 7) {
MINMAX(yaw_rotation, -6, 6); MINMAX(yaw_rotation, -6, 6);
MINMAX(pitch_rotation, -6, 6); MINMAX(pitch_rotation, -6, 6);

3
main.cpp
View File

@@ -33,7 +33,7 @@ McuData mcu_data = { // 单片机端回传结构体
SMALL_ENERGY_STATE, // 当前状态，自瞄-大符-小符 SMALL_ENERGY_STATE, // 当前状态，自瞄-大符-小符
0, // 云台角度标记位 0, // 云台角度标记位
1, // 是否启用数字识别 1, // 是否启用数字识别
ENEMY_BLUE, // 敌方颜色 ENEMY_RED, // 敌方颜色
0, // 能量机关x轴补偿量 0, // 能量机关x轴补偿量
0, // 能量机关y轴补偿量 0, // 能量机关y轴补偿量
}; };
@@ -93,6 +93,7 @@ int main(int argc, char *argv[]) {
bool ok = true; bool ok = true;
cout << "start running" << endl; cout << "start running" << endl;
do { do {
cout<<"save_mark: "<<save_mark<<endl;
char curr_state = mcu_data.state; char curr_state = mcu_data.state;
CNT_TIME("Total", { CNT_TIME("Total", {
if (curr_state == BIG_ENERGY_STATE) {//大能量机关模式 if (curr_state == BIG_ENERGY_STATE) {//大能量机关模式