motor_steer_robot.cpp

#include "motor_steer_robot.h"

void MotorSteerRobot::init() {
    CAN.begin(CanBitRate::BR_500k);
    // if (!CAN.begin(CanBitRate::BR_500k)) {
    //   return;
    // }

    while (! is_steer_connected) {
      sendSteerAngle(0, id_steer);

      requestSteerAngle(id_steer);
      readSensors();

      delay(50);
    }

    // RSP
    uint8_t buffer_rsp[3] = { 0x8C, 0x00, 0x00}; 
    buffer_rsp[2] = (id_steer + buffer_rsp[0] + buffer_rsp[1]) & 0xFF;
    CanMsg msg_rsp(CanStandardId(id_steer), sizeof(buffer_rsp), buffer_rsp);
    CAN.write(msg_rsp);
    delay(50);

    // Work Mode
    uint8_t buffer_mode[3] = { 0x82, 0x04, 0x00};
    buffer_mode[2] = (id_steer + buffer_mode[0] + buffer_mode[1]) & 0xFF;
    CanMsg msg_mode(CanStandardId(id_steer), sizeof(buffer_mode), buffer_mode);
    CAN.write(msg_mode);
    delay(100);

    // Max Current
    uint16_t max_current = 4400; // mA
    uint8_t buffer_current[4] = { 0x83,
                                  (uint8_t)((max_current >> 8) & 0xFF),
                                  (uint8_t)(max_current & 0xFF), 
                                  0x00};
    
    buffer_current[3] = (id_steer + buffer_current[0] + buffer_current[1] + buffer_current[2]) & 0xFF;
    CanMsg msg_current(CanStandardId(id_steer), sizeof(buffer_current), buffer_current);
    CAN.write(msg_current);
    delay(100);

    while (true) {
      sendSteerAngle(0, id_steer);

      requestSteerAngle(id_steer);
      readSensors();

      // -0.5 ~ 0.5 deg => init success
      if (current_steer_angle > -0.5 / scale_to_real_steer_deg &&
          current_steer_angle < 0.5 / scale_to_real_steer_deg) {
        break;
      } 
      delay(50);
    }
    is_connected = true;
}

void MotorSteerRobot::setup(float max_vel_, float max_steer_) {
  max_vel = max_vel_ * scale_to_comm_velocity;
  max_steer = max_steer_ * scale_to_comm_steer;
}

void MotorSteerRobot::loop() {
  checkConnected();
  requestSteerAngle(id_steer);
}

void MotorSteerRobot::checkConnected() {
  if (millis() - last_time_steer_received > 1000) {
    is_steer_connected = false;
  }
}

void MotorSteerRobot::sendCommand(CommandSteeringRobot cmd) {
    int velocity;
    int brake;
    int steer;

    switch (cmd.gear) {
    case GEAR_FORWARD:
        velocity = cmd.velocity;
        brake = cmd.brake;
        steer = cmd.steer;
        break;
    case GEAR_NEUTRAL:
        velocity = 0;
        brake = 0;
        steer = cmd.steer;
        break;
    case GEAR_REVERSE:
        velocity = -cmd.velocity;
        brake = cmd.brake;
        steer = cmd.steer;
        break;
    case GEAR_NONE:
    default:
        velocity = 0;
        brake = 200;
        steer = 0;
        break;
    }

    velocity = constrain(velocity, -max_vel, max_vel);
    steer = constrain(steer, -max_steer, max_steer);
    brake = constrain(brake, 0, 255);

    if (tick % 2 == 0){
      sendSteerAngle(steer, id_steer);
    }
    else {
      sendVelBrake(velocity, brake, id_drive_left);  // left motor drive 
      sendVelBrake(velocity, brake, id_drive_right); // right motor drive
    }
    
    tick++;
}

void MotorSteerRobot::sendVelBrake(int vel, int brake, uint8_t id_drive) {
    if (brake > 245) {
      // Brake
      int brake_scaled = 10000; // 10 A * 1000 scale
      uint8_t buffer_brake[4] = {
      (uint8_t)((brake_scaled >> 24) & 0xFF),
      (uint8_t)((brake_scaled >> 16) & 0xFF),
      (uint8_t)((brake_scaled >> 8) & 0xFF),
      (uint8_t)(brake_scaled & 0xFF)
      };
      
      uint32_t can_id_brake = (0x000C << 8) | id_drive; // handbrake
      arduino::CanMsg msg_brake(CanExtendedId(can_id_brake), sizeof(buffer_brake), buffer_brake);
      CAN.write(msg_brake);
    }

    else if (brake > 0) {
      // Brake
      int brake_scaled = brake * 100; // brake * 0.1 scaled (rf) * 1000 scaled (motor drvier)
      uint8_t buffer_brake[4] = {
      (uint8_t)((brake_scaled >> 24) & 0xFF),
      (uint8_t)((brake_scaled >> 16) & 0xFF),
      (uint8_t)((brake_scaled >> 8) & 0xFF),
      (uint8_t)(brake_scaled & 0xFF)
      };
      
      uint32_t can_id_brake = (0x0002 << 8) | id_drive;
      arduino::CanMsg msg_brake(CanExtendedId(can_id_brake), sizeof(buffer_brake), buffer_brake);
      CAN.write(msg_brake);
    }

    else {
      // Velocity
      int erpm = (float)vel * scale_to_real_velocity_mps * vel_to_rpm * rpm_to_erpm; // 24: pole

      uint8_t buffer_erpm[4] = {
                                (uint8_t)((erpm >> 24) & 0xFF),
                                (uint8_t)((erpm >> 16) & 0xFF),
                                (uint8_t)((erpm >> 8) & 0xFF),
                                (uint8_t)(erpm & 0xFF)
                              };

      uint32_t can_id_erpm = (0x0003 << 8) | id_drive;
      arduino::CanMsg msg_erpm(CanExtendedId(can_id_erpm), sizeof(buffer_erpm), buffer_erpm);
      CAN.write(msg_erpm);
      }
}

