From 7bd862d1d41fd57dfe6aecf0bf51a98ae9ede934 Mon Sep 17 00:00:00 2001 From: drhieu Date: Sun, 23 Jul 2023 23:18:19 -0500 Subject: [PATCH] added battery voltage to older firmware rover pro and soc to rover zero --- include/status_data.hpp | 4 +-- src/protocol_pro.cpp | 54 ++++++++++++++++++++++++++------------ src/protocol_zero_2.cpp | 58 ++++++++++++++++++++++++----------------- 3 files changed, 73 insertions(+), 43 deletions(-) diff --git a/include/status_data.hpp b/include/status_data.hpp index c421893..aa0110e 100644 --- a/include/status_data.hpp +++ b/include/status_data.hpp @@ -31,8 +31,8 @@ struct robotData { signed short int battery2_temp; unsigned short int battery1_current; unsigned short int battery2_current; - bool battery1_SOC; - bool battery2_SOC; + unsigned short int battery1_SOC; + unsigned short int battery2_SOC; unsigned short int battery1_fault_flag; unsigned short int battery2_fault_flag; diff --git a/src/protocol_pro.cpp b/src/protocol_pro.cpp index cc9b107..16e6f37 100644 --- a/src/protocol_pro.cpp +++ b/src/protocol_pro.cpp @@ -20,7 +20,7 @@ ProProtocolObject::ProProtocolObject(const char *device, REG_MOTOR_FB_CURRENT_LEFT, REG_MOTOR_FB_CURRENT_RIGHT, REG_MOTOR_TEMP_LEFT, REG_MOTOR_TEMP_RIGHT, REG_MOTOR_CHARGER_STATE, BuildNO, - BATTERY_VOLTAGE_A}; + BATTERY_VOLTAGE_A, REG_PWR_BAT_VOLTAGE_A}; pid_ = pid; PidGains oldgain = {pid_.kp, pid_.ki, pid_.kd}; if (robot_mode_ != Control::OPEN_LOOP) @@ -51,9 +51,7 @@ void ProProtocolObject::send_estop(bool estop) { robotstatus_mutex_.unlock(); } -robotData ProProtocolObject::status_request() { - return robotstatus_; -} +robotData ProProtocolObject::status_request() { return robotstatus_; } robotData ProProtocolObject::info_request() { return robotstatus_; } @@ -163,7 +161,8 @@ void ProProtocolObject::unpack_comm_response(std::vector robotmsg) { msgqueue.clear(); return; } else { - // !Reconstruct the vector so that the start byte is at the 0 position + // !Reconstruct the vector so that the start byte is at the 0 + // position std::vector temp; for (int x = startbyte_index; x < msgqueue.size(); x++) { temp.push_back(msgqueue[x]); @@ -221,6 +220,9 @@ void ProProtocolObject::unpack_comm_response(std::vector robotmsg) { robotstatus_.motor2_temp = b; break; case REG_PWR_BAT_VOLTAGE_A: + if (robotstatus_.robot_firmware == 10009) { + robotstatus_.battery1_SOC = b; + } break; case REG_PWR_BAT_VOLTAGE_B: break; @@ -231,7 +233,9 @@ void ProProtocolObject::unpack_comm_response(std::vector robotmsg) { case EncoderInterval_2: break; case REG_ROBOT_REL_SOC_A: - robotstatus_.battery1_SOC = b; + if (robotstatus_.robot_firmware != OVF_FIXED_FIRM_VER_) { + robotstatus_.battery1_SOC = b; + } break; case REG_ROBOT_REL_SOC_B: break; @@ -257,28 +261,44 @@ void ProProtocolObject::unpack_comm_response(std::vector robotmsg) { case BATTERY_STATUS_B: break; case BATTERY_MODE_A: - robotstatus_.battery1_fault_flag = b; + if (robotstatus_.robot_firmware != OVF_FIXED_FIRM_VER_) { + robotstatus_.battery1_fault_flag = b; + } break; case BATTERY_MODE_B: - robotstatus_.battery2_fault_flag = b; + if (robotstatus_.robot_firmware != OVF_FIXED_FIRM_VER_) { + robotstatus_.battery2_fault_flag = b; + } break; case BATTERY_TEMP_A: - robotstatus_.battery1_temp = b; + if (robotstatus_.robot_firmware != OVF_FIXED_FIRM_VER_) { + robotstatus_.battery1_temp = b; + } break; case BATTERY_TEMP_B: - robotstatus_.battery2_temp = b; + if (robotstatus_.robot_firmware != OVF_FIXED_FIRM_VER_) { + robotstatus_.battery2_temp = b; + } break; case BATTERY_VOLTAGE_A: - robotstatus_.battery1_voltage = b; + if (robotstatus_.robot_firmware != OVF_FIXED_FIRM_VER_) { + robotstatus_.battery1_voltage = b; + } break; case BATTERY_VOLTAGE_B: - robotstatus_.battery2_voltage = b; + if (robotstatus_.robot_firmware != OVF_FIXED_FIRM_VER_) { + robotstatus_.battery2_voltage = b; + } break; case BATTERY_CURRENT_A: - robotstatus_.battery1_current = b; + if (robotstatus_.robot_firmware != OVF_FIXED_FIRM_VER_) { + robotstatus_.battery1_current = b; + } break; case BATTERY_CURRENT_B: - robotstatus_.battery2_current = b; + if (robotstatus_.robot_firmware != OVF_FIXED_FIRM_VER_) { + robotstatus_.battery2_current = b; + } break; } // !Same battery system for both A and B on this robot @@ -300,7 +320,7 @@ void ProProtocolObject::unpack_comm_response(std::vector robotmsg) { robotstatus_.motor4_mos_temp = 0; robotstatus_.robot_guid = 0; robotstatus_.robot_speed_limit = 0; - if (robotstatus_.robot_firmware == OVF_FIXED_FIRM_VER_) { // check firmware version + if (robotstatus_.robot_firmware == OVF_FIXED_FIRM_VER_) { robotstatus_.linear_vel = 0.5 * (robotstatus_.motor1_rpm * 2 / MOTOR_RPM_TO_MPS_RATIO_ + robotstatus_.motor2_rpm * 2 / MOTOR_RPM_TO_MPS_RATIO_); @@ -329,8 +349,8 @@ void ProProtocolObject::unpack_comm_response(std::vector robotmsg) { msgqueue.resize(0); msgqueue = temp; temp.clear(); - } else { // !Found start byte but the msg contents were invalid, throw away - // broken message + } else { // !Found start byte but the msg contents were invalid, throw + // away broken message std::vector temp; for (int x = 1; x < msgqueue.size(); x++) { temp.push_back(msgqueue[x]); diff --git a/src/protocol_zero_2.cpp b/src/protocol_zero_2.cpp index 64ca5aa..116cf27 100644 --- a/src/protocol_zero_2.cpp +++ b/src/protocol_zero_2.cpp @@ -50,36 +50,37 @@ Zero2ProtocolObject::Zero2ProtocolObject( {std::numeric_limits::max(), std::numeric_limits::max()}); robotmode_num_ = 0; } - + /* set up the comm port */ - + /* - + std::cerr << "establishing connection to rover zero..." << std::endl; try{ - + */ - + register_comm_base(device); - - /* - } - catch{ - std::cerr << "error establishing connection to Rover Zero, please check cabling and power to the motor controller (VESC)" << std::endl; - } - - */ - - + + /* +} +catch{ + std::cerr << "error establishing connection to Rover Zero, please check +cabling and power to the motor controller (VESC)" << std::endl; +} + +*/ + /* create a dedicated write thread to send commands to the robot on fixed * interval */ - /* std::cerr << "creating thread to communicate with rover zero..." << std::endl; */ + /* std::cerr << "creating thread to communicate with rover zero..." << + * std::endl; */ write_to_robot_thread_ = - + std::thread([this]() { this->send_getvalues_command(10); }); - - /* create a dedicate thread to compute the desired robot motion, runs on fixed - * interval */ + + /* create a dedicate thread to compute the desired robot motion, runs on + * fixed interval */ motor_speed_update_thread_ = std::thread([this]() { this->motors_control_loop(30); }); std::cerr << "protocol is running..." << std::endl; @@ -150,8 +151,8 @@ void Zero2ProtocolObject::motors_control_loop(int sleeptime) { robotstatus_mutex_.lock(); linear_vel_target = robotstatus_.cmd_linear_vel; angular_vel_target = robotstatus_.cmd_angular_vel; - /* Convert from motors to wheels RPM based on the robot geometry and gear - * ratio */ + /* Convert from motors to wheels RPM based on the robot geometry and + * gear ratio */ rpm_FL = robotstatus_.motor1_rpm / MOTOR_RPM_TO_WHEEL_RPM_RATIO_; rpm_FR = robotstatus_.motor2_rpm / MOTOR_RPM_TO_WHEEL_RPM_RATIO_; rpm_BL = robotstatus_.motor1_rpm / MOTOR_RPM_TO_WHEEL_RPM_RATIO_; @@ -352,7 +353,15 @@ void Zero2ProtocolObject::unpack_comm_response(std::vector robotmsg) { robotstatus_.battery2_temp = 0; robotstatus_.battery1_current = vesc_all_input_current_; robotstatus_.battery2_current = 0; - robotstatus_.battery1_SOC = 0; + if (robotstatus_.battery1_voltage >= 16) { + robotstatus_.battery1_SOC = 100; + } else if (robotstatus_.battery1_voltage >= 15) { + robotstatus_.battery1_SOC = 75; + } else if (robotstatus_.battery1_voltage >= 14) { + robotstatus_.battery1_SOC = 50; + } else if (robotstatus_.battery1_voltage >= 13) { + robotstatus_.battery1_SOC = 25; + } robotstatus_.battery2_SOC = 0; robotstatus_.battery1_fault_flag = 0; robotstatus_.battery2_fault_flag = 0; @@ -380,7 +389,8 @@ void Zero2ProtocolObject::unpack_comm_response(std::vector robotmsg) { msgqueue.clear(); return; } else { - // !Reconstruct the vector so that the start byte is at the 0 position + // !Reconstruct the vector so that the start byte is at the 0 + // position std::vector temp; for (int x = start_byte_index; x < msgqueue.size(); x++) { temp.push_back(msgqueue[x]);