Final exam

ut-robomaster/src/communication/cv_board.cpp

@@ -24,6 +24,8 @@ void CVBoard::messageReceiveCallback(const ReceivedSerialMessage& message)
         case CV_MESSAGE_TYPE_TURRET_AIM:
             decodeTurretData(message);
             break;
+        // case CV_MESSAGE_TYPE_ECHO:
+        // echoData(message);
         default:
             break;
     }
@@ -35,6 +37,22 @@ void CVBoard::sendMessage()
     sendColorData();
 }
 
+// void CVBoard::echoData(const ReceivedSerialMessage& message)
+// {
+//     std::string data_string;
+//     memcpy(&data_string, &message.data, sizeof(message.header.dataLength));
+
+//     DJISerial::SerialMessage<sizeof(data_string)> msg;
+//     msg.messageType = CV_MESSAGE_TYPE_ECHO;
+
+//     std::string* data = reinterpret_cast<std::string*>(msg.data);
+
+//     data = &data_string;
+
+//     msg.setCRC16();
+//     drivers->uart.write(UART_PORT, reinterpret_cast<uint8_t*>(&msg), sizeof(msg));
+// }
+
 void CVBoard::sendOdometryData()
 {
     DJISerial::SerialMessage<sizeof(OdometryData)> message;

ut-robomaster/src/communication/cv_board.hpp

@@ -43,6 +43,11 @@ class CVBoard : public tap::communication::serial::DJISerial
      */
     void sendMessage();
 
+    /**
+     * Echo the message sent by the CV board to the CV board
+     */
+    // void echoData(const ReceivedSerialMessage& message);
+
     /**
      * Sends odometry data to the CV board
      */

ut-robomaster/src/communication/cv_message.hpp

@@ -1,6 +1,8 @@
 #pragma once
 #include <stdint.h>
 
+#include <string>
+
 namespace communication
 {
 
@@ -9,6 +11,7 @@ enum MessageTypes : uint8_t
     CV_MESSAGE_TYPE_ODOMETRY_DATA = 1,
     CV_MESSAGE_TYPE_TURRET_AIM = 2,
     CV_MESSAGE_TYPE_COLOR_DATA = 3,
+    CV_MESSAGE_TYPE_ECHO = 4,
 };
 
 enum ColorTypes : uint8_t

ut-robomaster/src/robots/hero/hero_constants.hpp

@@ -117,7 +117,8 @@ const MotorConfig
     FLYWHEEL_R{M3508_NOGEARBOX, MOTOR4, CAN_SHOOTER, true, "flywheel right", PID_FLYWHEEL, {}};
 
 // agitator
-const MotorConfig AGITATOR{M3508, MOTOR1, CAN_SHOOTER, false, "agitator", PID_AGITATOR, {}};
+const MotorConfig
+    AGITATOR{M3508, MOTOR1, CAN_SHOOTER, false, "agitator", PID_AGITATOR, {}};  // water wheel
 const MotorConfig FEEDER{M2006, MOTOR2, CAN_SHOOTER, false, "feeder", PID_FEEDER, {}};
 
 // turret
@@ -140,7 +141,7 @@ static constexpr float MAX_LINEAR_VEL = WHEEL_MAX_VEL * WHEEL_RADIUS;
 static constexpr float MAX_ANGULAR_VEL = WHEEL_MAX_VEL * WHEEL_RADIUS / WHEEL_LXY;  // rad/s
 
 const float TARGET_PROJECTILE_VELOCITY = 16;  // m/s
-const float FLYWHEEL_SPEED = 190.0f;
+const float FLYWHEEL_SPEED = 47.5f;
 
 const float BALLS_PER_SEC = 4.0f;
 const float BALLS_PER_REV = 6.0f;

ut-robomaster/src/robots/sentry/sentry_constants.hpp

@@ -25,8 +25,8 @@ const Sound SOUND_STARTUP = SOUND_SMB_POWERUP;
 
 // chassis ------------
 static constexpr int WHEELS = 4;
-static constexpr float WHEEL_DISTANCE_X = 0.391f;  // meters
-static constexpr float WHEEL_DISTANCE_Y = 0.315f;  // meters
+static constexpr float WHEEL_DISTANCE_X = 0.346f;  // meters
+static constexpr float WHEEL_DISTANCE_Y = 0.346f;  // meters
 static constexpr float WHEEL_RADIUS = 0.1524f;     // meters
 static constexpr float WHEEL_LXY = (WHEEL_DISTANCE_X + WHEEL_DISTANCE_Y) / 2.0f;
 

ut-robomaster/src/subsystems/agitator/agitator_subsystem.cpp

@@ -46,11 +46,12 @@ void AgitatorSubsystem::refresh()
     return;
 #endif
 
-    setAmputated(!hardwareOk());
+    // setAmputated(!hardwareOk());
 
     float time = getTimeMilliseconds() / 1000.0f;
     float velocity = getShapedVelocity(time, 1.0f, 0.0f, ballsPerSecond);
-    bool killSwitch = drivers->isKillSwitched() || isAmputated() || !flywheel->isActive();
+    // bool killSwitch = drivers->isKillSwitched() || isAmputated() || !flywheel->isActive();
+    bool killSwitch = drivers->isKillSwitched();
 
     agitator.setActive(!killSwitch);
     agitator.updateVelocity(velocity);

ut-robomaster/src/subsystems/chassis/chassis_subsystem.cpp

@@ -26,6 +26,7 @@ void ChassisSubsystem::initialize()
     {
         wheels[i].initialize();
     }
+    logTimer.stop();
 }
 
 void ChassisSubsystem::refresh()
