461 lines
12 KiB
C
461 lines
12 KiB
C
/*
|
|
Copyright 2021 codenocold codenocold@qq.com
|
|
Address : https://github.com/codenocold/dgm
|
|
This file is part of the dgm firmware.
|
|
The dgm firmware is free software: you can redistribute it and/or modify
|
|
it under the terms of the GNU General Public License as published by
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
(at your option) any later version.
|
|
The dgm firmware is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
GNU General Public License for more details.
|
|
You should have received a copy of the GNU General Public License
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
//#include "anticogging.h"
|
|
#include "calibration.h"
|
|
//#include "can.h"
|
|
#include "APP_Main.h"
|
|
#include "APP_Task.h"
|
|
#include "anticogging.h"
|
|
#include "controller.h"
|
|
#include "encoder.h"
|
|
#include "foc.h"
|
|
#include "pwm_curr.h"
|
|
#include "usr_config.h"
|
|
#include "util.h"
|
|
#include <math.h>
|
|
#include <string.h>
|
|
|
|
typedef struct sFSM {
|
|
tFSMState state;
|
|
tFSMState state_next;
|
|
uint8_t state_next_ready;
|
|
} tFSM;
|
|
|
|
static volatile tFSM mFSM;
|
|
|
|
volatile tMCStatusword StatuswordNew;
|
|
volatile tMCStatusword StatuswordOld;
|
|
;
|
|
|
|
#define CHARGE_BOOT_CAP_MS 10
|
|
#define CHARGE_BOOT_CAP_TICKS (uint16_t)((PWM_FREQUENCY * CHARGE_BOOT_CAP_MS) / 1000)
|
|
static uint16_t mChargeBootCapDelay = 0;
|
|
|
|
static void enter_state(void);
|
|
static void exit_state(void);
|
|
static void led_act_loop(void);
|
|
|
|
void MCT_init(void) {
|
|
mFSM.state = BOOT_UP;
|
|
mFSM.state_next = BOOT_UP;
|
|
mFSM.state_next_ready = 0;
|
|
|
|
StatuswordNew.status.status_code = 0;
|
|
StatuswordNew.errors.errors_code = 0;
|
|
StatuswordOld = StatuswordNew;
|
|
}
|
|
|
|
void MCT_reset_error(void) {
|
|
StatuswordNew.errors.errors_code &= 0x0000FFFF;
|
|
StatuswordOld.errors.errors_code &= 0x0000FFFF;
|
|
}
|
|
|
|
tFSMState MCT_get_state(void) {
|
|
return mFSM.state;
|
|
}
|
|
|
|
// return
|
|
// 0 Success
|
|
// -1 Invalid
|
|
// -2 Error code
|
|
// -3 Calib invalid
|
|
int MCT_set_state(tFSMState state) {
|
|
int ret = 0;
|
|
|
|
switch (mFSM.state) {
|
|
case BOOT_UP:
|
|
if (state == IDLE) {
|
|
mFSM.state_next = IDLE;
|
|
} else {
|
|
ret = -1;
|
|
}
|
|
break;
|
|
|
|
case IDLE:
|
|
switch (state) {
|
|
case IDLE:
|
|
FOC_disarm();
|
|
mChargeBootCapDelay = 0;
|
|
mFSM.state_next = IDLE;
|
|
break;
|
|
|
|
case RUN:
|
|
if (StatuswordNew.errors.errors_code) {
|
|
ret = -2;
|
|
} else if (!UsrConfig.calib_valid) {
|
|
ret = -3;
|
|
} else {
|
|
FOC_arm();
|
|
mChargeBootCapDelay = CHARGE_BOOT_CAP_TICKS;
|
|
mFSM.state_next = RUN;
|
|
}
|
|
break;
|
|
|
|
case CALIBRATION:
|
|
if (StatuswordNew.errors.errors_code) {
|
|
ret = -2;
|
|
} else {
|
|
FOC_arm();
|
|
mChargeBootCapDelay = CHARGE_BOOT_CAP_TICKS;
|
|
mFSM.state_next = CALIBRATION;
|
|
}
|
|
break;
|
|
|
|
case ANTICOGGING:
|
|
if (StatuswordNew.errors.errors_code) {
|
|
ret = -2;
|
|
} else if (!UsrConfig.calib_valid) {
|
|
ret = -3;
|
|
} else {
|
|
FOC_arm();
|
|
mChargeBootCapDelay = CHARGE_BOOT_CAP_TICKS;
|
|
mFSM.state_next = ANTICOGGING;
|
|
}
|
|
break;
|
|
|
|
default:
|
|
ret = -1;
|
|
break;
|
|
}
|
|
break;
|
|
|
|
default:
|
|
if (state == IDLE) {
|
|
mFSM.state_next = IDLE;
|
|
} else {
|
|
ret = -1;
|
|
}
|
|
break;
|
|
}
|
|
|
|
mFSM.state_next_ready = 0;
|
|
|
|
return ret;
|
|
}
|
|
|
|
static void enter_state(void) {
|
|
switch (mFSM.state) {
|
|
case BOOT_UP:
|
|
break;
|
|
|
|
case IDLE:
|
|
break;
|
|
|
|
case RUN:
|
|
CONTROLLER_reset();
|
|
StatuswordNew.status.switched_on = 1;
|
|
StatuswordNew.status.target_reached = 1;
|
|
StatuswordNew.status.current_limit_active = 0;
|
|
StatuswordOld.status = StatuswordNew.status;
|
|
break;
|
|
|
|
case CALIBRATION:
|
|
CALIBRATION_start();
|
|
break;
|
|
|
|
case ANTICOGGING:
|
|
CONTROLLER_reset();
|
|
ANTICOGGING_start();
|
|
break;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
static void exit_state(void) {
|
|
switch (mFSM.state) {
|
|
case BOOT_UP:
|
|
// CAN_reset_rx_timeout();
|
|
// CAN_reset_tx_timeout();
|
|
mFSM.state_next_ready = 1;
|
|
break;
|
|
|
|
case IDLE:
|
|
if (mChargeBootCapDelay) {
|
|
mChargeBootCapDelay--;
|
|
} else {
|
|
mFSM.state_next_ready = 1;
|
|
}
|
|
break;
|
|
|
|
case RUN:
|
|
FOC_disarm();
|
|
StatuswordNew.status.switched_on = 0;
|
|
StatuswordNew.status.target_reached = 0;
|
|
StatuswordNew.status.current_limit_active = 0;
|
|
StatuswordOld.status = StatuswordNew.status;
|
|
mFSM.state_next_ready = 1;
|
|
break;
|
|
|
|
case CALIBRATION:
|
|
CALIBRATION_end();
|
|
|
|
USR_CONFIG_save_config();
|
|
|
|
printf("UsrConfig.motor_phase_resistance:%f\r\n", UsrConfig.motor_phase_resistance);
|
|
printf("UsrConfig.motor_phase_inductance:%f\r\n", UsrConfig.motor_phase_inductance);
|
|
printf("UsrConfig.motor_pole_pairs:%ld\r\n", UsrConfig.motor_pole_pairs);
|
|
|
|
mFSM.state_next_ready = 1;
|
|
break;
|
|
|
|
case ANTICOGGING:
|
|
ANTICOGGING_end();
|
|
mFSM.state_next_ready = 1;
|
|
break;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
int time = 1;
|
|
int count = 0;
|
|
void MCT_high_frequency_task(void) {
|
|
/* state transition management */
|
|
if (mFSM.state_next != mFSM.state) {
|
|
exit_state();
|
|
if (mFSM.state_next_ready) {
|
|
mFSM.state = mFSM.state_next;
|
|
enter_state();
|
|
}
|
|
}
|
|
|
|
// if (time >= 0) {
|
|
// time++;
|
|
// }
|
|
//
|
|
// if (time == 5) {
|
|
// StatuswordNew.errors.errors_code = 0;
|
|
// int ret = MCT_set_state(IDLE);
|
|
// printf("IDLE ret:%d\r\n", ret);
|
|
// }
|
|
//
|
|
// if (time == 10) {
|
|
// int ret = MCT_set_state(CALIBRATION);
|
|
// printf("CALIBRATION ret:%d\r\n", ret);
|
|
// // UsrConfig.control_mode = CONTROL_MODE_VELOCITY_RAMP;
|
|
// // Controller.input_velocity = 0.01f;
|
|
//
|
|
// time = -1;
|
|
// }
|
|
// //
|
|
// if (UsrConfig.calib_valid && time == -1) {
|
|
// int ret = MCT_set_state(RUN);
|
|
// printf("RUN ret:%d\r\n", ret);
|
|
// UsrConfig.control_mode = CONTROL_MODE_POSITION_PROFILE;
|
|
// Controller.input_position = 0.0f;
|
|
// Controller.input_velocity = 0.0f;
|
|
// time = -2;
|
|
// }
|
|
//
|
|
// if (UsrConfig.calib_valid && time == -2) {
|
|
// count++;
|
|
// if (count < 50000) {
|
|
// Controller.input_position = 5.0f;
|
|
// Controller.input_updated = true;
|
|
// }
|
|
// if (count > 50000) {
|
|
// Controller.input_position = 0.5f;
|
|
// Controller.input_updated = true;
|
|
// }
|
|
// if (count > 100000) {
|
|
// count = 0;
|
|
// }
|
|
//
|
|
// time = -2;
|
|
// }
|
|
|
|
|
|
// ENCODER_loop();
|
|
// FOC_voltage(0,0.001f,Encoder.phase);
|
|
// Foc.v_bus = read_vbus();
|
|
Foc.v_bus = 24.0f;
|
|
UTILS_LP_FAST(Foc.v_bus_filt, Foc.v_bus, 0.05f);
|
|
Foc.i_a = read_iphase_a();
|
|
Foc.i_b = read_iphase_b();
|
|
Foc.i_c = read_iphase_c();
|
|
|
|
|
|
switch (mFSM.state) {
|
|
case BOOT_UP:
|
|
break;
|
|
|
|
case CALIBRATION:
|
|
CALIBRATION_loop();
|
|
break;
|
|
|
|
case ANTICOGGING:
|
|
ANTICOGGING_loop();
|
|
|
|
case RUN: {
|
|
CONTROLLER_loop();
|
|
|
|
// check motor current
|
|
// If Ia + Ib + Ic == 0 holds then we have: Inorm^2 = Id^2 + Iq^2 = Ialpha^2 + Ibeta^2 = 2/3 * (Ia^2 + Ib^2 + Ic^2)
|
|
float Inorm_sq = 2.0f / 3.0f * (SQ(Foc.i_a) + SQ(Foc.i_b) + SQ(Foc.i_c));
|
|
if (Inorm_sq > SQ(UsrConfig.protect_over_current)) {
|
|
FOC_disarm();
|
|
MCT_set_state(IDLE);
|
|
StatuswordNew.errors.over_current = 1;
|
|
}
|
|
|
|
// check I bus current
|
|
if (Foc.i_bus > UsrConfig.protect_i_bus_max) {
|
|
FOC_disarm();
|
|
MCT_set_state(IDLE);
|
|
StatuswordNew.errors.over_current = 1;
|
|
}
|
|
} break;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
void MCT_safety_task(void) {
|
|
// VBUS check
|
|
if (mFSM.state != BOOT_UP) {
|
|
// Over voltage check
|
|
if (Foc.v_bus > UsrConfig.protect_over_voltage) {
|
|
StatuswordNew.errors.over_voltage = 1;
|
|
}
|
|
|
|
// Under voltage check
|
|
if (Foc.v_bus < UsrConfig.protect_under_voltage) {
|
|
StatuswordNew.errors.under_voltage = 1;
|
|
}
|
|
|
|
// Enchoder state check
|
|
if (MT6816.check_err_count > 50 || MT6816.rx_err_count > 50) {
|
|
StatuswordNew.errors.encoder_offline = 1;
|
|
}
|
|
}
|
|
|
|
watch_dog_feed();
|
|
}
|
|
|
|
void MCT_low_priority_task(void) {
|
|
bool isSend = false;
|
|
|
|
// State check
|
|
if (StatuswordOld.status.status_code != StatuswordNew.status.status_code) {
|
|
isSend = true;
|
|
StatuswordOld.status.status_code = StatuswordNew.status.status_code;
|
|
}
|
|
|
|
// Error check
|
|
if (StatuswordOld.errors.errors_code != StatuswordNew.errors.errors_code) {
|
|
if (StatuswordNew.errors.errors_code) {
|
|
FOC_disarm();
|
|
MCT_set_state(IDLE);
|
|
}
|
|
|
|
isSend = true;
|
|
StatuswordOld.errors.errors_code = StatuswordNew.errors.errors_code;
|
|
}
|
|
|
|
if (isSend) {
|
|
CAN_tx_statusword(StatuswordNew);
|
|
}
|
|
|
|
led_act_loop();
|
|
CAN_comm_loop();
|
|
}
|
|
|
|
static void led_act_loop(void) {
|
|
static uint16_t tick = 0;
|
|
static uint32_t tick_100Hz = 0;
|
|
|
|
// 100Hz
|
|
if (get_ms_since(tick_100Hz) < 10) {
|
|
return;
|
|
}
|
|
tick_100Hz = SystickCount;
|
|
|
|
switch (mFSM.state) {
|
|
case IDLE:
|
|
if (tick == 0) {
|
|
LED_ACT_SET();
|
|
} else if (tick == 10) {
|
|
LED_ACT_RESET();
|
|
} else if (tick > 100) {
|
|
tick = 0xFFFF;
|
|
}
|
|
break;
|
|
|
|
case RUN:
|
|
if (tick == 0) {
|
|
LED_ACT_SET();
|
|
} else if (tick == 10) {
|
|
LED_ACT_RESET();
|
|
} else if (tick == 20) {
|
|
LED_ACT_SET();
|
|
} else if (tick == 30) {
|
|
LED_ACT_RESET();
|
|
} else if (tick > 100) {
|
|
tick = 0xFFFF;
|
|
}
|
|
break;
|
|
|
|
case CALIBRATION:
|
|
if (tick == 0) {
|
|
LED_ACT_SET();
|
|
} else if (tick == 10) {
|
|
LED_ACT_RESET();
|
|
} else if (tick == 20) {
|
|
LED_ACT_SET();
|
|
} else if (tick == 30) {
|
|
LED_ACT_RESET();
|
|
} else if (tick == 40) {
|
|
LED_ACT_SET();
|
|
} else if (tick == 50) {
|
|
LED_ACT_RESET();
|
|
} else if (tick > 150) {
|
|
tick = 0xFFFF;
|
|
}
|
|
break;
|
|
|
|
case ANTICOGGING:
|
|
if (tick == 0) {
|
|
LED_ACT_SET();
|
|
} else if (tick == 10) {
|
|
LED_ACT_RESET();
|
|
} else if (tick == 20) {
|
|
LED_ACT_SET();
|
|
} else if (tick == 30) {
|
|
LED_ACT_RESET();
|
|
} else if (tick == 40) {
|
|
LED_ACT_SET();
|
|
} else if (tick == 50) {
|
|
LED_ACT_RESET();
|
|
} else if (tick == 60) {
|
|
LED_ACT_SET();
|
|
} else if (tick == 70) {
|
|
LED_ACT_RESET();
|
|
} else if (tick > 200) {
|
|
tick = 0xFFFF;
|
|
}
|
|
break;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
|
|
tick++;
|
|
}
|