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

* [feat]: added servo for hero trigger Able to rotate from -90 (deg) to +70 (deg) * [fix]: adjusted accuracy for 0 deg * finish the chassis and gimbal on the hero(need test) * start add the shooter * test example * pass the load test and pitch test, need keyboard and mouse test * need test * most param need test * just leave the reload motor parameter * just leave the reload motor parameter * the data for the weak one is ready * pass the test in the match * add two shoot mode * the lock for the safe mode * fix the problem in pr --------- Co-authored-by: Ziyi Wang <anderprima54@gmail.com>
illini-robomaster · Apr 6, 2024 · d70528d · d70528d
1 parent 699ee03
commit d70528d
Show file tree

Hide file tree

Showing 15 changed files with 1,229 additions and 12 deletions.
diff --git a/boards/CMakeLists.txt b/boards/CMakeLists.txt
@@ -19,7 +19,7 @@
 # ---------------------------------------------------------------------- #
 
 # generic board interface
-add_library(board_interface INTERFACE)
+add_library(board_interface INTERFACE ../examples/motor/HERO_SERVO.cc)
 target_compile_definitions(board_interface INTERFACE USE_HAL_DRIVER)
 target_compile_options(board_interface INTERFACE
     $<$<COMPILE_LANGUAGE:ASM>:-x assembler-with-cpp>

diff --git a/examples/dbus/CMakeLists.txt b/examples/dbus/CMakeLists.txt
@@ -21,6 +21,6 @@
 project(example_dbus ASM C CXX)
 
 irm_add_arm_executable(${PROJECT_NAME}
-    TARGET DJI_Board_TypeA
+    TARGET DJI_Board_TypeC
     SOURCES main.cc)
 
diff --git a/examples/dbus/main.cc b/examples/dbus/main.cc
@@ -27,8 +27,8 @@
 static remote::DBUS* dbus;
 
 void RM_RTOS_Init(void) {
-  print_use_uart(&huart8);
-  dbus = new remote::DBUS(&huart1);
+  print_use_uart(&huart1);
+  dbus = new remote::DBUS(&huart3);
 }
 
 void RM_RTOS_Default_Task(const void* arguments) {

diff --git a/examples/hero/CMakeLists.txt b/examples/hero/CMakeLists.txt
@@ -23,3 +23,7 @@ project(example_hero ASM C CXX)
 irm_add_arm_executable(${PROJECT_NAME}
         TARGET DJI_Board_TypeA
         SOURCES main.cc)
+
+irm_add_arm_executable(${PROJECT_NAME}_shooter_2023
+        TARGET DJI_Board_TypeC
+        SOURCES hero_shooter_2023.cc)
diff --git a/examples/hero/hero_shooter_2023.cc b/examples/hero/hero_shooter_2023.cc
@@ -0,0 +1,257 @@
+/****************************************************************************
+ *                                                                          *
+ *  Copyright (C) 2023 RoboMaster.                                          *
+ *  Illini RoboMaster @ University of Illinois at Urbana-Champaign          *
+ *                                                                          *
+ *  This program 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.                                     *
+ *                                                                          *
+ *  This program 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 "chassis.h"
+
+#include "bsp_gpio.h"
+#include "bsp_os.h"
+#include "bsp_print.h"
+#include "cmsis_os.h"
+#include "controller.h"
+#include "dbus.h"
+#include "main.h"
+#include "motor.h"
+#include "protocol.h"
+#include "rgb.h"
+#include "oled.h"
+#include "bsp_buzzer.h"
+#include "bsp_laser.h"
+
+// Global Variables
+static remote::DBUS* dbus = nullptr;
+static bsp::CAN* can1 = nullptr;
+static bsp::CAN* can2 = nullptr;
+static display::RGB* RGB = nullptr;
+
+// Special Modes
+static BoolEdgeDetector LoadDetect(false);
+
+static control::MotorCANBase* force_motor = nullptr;
+static control::MotorCANBase* load_motor = nullptr;
+static control::MotorCANBase* reload_motor = nullptr;
+static control::PDIHV* trigger = nullptr; // CCW for positive angle, CW for negative angle
+static control::ServoMotor* load_servo = nullptr;
+static control::ServoMotor* reload_servo = nullptr;
+static control::ServoMotor* force_servo = nullptr;
+
+static BoolEdgeDetector ForceWeakDetect(false);
+static BoolEdgeDetector ForceStrongDetect(false);
+
+void RM_RTOS_Init() {
+  // peripherals initialization
+  print_use_uart(&huart1);
+  bsp::SetHighresClockTimer(&htim5);
+  // Dbus
+  dbus = new remote::DBUS(&huart3);
+  // Can
+  can1 = new bsp::CAN(&hcan1, true);
+  can2 = new bsp::CAN(&hcan2, false);
+  // RGB
+  RGB = new display::RGB(&htim5, 3, 2, 1, 1000000);
+
+  //Shooter initialization
+  load_motor = new control::Motor3508(can2, 0x201);
+  reload_motor = new control::Motor3508(can2, 0x202);
+  force_motor = new control::Motor3508(can2, 0x203);
+  // magic number from the data test for this servo
+  trigger = new control::PDIHV(&htim1, 1, 1980000, 500, 1500);
+
+  // Servo control for each shooter motor
+  control::servo_t servo_data;
+  servo_data.motor = load_motor;
+  servo_data.max_speed = 4 * PI; // params need test
+  servo_data.max_acceleration = 10 * PI;
+  servo_data.transmission_ratio = M3508P19_RATIO;
+  servo_data.omega_pid_param = new float [3] {150, 1.2, 5}; // pid need test
+  servo_data.max_iout = 1000;
+  servo_data.max_out = 13000;
+  load_servo = new control::ServoMotor(servo_data);
+
+  servo_data.motor = reload_motor;
+  servo_data.max_speed = 4 * PI; // params need test
+  servo_data.max_acceleration = 10 * PI;
+  servo_data.omega_pid_param = new float [3] {150, 15, 5}; // pid need test
+  reload_servo = new control::ServoMotor(servo_data);
+
+  servo_data.motor = force_motor;
+  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_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 = 3.24 * PI * 99.506 / M3508P19_RATIO; // 99.506 is the ratio of the reload servo, devide the 3508 ratio
+  // 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
+  int i = 0;
+  // force variable
+  bool force_weak = false;
+  bool force_strong = false;
+  bool force_transforming = false;
+  float force_pos = 0;
+  float force_angle = 55 * PI ;
+
+//   waiting for the start signal
+  while (true) {
+    if (dbus->keyboard.bit.B || dbus->swr == remote::DOWN) break;
+    osDelay(100);
+  }
+
+  while (true) {
+    // whether change the force motor position
+    ForceWeakDetect.input(dbus->keyboard.bit.F || dbus->wheel.wheel < 1024);
+    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_weak) {
+      print("weak");
+      force_transforming = true;
+      force_weak = true;
+      force_strong = false;
+    } else if (ForceStrongDetect.posEdge() && !ForceWeakDetect.posEdge() && !force_strong) {
+      print("strong");
+      force_transforming = true;
+      force_weak = false;
+      force_strong = true;
+    }
+    // force transforming
+    if (force_transforming) {
+      while (true) {
+        // break condition (reach the desire position)
+        if (++i > 10 && abs(force_motor->GetOmega()) <= 0.001) break;
+        // set the speed and acceleration for the reload motor
+        // set target pull position once
+        if (i == 1) {
+          // direction need test
+          if (force_weak && !force_strong) {
+            force_pos += force_angle;
+          } else if (force_strong && !force_weak) {
+            force_pos -= force_angle;
+          }
+          force_servo->SetTarget(force_pos);
+        }
+        force_servo->CalcOutput();
+        control::MotorCANBase::TransmitOutput(can2_force, 1);
+        osDelay(2);
+      }
+      // after changing the force motor position
+      i = 0;
+      force_transforming = false;
+      force_motor->SetOutput(0);
+      control::MotorCANBase::TransmitOutput(can2_force, 1);
+      osDelay(100); // need test the delay time(wait for the)
+    }
+    // the shoot and load process is automatic
+    // 1. using servo pull the trigger (shooting process)
+    // 2. reload motor pull the bullet board to the desire position (load process below)
+    // 3. using servo push the trigger the hold the bullet board
+    // 4. load motor load one bullet on the board
+    // 5. reload motor release to the original position to prepare for the next load
+    // 6. optional: determine whether we need to change the force motor position
+    // 7. repeat 1-6
+    i = 0;
+    // Load Detector
+    LoadDetect.input(dbus->swr == remote::UP);
+    if (LoadDetect.posEdge()) {
+      // step 1
+      trigger->SetOutPutAngle(0);  // need test the trigger angle
+      osDelay(100);                 // need test the delay time(wait for the)
+                                    // step 2
+      while (true) {
+        // break condition (reach the desire position)
+        if (++i > 50 / 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("target: %f", reload_servo->GetTarget());
+        }
+        print("reload theta: %f\n", reload_servo->GetTheta());
+        reload_servo->CalcOutput();
+        control::MotorCANBase::TransmitOutput(can2_reloader, 1);
+        osDelay(2);
+      }
+      // step 3
+      trigger->SetOutPutAngle(-80); // need test the trigger angle
+      osDelay(1700); // need test the delay time(wait for the)
+      // 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(50); // need test the delay time(wait for the)
+
+      // step 4
+      while (true) {
+        // break condition (loading)
+        if (++i > 50 / 2 && abs(load_servo->GetOmega()) <= 0.001) break;
+        // loading once
+        if (!loading) {
+          loading = true;
+          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);
+      loading = false;
+      osDelay(100); // need test the delay time(wait for the
+      // step 5
+      while (true) {
+        // break condition (reach the desire position)
+        if (++i > 50 / 2 && abs(reload_servo->GetOmega()) <= 0.001) break;
+        // set target push position once
+        if (!reload_push) {
+          reload_push = true;
+          reload_servo->SetTarget(0, true);
+        }
+        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);
+  }
+}
diff --git a/examples/motor/CMakeLists.txt b/examples/motor/CMakeLists.txt
@@ -79,3 +79,6 @@ irm_add_arm_executable(${PROJECT_NAME}_m3510
 irm_add_arm_executable(${PROJECT_NAME}_m3510_PID
         TARGET DJI_Board_TypeC
         SOURCES m3510_PID.cc)
+irm_add_arm_executable(${PROJECT_NAME}_HERO_SERVO
+        TARGET DJI_Board_TypeC
+        SOURCES HERO_SERVO.cc)
diff --git a/examples/motor/HERO_SERVO.cc b/examples/motor/HERO_SERVO.cc
@@ -0,0 +1,58 @@
+#include "bsp_gpio.h"
+#include "bsp_print.h"
+#include "cmsis_os.h"
+#include "main.h"
+#include "motor.h"
+
+/**
+ * @brief when installing, please use this example to set angle to 0.0 degree for calibration.
+ * The current program is functioning, however, might not be 100% accurate
+ */
+
+// All of these following parameters are tuned for this servo.
+uint8_t PWM_CHANNEL = 1;
+uint32_t TIM_CLOCK_FREQ = 1980000; /* TODO: could use more calibration if data is available*/
+// rule of thumb, TIM_CLOCK_FREQ could use more calibration if more data is available for PDI-HV5932
+uint32_t MOTOR_OUT_FREQ = 500; /* TODO: could use more calibration if data is available*/
+uint32_t PULSE_WIDTH = 1500;
+// PULSE_WIDTH when servo is idle
+
+bsp::GPIO* key = nullptr;
+
+#define KEY_GPIO_GROUP GPIOA
+#define KEY_GPIO_PIN GPIO_PIN_0
+
+control::PDIHV* motor1;
+
+void RM_RTOS_Init(){
+  print_use_uart(&huart1);
+  key = new bsp::GPIO(KEY_GPIO_GROUP, KEY_GPIO_PIN);
+  motor1 = new control::PDIHV(&htim1, PWM_CHANNEL,TIM_CLOCK_FREQ,MOTOR_OUT_FREQ, PULSE_WIDTH);
+//  motor1->SetOutput(1500);
+  osDelay(300);
+}
+
+void RM_RTOS_Default_Task(const void* args) {
+  UNUSED(args);
+  float angle = 0.0;
+  // angle could be set to 80 ~ -80 deg
+  int16_t power = 1947;
+  // power is from range 972 to 1947, data on purchasing page is not available, pulse width for central point is 1500
+  while(true){
+    motor1->SetOutPutAngle(angle);
+
+//    motor1->SetOutput(power);
+    if(!key->Read()){
+      angle += 10.0;
+      power -= 10;
+    }
+//    power += 10;
+    osDelay(2);
+//    angle += 1.0;
+    print("angle: %f\r\n", angle);
+    print("power: %d\r\n", power);
+    print("\r\n");
+
+
+  }
+  }
diff --git a/examples/motor/pwm.cc b/examples/motor/pwm.cc
@@ -31,8 +31,8 @@
 #define LEFT_MOTOR_PWM_CHANNEL 1
 #define RIGHT_MOTOR_PWM_CHANNEL 4
 #define TIM_CLOCK_FREQ 1000000
-#define MOTOR_OUT_FREQ 500
-#define SNAIL_IDLE_THROTTLE 1080
+#define MOTOR_OUT_FREQ 1500
+#define SNAIL_IDLE_THROTTLE 500
 
 control::MotorPWMBase* motor1;
 control::MotorPWMBase* motor2;

diff --git a/examples/relay/TypeC.cc b/examples/relay/TypeC.cc
@@ -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) {

diff --git a/shared/libraries/chassis.cc b/shared/libraries/chassis.cc
@@ -38,7 +38,7 @@ Chassis::Chassis(const chassis_t chassis) : pids_() {
       motors_[FourWheel::back_left] = chassis.motors[FourWheel::back_left];
       motors_[FourWheel::back_right] = chassis.motors[FourWheel::back_right];
 
-      float* pid_param = new float[3]{40, 3, 0};
+      float* pid_param = new float[3]{40, 6, 1};
       float motor_max_iout = 2000;
       float motor_max_out = 20000;
       pids_[FourWheel::front_left].Reinit(pid_param, motor_max_iout, motor_max_out);