Chassis.h

Go to the documentation of this file.

//
// Created by ankit on 1/31/23.
//
 
#ifndef TR_EMBEDDED_CHASSIS_H
#define TR_EMBEDDED_CHASSIS_H
 
#include <motor/DJIMotor.h>
#include <communications/CANHandler.h>
#include <peripherals/imu/BNO055.h>
#include <peripherals/oled/SSD1308.h>
#include <subsystems/ChassisKalman.h>
// #include <algorithms/WheelKalman.h>
#include <algorithms/Pose2D.h>
#include <algorithms/WheelSpeeds.h>
#include <algorithms/ChassisSpeeds.h>
 
#define CAN_BUS_TYPE CANHandler::CANBUS_1
#define MOTOR_TYPE M3508
#define INPUT_THRESHOLD 0.01
 
#define I2C_SDA PB_7
#define I2C_SCL PB_8
#define IMU_RESET PA_8
 
#define MAX_BEYBLADE_SPEED 1800
#define BEYBLADE_ACCELERATION 0.05
 
class Chassis
{
public:
    Chassis(short lfId, short rfId, short lbId, short rbId, I2C *i2c);
 
    enum BrakeMode
    {
        BRAKE,
        COAST
    };
 
    void driveMotors(WheelSpeeds speeds);
 
    void driveXYR(ChassisSpeeds speeds);
 
    void driveXYRPower(float chassis_power, uint16_t chassis_power_limit, double lX, double lY, double dt, bool beyblading, double &rotationalPower);
 
    void driveOffsetAnglePower(float chassis_power, uint16_t chassis_power_limit, ChassisSpeeds speeds, double angleOffset, int dt, double &rotationalPower);
 
    void driveTurretRelativePower(float chassis_power, uint16_t chassis_power_limit, ChassisSpeeds speeds, double turretAngleDegrees, int dt, double &rotationalPower);
 
    void driveFieldRelative(ChassisSpeeds speeds);
 
    void driveTurretRelative(ChassisSpeeds speeds, double turretAngleDegrees);
 
    void driveOffsetAngle(ChassisSpeeds speeds, double angleOffset);
 
    void driveAngle(double angleRadians, double speedRPM, double rotationVelcotiyRPM);
 
    void beyblade(double xVelocityRPM, double yVelocityRPM, double turretAngleDegrees, bool switchDirections);
 
    DJIMotor getMotor(int index);
 
    BrakeMode getBrakeMode();
 
    void setBrakeMode(BrakeMode brakeMode);
 
    void initializeImu();
 
    void periodic();
 
    void printMotorAngle();
 
    double degreesToRadians(double degrees);
 
    int getHeadingDegreesYaw();
 
    int getHeadingDegreesPitch();
 
    int getHeadingDegreesRoll();
 
    Pose2D getPose();
 
    ChassisSpeeds getSpeeds();
    void readImu();
 
 
 
    bool allMotorsConnected();
 
    int8_t isInverted[4];
 
    double prevVel;
 
    int testData[300][4];
    int testDataIndex = 0;
 
 
private:
    DJIMotor LF, RF, LB, RB;
    BrakeMode brakeMode;
 
    double beybladeSpeed;
    bool beybladeIncreasing;
    BNO055 imu;
    BNO055_ANGULAR_POSITION_typedef imuAngles;
 
    double rpmToTicksPerSecond(double RPM);
    double ticksPerSecondToRPM(double ticksPerSecond);
    double ticksPerSecondToInchesPerSecond(double ticksPerSecond);
    double rpmToInchesPerSecond(double RPM);
 
    WheelSpeeds chassisSpeedsToWheelSpeeds(ChassisSpeeds chassisSpeeds);
 
    void setMotorPower(int index, double power);
    void setMotorSpeedRPM(int index, double speed);
    void setMotorSpeedTicksPerSecond(int index, double speed);
 
    double getMotorSpeedRPM(int index);
 
    ChassisKalman chassisKalman;
    double testAngle;
    int lastTimeMs;
 
    short lfId;
    short rfId;
    short lbId;
    short rbId;
};
 
#endif // TR_EMBEDDED_CHASSIS_H