Skip to content

Commit

Permalink
dart can store energy and return to the normal postion.
Browse files Browse the repository at this point in the history
  • Loading branch information
@Raventhatfly
Raventhatfly committed Jul 26, 2023
1 parent 4378a14 commit 539da93
Show file tree
Hide file tree
Showing 6 changed files with 116 additions and 37 deletions.
12 changes: 6 additions & 6 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -335,12 +335,12 @@ target_include_directories(ut_buzzer PRIVATE dev/application/unit_tests/ut_buzze
# Referee
# temporarily disabled by Tony Zhang
# TODO re-enable this one, since referee is outdated
#add_executable(ut_referee_interface
# dev/common/CRC8.cpp
# dev/common/CRC16.cpp
# dev/interface/referee/referee_interface.cpp
# dev/application/unit_tests/ut_referee_if/ut_referee_if.cpp)
#target_include_directories(ut_referee_interface PRIVATE dev/application/unit_tests/ut_referee_if)
add_executable(ut_referee_interface
dev/common/CRC8.cpp
dev/common/CRC16.cpp
dev/interface/referee/referee_interface.cpp
dev/application/unit_tests/ut_referee_if/ut_referee_if.cpp)
target_include_directories(ut_referee_interface PRIVATE dev/application/unit_tests/ut_referee_if)

add_executable(ut_sd_card
dev/interface/sd_card/sd_card_interface.cpp
Expand Down
2 changes: 1 addition & 1 deletion dev/application/unit_tests/ut_referee_if/hardware_conf.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,7 @@
#endif

#if !defined(ENABLE_USB_SHELL) || defined(__DOXYGEN__)
#define ENABLE_USB_SHELL FALSE
#define ENABLE_USB_SHELL TRUE
#endif

#endif //META_INFANTRY_HARDWARE_CONF_H
41 changes: 23 additions & 18 deletions dev/application/unit_tests/ut_referee_if/ut_referee_if.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,21 +21,23 @@ class RefereeEchoThread : public BaseStaticThread <2048> {
setName("referee_echo");
while(!shouldTerminate()) {

// LOG("chassis_volt = %u" , (unsigned int) Referee::power_heat_data.chassis_volt);
// LOG("chassis_current = %u" , (unsigned int) Referee::power_heat_data.chassis_current);
// LOG("chassis_power = %f" , Referee::power_heat_data.chassis_power);
// LOG("chassis_power_buffer = %u" , (unsigned int) Referee::power_heat_data.chassis_power_buffer);
// LOG("shooter_heat0 = %u" , (unsigned int) Referee::power_heat_data.shooter_id1_17mm_cooling_heat);
// LOG("");
//
// LOG("bullet_type = %u" , (unsigned int) Referee::shoot_data.bullet_type);
// LOG("bullet_freq = %u" , (unsigned int) Referee::shoot_data.bullet_freq);
// LOG("bullet_speed = %u" , (unsigned int) Referee::shoot_data.bullet_speed);
// LOG("");
LOG("chassis_volt = %u" , (unsigned int) Referee::power_heat.chassis_volt);
LOG("chassis_current = %u" , (unsigned int) Referee::power_heat.chassis_current);
LOG("chassis_power = %f" , Referee::power_heat.chassis_power);
LOG("chassis_power_buffer = %u" , (unsigned int) Referee::power_heat.chassis_power_buffer);
LOG("shooter_heat0 = %u" , (unsigned int) Referee::power_heat.shooter_id1_17mm_cooling_heat);
LOG("");

//
// LOG("armor_id = %u" , (unsigned int) Referee::robot_hurt.armor_id);
// LOG("hurt_type = %u" , (unsigned int) Referee::robot_hurt.hurt_type);
// LOG("");
LOG("armor_id = %u" , (unsigned int) Referee::robot_hurt.armor_id);
LOG("hurt_type = %u" , (unsigned int) Referee::robot_hurt.hurt_type);
LOG("launch opening status = %u", (unsigned int) Referee::dart_client.dart_launch_opening_status);
LOG("attack target = %u", (unsigned int) Referee::dart_client.dart_attack_target);
LOG("target change time = %u", (unsigned int) Referee::dart_client.target_change_time);
LOG("launch cmd time = %u", (unsigned int) Referee::dart_client.operate_launch_cmd_time);
LOG("remain_time = %u ",(unsigned int) Referee::ext_dart_remaining_time.dart_remaining_time);

LOG("");
sleep(TIME_MS2I(200));
}
}
Expand Down Expand Up @@ -71,7 +73,8 @@ DEF_SHELL_CMD_START(cmd_add_rect)

rect.radius = 200;

Referee::set_graphic(rect);
// Referee::set_graphic(rect);
Referee::add_tx_graphic(rect);
enabled_send = true;
chprintf(chp, "added" SHELL_NEWLINE_STR);
return true; // command executed successfully
Expand Down Expand Up @@ -107,7 +110,8 @@ DEF_SHELL_CMD_START(cmd_modify_rect)

rect.radius = 200;

Referee::set_graphic(rect);
// Referee::set_graphic(rect);
Referee::add_tx_graphic(rect);
chprintf(chp, "modified" SHELL_NEWLINE_STR);
return true; // command executed successfully
DEF_SHELL_CMD_END
Expand Down Expand Up @@ -142,7 +146,8 @@ DEF_SHELL_CMD_START(cmd_del_rect)

rect.radius = 200;

Referee::set_graphic(rect);
// Referee::set_graphic(rect);
Referee::add_tx_graphic(rect);
enabled_send = false;
chprintf(chp, "deleted" SHELL_NEWLINE_STR);
return true; // command executed successfully
Expand All @@ -165,7 +170,7 @@ int main() {
// Start ChibiOS shell at high priority, so even if a thread stucks, we still have access to shell.
Shell::start(HIGHPRIO);

Referee::init(NORMALPRIO);
Referee::init();
refereeEchoThread.start(NORMALPRIO+1);

// See chconf.h for what this #define means.
Expand Down
2 changes: 1 addition & 1 deletion dev/application/vehicles/dart/can_motor_config.h
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
//
// Created by 钱晨 on 11/14/21.
// Created by Wu Feiyang on 7/6/23.
//

#ifndef META_INFANTRY_CANBUS_MOTOR_CFG_H
Expand Down
86 changes: 75 additions & 11 deletions dev/application/vehicles/dart/user_dart.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,29 +3,59 @@
//
#include "user_dart.h"
UserDart::UserThread UserDart::userThread;
bool UserDart::UserThread::start_flag;
bool UserDart::timer_started;

void UserDart::start(tprio_t user_thd_prio) {
userThread.start(user_thd_prio);
}

void UserDart::return_puller() {
CANMotorController::set_target_angle(CANMotorCFG::STORE_ENERGY_LEFT,0);
CANMotorController::set_target_angle(CANMotorCFG::STORE_ENERGY_RIGHT,0);
CANMotorCFG::enable_a2v[CANMotorCFG::STORE_ENERGY_LEFT] = false;
CANMotorCFG::enable_v2i[CANMotorCFG::STORE_ENERGY_LEFT] = false;
CANMotorCFG::enable_a2v[CANMotorCFG::STORE_ENERGY_RIGHT] = false;
CANMotorCFG::enable_v2i[CANMotorCFG::STORE_ENERGY_RIGHT] = false;
chThdSleepMilliseconds(10);
CANMotorCFG::enable_a2v[CANMotorCFG::STORE_ENERGY_LEFT] = true;
CANMotorCFG::enable_v2i[CANMotorCFG::STORE_ENERGY_LEFT] = true;
CANMotorCFG::enable_a2v[CANMotorCFG::STORE_ENERGY_RIGHT] = true;
CANMotorCFG::enable_v2i[CANMotorCFG::STORE_ENERGY_RIGHT] = true;
timer_started = false;
}


void UserDart::UserThread::main(){
setName("RemoteControl");
static float angle;
feedback = CANMotorIF::motor_feedback[CANMotorCFG::YAW];
angle = feedback.accumulate_angle();
Rudder rudder1(&PWMD4, nullptr,0,Rudder::MG995);
rudder1.start();
start_flag = true;
timer_started = false;
static virtual_timer_t release_vt;
chVTObjectInit(&release_vt);
int start_time;
int current_time;



while(!shouldTerminate()){
chSysLock();

if(Remote::rc.s1 == Remote::S_UP){ // Forced Relax Mode
chSysLock();
for(uint i=0;i<CANMotorCFG::MOTOR_COUNT;i++){
CANMotorCFG::enable_a2v[i] = false;
CANMotorCFG::enable_v2i[i] = false;
}
CANMotorController::set_target_current(CANMotorCFG::YAW,0);
CANMotorController::set_target_current(CANMotorCFG::STORE_ENERGY_LEFT,0);
CANMotorController::set_target_current(CANMotorCFG::STORE_ENERGY_RIGHT,0);
}else if(Remote::rc.s1 == Remote::S_MIDDLE){ // Remote contorl mode
chSysUnlock();
}else if(Remote::rc.s1 == Remote::S_MIDDLE && Remote::rc.s2 == Remote::S_UP){ // Remote contorl mode
chSysLock();
CANMotorCFG::enable_v2i[CANMotorCFG::YAW] = true;
CANMotorCFG::enable_a2v[CANMotorCFG::YAW] = false;
CANMotorCFG::enable_v2i[CANMotorCFG::STORE_ENERGY_LEFT] = true;
Expand All @@ -40,25 +70,59 @@ void UserDart::UserThread::main(){
CANMotorController::set_target_vel(CANMotorCFG::TRIGGER_ADJUST,2000*Remote::rc.ch1);
chSysUnlock();
rudder1.set_rudder_angle(180 - (int)Remote::rc.ch0 * 180);

}else if(Remote::rc.s1 == Remote::S_DOWN && Remote::rc.s2 == Remote::S_UP){ // PC control mode, goes back to initial encoder angle

CANMotorCFG::enable_v2i[CANMotorCFG::YAW] = true;
CANMotorCFG::enable_a2v[CANMotorCFG::YAW] = true;
CANMotorController::set_target_angle(CANMotorCFG::YAW,angle);
uint8_t status = Referee::dart_client.dart_launch_opening_status;
if(status == 0){
if(Remote::rc.ch3 > 0.2){
start_flag = true;
}
if(start_flag){
if(!timer_started){
start_time = TIME_I2MS(chVTGetSystemTimeX());
timer_started = true;
}
current_time = TIME_I2MS(chVTGetSystemTimeX());
if(current_time - start_time < 12000){
CANMotorController::set_target_angle(CANMotorCFG::STORE_ENERGY_LEFT,3900);
CANMotorController::set_target_angle(CANMotorCFG::STORE_ENERGY_RIGHT,-3900);

chThdSleepSeconds(1);
CANMotorCFG::enable_a2v[CANMotorCFG::STORE_ENERGY_LEFT] = false;
CANMotorCFG::enable_v2i[CANMotorCFG::STORE_ENERGY_LEFT] = false;
CANMotorCFG::enable_a2v[CANMotorCFG::STORE_ENERGY_RIGHT] = false;
CANMotorCFG::enable_v2i[CANMotorCFG::STORE_ENERGY_RIGHT] = false;
chThdSleepMilliseconds(5);
CANMotorCFG::enable_a2v[CANMotorCFG::STORE_ENERGY_LEFT] = true;
CANMotorCFG::enable_v2i[CANMotorCFG::STORE_ENERGY_LEFT] = true;
CANMotorCFG::enable_a2v[CANMotorCFG::STORE_ENERGY_RIGHT] = true;
CANMotorCFG::enable_v2i[CANMotorCFG::STORE_ENERGY_RIGHT] = true;
}else if(current_time - start_time >= 12000){
CANMotorController::set_target_angle(CANMotorCFG::STORE_ENERGY_LEFT,0);
CANMotorController::set_target_angle(CANMotorCFG::STORE_ENERGY_RIGHT,0);
timer_started = false;
}
}

}

}else if(Remote::rc.s1 == Remote::S_MIDDLE && Remote::rc.s2 == Remote::S_MIDDLE){
chSysLock();
}else if(Remote::rc.s1 == Remote::S_DOWN){ // PC control mode, goes back to initial encoder angle
CANMotorCFG::enable_v2i[CANMotorCFG::YAW] = true;
CANMotorCFG::enable_a2v[CANMotorCFG::YAW] = true;
CANMotorCFG::enable_v2i[CANMotorCFG::STORE_ENERGY_LEFT] = false;
CANMotorCFG::enable_a2v[CANMotorCFG::STORE_ENERGY_LEFT] = false;
CANMotorCFG::enable_v2i[CANMotorCFG::STORE_ENERGY_RIGHT] = false;
CANMotorCFG::enable_a2v[CANMotorCFG::STORE_ENERGY_RIGHT] = false;
CANMotorController::set_target_angle(CANMotorCFG::YAW,angle);
uint8_t remain_time = Referee::ext_dart_remaining_time.dart_remaining_time;
if(remain_time == 0){
CANMotorCFG::enable_v2i[CANMotorCFG::STORE_ENERGY_LEFT] = true;
CANMotorCFG::enable_a2v[CANMotorCFG::STORE_ENERGY_LEFT] = true;
CANMotorCFG::enable_v2i[CANMotorCFG::STORE_ENERGY_RIGHT] = true;
CANMotorCFG::enable_a2v[CANMotorCFG::STORE_ENERGY_RIGHT] = true;
}
chSysUnlock();
}

chSysUnlock();

// feedback = CANMotorIF::motor_feedback[CANMotorCFG::YAW];
// LOG("initial angle: %f, current angle: %f\r\n",angle,feedback.accumulate_angle());
chThdSleepMilliseconds(100);
Expand Down
10 changes: 10 additions & 0 deletions dev/application/vehicles/dart/user_dart.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,11 +18,21 @@ class UserDart{
static void start(tprio_t user_thd_prio);
private:
class UserThread:public BaseStaticThread<512>{
public:
static bool start_flag;
private:
void main() final;
CANMotorFeedback feedback;
};
static bool timer_started;

static UserThread userThread;

static void return_puller();

static void release_dart();


};


Expand Down

0 comments on commit 539da93

Please sign in to comment.