@@ -106,7 +107,11 @@ void ChassisSubsystem::input(Vector2f move, float spin)
         overdrive -= correction;
     }
 
+#if defined(TARGET_SENTRY)
+    setOmniVelocities(v, wZ);
+#else
     setMecanumWheelVelocities(v, wZ);
+#endif
 }
 
 void ChassisSubsystem::setMecanumWheelVelocities(Vector2f v, float wZ)
@@ -118,6 +123,14 @@ void ChassisSubsystem::setMecanumWheelVelocities(Vector2f v, float wZ)
     targetWheelVels[3] = (-v.y - v.x - wZ * WHEEL_LXY) / WHEEL_RADIUS;  // rad/s
 }
 
+void ChassisSubsystem::setOmniVelocities(Vector2f v, float wZ)
+{
+    targetWheelVels[0] = (v.y + v.x + wZ) / WHEEL_RADIUS;  // rad/s
+    targetWheelVels[1] = (v.y - v.x - wZ) / WHEEL_RADIUS;  // rad/s
+    targetWheelVels[2] = (v.y - v.x + wZ) / WHEEL_RADIUS;  // rad/s
+    targetWheelVels[3] = (v.y + v.x - wZ) / WHEEL_RADIUS;  // rad/s
+}
+
 Vector3f ChassisSubsystem::measureVelocity()
 {
     float w1 = wheels[0].measureVelocity();  // rev/s

ut-robomaster/src/subsystems/chassis/chassis_subsystem.hpp

@@ -9,6 +9,7 @@
 #include "utils/power_limiter/power_limiter.hpp"
 
 #include "drivers.hpp"
+using tap::arch::MilliTimeout;
 
 namespace subsystems::chassis
 {
@@ -36,11 +37,14 @@ class ChassisSubsystem : public Subsystem
     /// @return x,y is linear velocity (m/s) and z is angular velocity (rad/s)
     Vector3f measureVelocity();
 
+    void setOmniVelocities(Vector2f v, float wZ);
+
 private:
     src::Drivers* drivers;
     power_limiter::PowerLimiter powerLimiter;
     MotorController wheels[WHEELS];
     float targetWheelVels[WHEELS] = {0.0f, 0.0f, 0.0f, 0.0f};
+    MilliTimeout logTimer;
 
     /// @brief Calculate and set wheel velocities for desired robot motion (based on
     /// https://research.ijcaonline.org/volume113/number3/pxc3901586.pdf).

ut-robomaster/src/subsystems/chassis/command_sentry_position.cpp

@@ -3,27 +3,143 @@
 namespace commands
 {
 
-void CommandSentryPosition::initialize() { moveTimer.stop(); }
+// old ard code
+// void CommandSentryPosition::initialize() { moveTimer.stop(); }
+
+void CommandSentryPosition::initialize() { keyboardInputMove = Vector2f(0.0f); }
 
 void CommandSentryPosition::execute()
 {
-    // wait until game starts then wait some more to avoid excess drifting
-    if (drivers->isGameActive() && moveTimer.isStopped())
+    float yawAngle = turret->getTargetLocalYaw();
+    Vector2f inputMove = Vector2f(0.0f);
+    float inputSpin = 0.0f;
+
+    // get keyboard input
+    float keyboardInputSpin;
+    bool hasKeyboardInput = applyKeyboardInput(keyboardInputMove, keyboardInputSpin);
+
+    // get joystick input
+    Vector2f joystickInputMove;
+    float joystickInputSpin;
+    bool hasJoystickInput = applyJoystickInput(joystickInputMove, joystickInputSpin);
+
+    // decide which input source to use (keyboard has inertia)
+    if (!hasKeyboardInput && hasJoystickInput)
+    {
+        inputMove = joystickInputMove;
+        inputSpin = joystickInputSpin;
+    }
+    else
+    {
+        inputMove = keyboardInputMove;
+        inputSpin = keyboardInputSpin;
+    }
+
+    // auto-align chassis to turret when moving
+    if (inputMove.getLengthSquared() > 0.0f && inputSpin == 0.0f)
+    {
+        inputSpin = calculateAutoAlignCorrection(yawAngle, CHASSIS_AUTOALIGN_ANGLE) *
+                    CHASSIS_AUTOALIGN_FACTOR;
+    }
+
+    // override spin input while beyblading
+    if (beyblade)
     {
-        moveTimer.restart(10'000);  // 10s
+        inputSpin = 1.0f;
     }
 
-    if (!moveTimer.isExpired())
+    // rotate movement vector relative to turret
+    if (turretRelative)
     {
-        chassis->input(Vector2f(0.0f), 0.0f);
-        return;
+        inputMove = inputMove.rotate(yawAngle);
     }
 
-    // speen
-    chassis->input(Vector2f(0.0f), 1.0f);
+    chassis->input(inputMove, inputSpin);
+
+    // if (drivers->isGameActive() && moveTimer.isStopped())
+    // {
+    //     moveTimer.restart(10'000);  // 10s
+    // }
+
+    // if (!moveTimer.isExpired())
+    // {
+    //     chassis->input(Vector2f(0.0f), 0.0f);
+    //     return;
+    // }
+
+    // // speen
+    // chassis->input(Vector2f(0.0f), 1.0f);
 }
 
 void CommandSentryPosition::end(bool) { chassis->input(Vector2f(0.0f), 0.0f); }
+
 bool CommandSentryPosition::isFinished() const { return false; }
 
+
+bool CommandSentryPosition::applyKeyboardInput(Vector2f &inputMove, float &inputSpin)
+{
+    Remote *remote = &drivers->remote;
+
+    inputSpin = 0.0f;  // no keyboard spin controls
+
+    Vector2f rawMoveInput = Vector2f(
+        remote->keyPressed(Remote::Key::D) - remote->keyPressed(Remote::Key::A),
+        remote->keyPressed(Remote::Key::W) - remote->keyPressed(Remote::Key::S));
+
+    float rawInputLen = rawMoveInput.getLength();
+
+    if (rawInputLen > 0.0f)
+    {
+        Vector2f moveDir = rawMoveInput / rawInputLen;  // normalize input
+        inputMove += moveDir * KEYBOARD_ACCEL * DT;     // incorporate input
+        inputMove /= max(1.0f, inputMove.getLength());  // clamp length
+    }
+    else
+    {
+        // decelerate when input stops
+        float len = inputMove.getLength();
+        if (len > 0.0f)
+        {
+            inputMove *= max(1.0f - KEYBOARD_DECEL * DT / len, 0.0f);
+        }
+    }
+
+    return rawMoveInput != Vector2f(0.0f);
+}
+
+
+
+bool CommandSentryPosition::applyJoystickInput(Vector2f &inputMove, float &inputSpin)
+{
+    Remote *remote = &drivers->remote;
+
+    inputMove = Vector2f(
+        remote->getChannel(Remote::Channel::RIGHT_HORIZONTAL),
+        remote->getChannel(Remote::Channel::RIGHT_VERTICAL));
+
+    inputSpin = remote->getChannel(Remote::Channel::WHEEL);
+
+    float inputMoveLen = inputMove.getLength();
+    if (inputMoveLen < ANALOG_DEAD_ZONE)
+    {
+        inputMove = Vector2f(0.0f);
+    }
+    else
+    {
+        inputMove /= max(1.0f, inputMoveLen);  // clamp length
+    }
+
+    if (abs(inputSpin) < ANALOG_DEAD_ZONE)
+    {
+        inputSpin = 0.0f;
+    }
+
+    // apply quadratic input ramping
+    inputMove *= inputMove.getLength();
+    inputSpin *= abs(inputSpin);
+
+    return inputMove != Vector2f(0.0f) || inputSpin != 0.0f;
+}
+
+
 }  // namespace commands

ut-robomaster/src/subsystems/chassis/command_sentry_position.hpp

@@ -1,24 +1,40 @@
 #pragma once
 
-#include "tap/architecture/timeout.hpp"
+//#include "tap/architecture/timeout.hpp"
 #include "tap/control/command.hpp"
 
-#include "chassis_subsystem.hpp"
+
+#include "robots/robot_constants.hpp"
+//this is older sentry code dont know why it had chopped path
+//#include "chassis_subsystem.hpp"  
+#include "subsystems/chassis/chassis_subsystem.hpp"
+#include "subsystems/turret/turret_subsystem.hpp"
+#include "utils/chassis_auto_align.hpp"
+
 #include "drivers.hpp"
 
 namespace commands
 {
 using namespace tap::communication::serial;
 using namespace modm;
 using subsystems::chassis::ChassisSubsystem;
-using tap::arch::MilliTimeout;
+// using tap::arch::MilliTimeout;
+using subsystems::turret::TurretSubsystem;
 
 class CommandSentryPosition : public tap::control::Command
 {
 public:
-    CommandSentryPosition(src::Drivers *drivers, ChassisSubsystem *chassis)
+    CommandSentryPosition(
+        src::Drivers *drivers,
+        ChassisSubsystem *chassis,
+        TurretSubsystem *turret,
+        bool turretRelative = false,
+        bool beyblade = false)
         : drivers(drivers),
-          chassis(chassis)
+          chassis(chassis),
+          turret(turret),
+          turretRelative(turretRelative),
+          beyblade(beyblade)
     {
         addSubsystemRequirement(chassis);
     }
@@ -36,6 +52,14 @@ class CommandSentryPosition : public tap::control::Command
 private:
     src::Drivers *drivers;
     ChassisSubsystem *chassis;
-    MilliTimeout moveTimer;
+    //MilliTimeout moveTimer;
+    TurretSubsystem *turret;
+
+    Vector2f keyboardInputMove = Vector2f(0.0f);
+    const bool turretRelative = false;
+    const bool beyblade = false;
+
+    bool applyKeyboardInput(Vector2f &moveOut, float &spinOut);
+    bool applyJoystickInput(Vector2f &moveOut, float &spinOut);
 };
 }  // namespace commands

ut-robomaster/src/subsystems/flywheel/flywheel_subsystem.cpp

@@ -41,7 +41,7 @@ void FlywheelSubsystem::refresh()
 
     for (int i = 0; i < FLYWHEELS; i++)
     {
-        motors[i].setActive(!killSwitch && !isAmputated());
+        motors[i].setActive(!killSwitch);
         motors[i].updateVelocity(velocity);
     }
 }