Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[Feat!!!]: add the whole hero code #64

Merged
merged 17 commits into from
Apr 6, 2024
Merged

[Feat!!!]: add the whole hero code #64

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
17 commits
Select commit Hold shift + click to select a range
bde53c3
[feat]: added servo for hero trigger
Andd54 Jul 29, 2023
ba6ca08
[fix]: adjusted accuracy for 0 deg
Andd54 Jul 29, 2023
794a0f8
finish the chassis and gimbal on the hero(need test)
N9nGe Aug 1, 2023
21c3d75
start add the shooter
N9nGe Aug 2, 2023
c568ffd
Merge branch 'main' into andd54/hero_trigger
N9nGe Aug 2, 2023
20f6db9
Merge branch 'andd54/hero_trigger' into ngao4/Hero
N9nGe Aug 2, 2023
1b5c4d1
test example
N9nGe Aug 2, 2023
1e2fe9d
pass the load test and pitch test, need keyboard and mouse test
N9nGe Aug 2, 2023
94dc9dd
need test
N9nGe Aug 3, 2023
e995baf
most param need test
N9nGe Aug 3, 2023
fe4ba7b
just leave the reload motor parameter
N9nGe Aug 3, 2023
36c3b1c
just leave the reload motor parameter
N9nGe Aug 3, 2023
4ec437b
the data for the weak one is ready
N9nGe Aug 4, 2023
c09d2fb
pass the test in the match
N9nGe Aug 5, 2023
ebc45c8
add two shoot mode
N9nGe Aug 5, 2023
7fb02a3
the lock for the safe mode
N9nGe Sep 30, 2023
ad9972b
fix the problem in pr
N9nGe Mar 30, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
119 changes: 60 additions & 59 deletions examples/hero/hero_shooter_2023.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -91,23 +91,23 @@ void RM_RTOS_Init() {
reload_servo = new control::ServoMotor(servo_data);

servo_data.motor = force_motor;
servo_data.max_speed = 6 * PI; // params need test
servo_data.max_acceleration = 50 * PI;
servo_data.max_speed = 4 * PI; // params need test
servo_data.max_acceleration = 10 * PI;
servo_data.omega_pid_param = new float [3] {150, 1.2, 5}; // pid need test
force_servo = new control::ServoMotor(servo_data);
}