void MotorSteerRobot::sendSteerAngle(int steer_, uint32_t id_steer) {
  int32_t steer = (float)steer_ * scale_to_real_steer_deg * wheel_angle_to_motor_pulse;
  int speed = 1000;

  uint8_t buffer_steer[8] = {
    0xF5,
    (speed >> 8) & 0xFF,
    speed & 0xFF,
    1,                             //가속도(0~255)
    (uint8_t)((steer >> 16) & 0xFF),  //
    (uint8_t)((steer >> 8) & 0xFF),
    (uint8_t)(steer & 0xFF),
    0x00  //checksum
  };
  
  // delay(10);

  buffer_steer[7] = (id_steer + buffer_steer[0] + buffer_steer[1] + buffer_steer[2] 
                    + buffer_steer[3] + buffer_steer[4] + buffer_steer[5] + buffer_steer[6]) & 0xFF;
  arduino::CanMsg msg_steer(CanStandardId(id_steer), sizeof(buffer_steer), buffer_steer);
  CAN.write(msg_steer);
}

void MotorSteerRobot::requestSteerAngle(uint32_t id_steer_) {
  uint8_t msg_request[2] = { 0x31, 0x32 };  // can command: (0x31) + CRC(0x32)
  CanMsg msg_request_steer_angle(CanStandardId(id_steer_), sizeof(msg_request), msg_request);
  CAN.write(msg_request_steer_angle);
}

int MotorSteerRobot::readSteerAngle(CanMsg msg_read) {
  // 48-bit signed value 처리
  uint64_t raw_value = ((uint64_t)msg_read.data[1] << 40) |
                        ((uint64_t)msg_read.data[2] << 32) |
                        ((uint64_t)msg_read.data[3] << 24) |
                        ((uint64_t)msg_read.data[4] << 16) |
                        ((uint64_t)msg_read.data[5] << 8)  |
                        ((uint64_t)msg_read.data[6]);

  // 부호 확장 (48-bit signed → int64_t)
  if (raw_value & 0x800000000000) {  // MSB(47번째 비트)가 1이면 음수
    raw_value |= 0xFFFF000000000000; // 상위 16비트를 1로 채워 부호 확장
  }
  int64_t signed_value = (int64_t)raw_value;

  float steer_angle_deg = (float)signed_value / wheel_angle_to_motor_pulse;
  int steer_angle_scaled = (int)(steer_angle_deg * scale_to_comm_steer);
  
  // Serial.println("angle received");
  return steer_angle_scaled;
}

uint16_t MotorSteerRobot::readDriveVelocity(CanMsg msg_read) {
  int32_t erpm = ((int32_t)msg_read.data[0] << 24) |
                 ((int32_t)msg_read.data[1] << 16) |
                 ((int32_t)msg_read.data[2] << 8)  |
                 ((int32_t)msg_read.data[3]);

  float velocity_mps = (float)erpm / rpm_to_erpm / vel_to_rpm;
  // uint16_t veloicty_scaled = (int)(velocity_mps * scale_to_comm_velocity);
  uint16_t veloicty_scaled = (uint16_t)(abs(velocity_mps * scale_to_comm_velocity));

  return veloicty_scaled;
}

void MotorSteerRobot::readSensors() {
  if (! CAN.available()) {
    // Serial.println("CAN Not Available!");
    return;
  }

  CanMsg msg_read = CAN.read();
  uint32_t rx_id = msg_read.isStandardId() ? msg_read.getStandardId() : msg_read.getExtendedId();
  // Serial.print("CAN ID: ");
  // Serial.println(rx_id);

  if (rx_id == id_steer && msg_read.data[0] == 0x31) {
    current_steer_angle = readSteerAngle(msg_read);

    last_time_steer_received = millis();
    is_steer_connected = true;
  }

  // if (rx_id == id_steer) {
  //     Serial.println(msg_read.data[0]);
  // }

  if ((rx_id & 0xFF) == id_drive_left && (rx_id & 0xFFFFFF00) == 0x0900) {
    current_velocity_left = readDriveVelocity(msg_read);
  }

  if ((rx_id & 0xFF) == id_drive_right && (rx_id & 0xFFFFFF00) == 0x0900) {
    current_velocity_right = readDriveVelocity(msg_read);
  }
}