Skip to content

added battery voltage to older firmware rover pro and soc to rover zero #31

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
drhieu wants to merge 1 commit into
base: release
Choose a base branch
from
Open

added battery voltage to older firmware rover pro and soc to rover zero #31

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions include/status_data.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down
54 changes: 37 additions & 17 deletions src/protocol_pro.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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_; }

Expand Down Expand Up @@ -163,7 +161,8 @@ void ProProtocolObject::unpack_comm_response(std::vector<uint8_t> 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<uint32_t> temp;
for (int x = startbyte_index; x < msgqueue.size(); x++) {
temp.push_back(msgqueue[x]);
Expand Down Expand Up @@ -221,6 +220,9 @@ void ProProtocolObject::unpack_comm_response(std::vector<uint8_t> 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;
Expand All @@ -231,7 +233,9 @@ void ProProtocolObject::unpack_comm_response(std::vector<uint8_t> 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;
Expand All @@ -257,28 +261,44 @@ void ProProtocolObject::unpack_comm_response(std::vector<uint8_t> 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
Expand All @@ -300,7 +320,7 @@ void ProProtocolObject::unpack_comm_response(std::vector<uint8_t> 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_);
Expand Down Expand Up @@ -329,8 +349,8 @@ void ProProtocolObject::unpack_comm_response(std::vector<uint8_t> 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<uint32_t> temp;
for (int x = 1; x < msgqueue.size(); x++) {
temp.push_back(msgqueue[x]);
Expand Down
58 changes: 34 additions & 24 deletions src/protocol_zero_2.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -50,36 +50,37 @@ Zero2ProtocolObject::Zero2ProtocolObject(
{std::numeric_limits<float>::max(), std::numeric_limits<float>::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;
Expand Down Expand Up @@ -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_;
Expand Down Expand Up @@ -352,7 +353,15 @@ void Zero2ProtocolObject::unpack_comm_response(std::vector<uint8_t> 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;
Expand Down Expand Up @@ -380,7 +389,8 @@ void Zero2ProtocolObject::unpack_comm_response(std::vector<uint8_t> 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<uint8_t> temp;
for (int x = start_byte_index; x < msgqueue.size(); x++) {
temp.push_back(msgqueue[x]);
Expand Down