void RM_RTOS_Default_Task(const void* args) {
UNUSED(args);
// control::MotorCANBase* can2_reloader[] = { reload_motor};
control::MotorCANBase* can2_reloader[] = { reload_motor};
control::MotorCANBase* can2_loader[] = { load_motor};
control::MotorCANBase* can2_force[] = { force_motor};

// Variable initialization (params need test)
// reload variables
// bool reload_pull = false;
// bool reload_push = false;
// float reload_pos = 10 * PI;
bool reload_pull = false;
bool reload_push = false;
float reload_pos = 10 * PI;
// load variable
bool loading = false;
float load_angle = 2 * PI / 6 * 99.506 / M3508P19_RATIO; // 99.506 is the ratio of the load servo, devide the 3508 ratio
Expand All @@ -126,18 +126,19 @@ void RM_RTOS_Default_Task(const void* args) {
}

while (true) {
// Dead
// while (Dead || GimbalDead) osDelay(100);

// whether change the force motor position
ForceWeakDetect.input(dbus->keyboard.bit.F || dbus->swr == remote::UP);
ForceStrongDetect.input(dbus->keyboard.bit.C || dbus->swr == remote::UP);
ForceWeakDetect.input(dbus->keyboard.bit.F || dbus->wheel.wheel > remote::WheelDigitalValue);
ForceStrongDetect.input(dbus->keyboard.bit.C
|| (dbus->wheel.wheel == remote::WheelDigitalValue && dbus->previous_wheel_value == remote::WheelDigitalValue));

// just execute force transform once
if (ForceWeakDetect.posEdge() && !ForceStrongDetect.posEdge() && !force_week) {
N9nGe marked this conversation as resolved.
Show resolved Hide resolved
print("week");
N9nGe marked this conversation as resolved.
Show resolved Hide resolved
force_transforming = true;
force_week = true;
force_strong = false;
} else if (ForceStrongDetect.posEdge() && !ForceWeakDetect.posEdge() && !force_strong) {
print("strong");
force_transforming = true;
force_week = false;
force_strong = true;
Expand Down Expand Up @@ -182,32 +183,32 @@ void RM_RTOS_Default_Task(const void* args) {
LoadDetect.input(dbus->swr == remote::UP);
if (LoadDetect.posEdge()) {
// step 1
// trigger->SetOutPutAngle(20); // need test the trigger angle
// osDelay(1000); // need test the delay time(wait for the)
// // step 2
// while (true) {
// // break condition (reach the desire position)
// if (++i > 20 / 2 && abs(reload_servo->GetOmega()) <= 0.001) break;
// // set the speed and acceleration for the reload motor
// // set target pull position once
// if (!reload_pull) {
// reload_pull = true;
// reload_servo->SetTarget(reload_pos);
// }
// print("reload theta: %f\n", reload_servo->GetTheta());
// reload_servo->CalcOutput();
// control::MotorCANBase::TransmitOutput(can2_reloader, 1);
// osDelay(2);
// }
// // after reload pulling
// i = 0;
// reload_motor->SetOutput(0);
// print("omega %f", reload_servo->GetOmega());
// control::MotorCANBase::TransmitOutput(can2_reloader, 1);
// reload_pull = false;
// osDelay(100); // need test the delay time(wait for the)
// // step 3
// trigger->SetOutPutAngle(-80); // need test the trigger angle
trigger->SetOutPutAngle(20); // need test the trigger angle
osDelay(1000); // need test the delay time(wait for the)
// step 2
while (true) {
// break condition (reach the desire position)
if (++i > 20 / 2 && abs(reload_servo->GetOmega()) <= 0.001) break;
// set the speed and acceleration for the reload motor
// set target pull position once
if (!reload_pull) {
reload_pull = true;
reload_servo->SetTarget(reload_pos);
}
print("reload theta: %f\n", reload_servo->GetTheta());
reload_servo->CalcOutput();
control::MotorCANBase::TransmitOutput(can2_reloader, 1);
osDelay(2);
}
// after reload pulling
i = 0;
reload_motor->SetOutput(0);
print("omega %f", reload_servo->GetOmega());
control::MotorCANBase::TransmitOutput(can2_reloader, 1);
reload_pull = false;
osDelay(100); // need test the delay time(wait for the)
// step 3
trigger->SetOutPutAngle(-80); // need test the trigger angle
osDelay(100); // need test the delay time(wait for the)
// step 4
while (true) {
Expand All @@ -230,30 +231,30 @@ void RM_RTOS_Default_Task(const void* args) {
control::MotorCANBase::TransmitOutput(can2_loader, 1);
loading = false;
osDelay(100); // need test the delay time(wait for the
// // step 5
// while (true) {
// // break condition (reach the desire position)
// if (++i > 20 / 2 && abs(reload_servo->GetOmega()) <= 0.001) break;
// // set target push position once
// if (!reload_push) {
// reload_push = true;
// reload_servo->SetTarget(0, true);
// print("reload target: %f\n", reload_servo->GetTarget());
// print("reload theta: %f\n", reload_servo->GetTheta());
// }
// reload_servo->CalcOutput();
// control::MotorCANBase::TransmitOutput(can2_reloader, 1);
// osDelay(2);
// }
// // after reload pushing
// i = 0;
// reload_motor->SetOutput(0);
// control::MotorCANBase::TransmitOutput(can2_reloader, 1);
// reload_push = false;
// osDelay(100); // need test the delay time(wait for the)
// step 6(TODO)
// step 5
while (true) {
// break condition (reach the desire position)
if (++i > 20 / 2 && abs(reload_servo->GetOmega()) <= 0.001) break;
// set target push position once
if (!reload_push) {
reload_push = true;
reload_servo->SetTarget(0, true);
print("reload target: %f\n", reload_servo->GetTarget());
print("reload theta: %f\n", reload_servo->GetTheta());
}
reload_servo->CalcOutput();
control::MotorCANBase::TransmitOutput(can2_reloader, 1);
osDelay(2);
}
// after reload pushing
i = 0;
reload_motor->SetOutput(0);
control::MotorCANBase::TransmitOutput(can2_reloader, 1);
reload_push = false;
osDelay(100); // need test the delay time(wait for the)

}
dbus->previous_wheel_value = dbus->wheel.wheel;
osDelay(10);
}
}
2 changes: 1 addition & 1 deletion examples/relay/TypeC.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,7 @@ static bsp::Relay* relay;

void RM_RTOS_Init(void) {
print_use_uart(&huart1);
relay = new bsp::Relay(RELAY_1_GPIO_Port, RELAY_1_Pin); /* USE GPIO_1 (PB14) */
relay = new bsp::Relay(RELAY_1_GPIO_Port, RELAY_1_Pin); /* USE GPIO_1 (PB14)->PIN 7 in the board*/
}

void RM_RTOS_Default_Task(const void* arguments) {
Expand Down
60 changes: 48 additions & 12 deletions vehicles/Hero/main.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@
#include "bsp_gpio.h"
#include "bsp_os.h"
#include "bsp_print.h"
#include "bsp_relay.h"
#include "cmsis_os.h"
#include "controller.h"
#include "dbus.h"
Expand All @@ -39,13 +40,17 @@ static remote::DBUS* dbus = nullptr;
static bsp::CAN* can1 = nullptr;
static bsp::CAN* can2 = nullptr;
static display::RGB* RGB = nullptr;
static bsp::Relay* relay = nullptr;

// Special Modes
static BoolEdgeDetector FakeDeath(false);
static BoolEdgeDetector LoadDetect(false);
static BoolEdgeDetector ElevationDetect(false);
static BoolEdgeDetector PreloadDetect(false);
static volatile bool Dead = false;
static volatile bool GimbalDead = false;
static volatile bool Elevation = false;
static volatile bool Preload = false;

// Delays
static const int KILLALL_DELAY = 100;
Expand Down Expand Up @@ -119,6 +124,7 @@ const osThreadAttr_t chassisTaskAttribute = {.name = "chassisTask",
osThreadId_t chassisTaskHandle;
// TODO:
// 1. test keyboard and mouse data
// 2. test relay

// Params Initialization
static control::MotorCANBase* fl_motor = nullptr;
Expand All @@ -142,6 +148,19 @@ void chassisTask(void* arg) {
}

while (true) {

// Elevation(shift)
ElevationDetect.input(dbus->keyboard.bit.SHIFT);
if (ElevationDetect.posEdge()) {
Elevation = !Elevation;
if (Elevation) {
relay->On();
} else {
relay->Off();
}
osDelay(100);
}

// read the data from keyboard
if (dbus->keyboard.bit.A) vx_keyboard -= 61.5;
if (dbus->keyboard.bit.D) vx_keyboard += 61.5;
Expand Down Expand Up @@ -177,8 +196,7 @@ void chassisTask(void* arg) {
} else {
chassis->SetSpeed(0, 0, 0);
}
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Not sure what elevation does, better explain more and the numbers in it with comments

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

elevation is for the gimbal. when elevation, the mouse is used to control the shooter rather than chassis.

print("vel: vx: %f, vy: %f, wz: %f\r\n", vx, vy, wz);
print("power limit: %d\r\n", referee->game_robot_status.chassis_power_limit);

chassis->Update(true, (float)referee->game_robot_status.chassis_power_limit,
referee->power_heat_data.chassis_power,
(float)referee->power_heat_data.chassis_power_buffer);
Expand Down Expand Up @@ -360,6 +378,26 @@ void shooterTask(void* arg) {
while (Dead || GimbalDead) osDelay(100);

i = 0;
if (Preload) {
while (true) {
// break condition (loading)
if (++i > 10 && abs(load_servo->GetOmega()) <= 0.001) break;
// loading once
if (i == 1) {
Preload = false;
load_servo->SetTarget(load_servo->GetTarget() - load_angle);
}
load_servo->CalcOutput();
control::MotorCANBase::TransmitOutput(can2_loader, 1);
osDelay(2);
}
// after loading
i = 0;
load_motor->SetOutput(0);
control::MotorCANBase::TransmitOutput(can2_loader, 1);
osDelay(100); // need test the delay time
}

// whether change the force motor position
ForceWeakDetect.input(dbus->keyboard.bit.F || dbus->wheel.wheel > remote::WheelDigitalValue);
ForceStrongDetect.input(dbus->keyboard.bit.C
Expand Down Expand Up @@ -651,6 +689,9 @@ void RM_RTOS_Init() {
referee_uart->SetupRx(300);
referee_uart->SetupTx(300);
referee = new communication::Referee;

// Relay initialization
relay = new bsp::Relay(RELAY_1_GPIO_Port, RELAY_1_Pin); /* USE GPIO_1 (PB14)->PIN 7 in the board */
}

//==================================================================================================
Expand All @@ -661,7 +702,7 @@ void RM_RTOS_Threads_Init(void) {
refereeTaskHandle = osThreadNew(refereeTask, nullptr, &refereeTaskAttribute);
chassisTaskHandle = osThreadNew(chassisTask, nullptr, &chassisTaskAttribute);
gimbalTaskHandle = osThreadNew(gimbalTask, nullptr, &gimbalTaskAttribute);
// shooterTaskHandle = osThreadNew(shooterTask, nullptr, &shooterTaskAttribute);
shooterTaskHandle = osThreadNew(shooterTask, nullptr, &shooterTaskAttribute);
// selfTestTaskHandle = osThreadNew(self_Check_Task, nullptr, &selfTestingTask);
}

Expand Down Expand Up @@ -710,8 +751,6 @@ void KillAll() {
//==================================================================================================
// RTOS Default Task
//==================================================================================================
// Debug signal
static bool debug = false;

void RM_RTOS_Default_Task(const void* args) {
UNUSED(args);
Expand All @@ -723,14 +762,11 @@ void RM_RTOS_Default_Task(const void* args) {
Dead = true;
KillAll();
}

if (debug) {
set_cursor(0, 0);
clear_screen();
print("power limit: %.3f chassis power: %.3f power buffer: %.3f\r\n", (float)referee->game_robot_status.chassis_power_limit,
referee->power_heat_data.chassis_power,
(float)referee->power_heat_data.chassis_power_buffer);
PreloadDetect.input(dbus->swl == remote::UP);
if (PreloadDetect.posEdge() && !Preload) {
Preload = true;
}

osDelay(DEFAULT_TASK_DELAY);
}
}