mirrors/ugv_sdk
1
0
Fork 0
mirror of https://github.com/westonrobot/ugv_sdk synced 2023-04-08 06:32:14 +08:00
Code Issues Packages Projects Releases Wiki Activity

major update to repo structure

Browse Source
This commit is contained in:
Ruixiang Du
2020-10-23 10:44:15 +08:00
parent f981dc4f36
commit 1bfa475ac0
5253 changed files with 302 additions and 757923 deletions
Download Patch File Download Diff File Expand all files Collapse all files

116
apps/scout_monitor/src/ncolors.cpp
View File

@@ -1,116 +0,0 @@
/*
* ncolors.cpp
*
* Created on: Jun 20, 2019 06:22
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#include "monitor/ncolors.hpp"
#include <iostream>
namespace
{
int IsBold(int fg)
{
/* return the intensity bit */
int i;
i = 1 << 3;
return (i & fg);
}
int ColorNum(int fg, int bg)
{
int B, bbb, ffff;
B = 1 << 7;
bbb = (7 & bg) << 4;
ffff = 7 & fg;
return (B | bbb | ffff);
}
short CursorColor(int fg)
{
switch (7 & fg)
{ /* RGB */
case 0: /* 000 */
return (COLOR_BLACK);
case 1: /* 001 */
return (COLOR_BLUE);
case 2: /* 010 */
return (COLOR_GREEN);
case 3: /* 011 */
return (COLOR_CYAN);
case 4: /* 100 */
return (COLOR_RED);
case 5: /* 101 */
return (COLOR_MAGENTA);
case 6: /* 110 */
return (COLOR_YELLOW);
case 7: /* 111 */
return (COLOR_WHITE);
default:
return COLOR_BLACK;
}
}
} // namespace
namespace westonrobot
{
void NColors::InitColors()
{
if (has_colors() != FALSE)
start_color();
else
std::cerr << "Your terminal does not support color" << std::endl;
int fg, bg;
int colorpair;
for (bg = 0; bg <= 7; bg++)
{
for (fg = 0; fg <= 7; fg++)
{
colorpair = ColorNum(fg, bg);
init_pair(colorpair, CursorColor(fg), CursorColor(bg));
}
}
}
void NColors::SetColor(int fg, int bg)
{
// set the color pair (ColorNum) and bold/bright (A_BOLD)
attron(COLOR_PAIR(ColorNum(fg, bg)));
if (IsBold(fg))
attron(A_BOLD);
}
void NColors::UnsetColor(int fg, int bg)
{
// unset the color pair (ColorNum) and bold/bright (A_BOLD)
attroff(COLOR_PAIR(ColorNum(fg, bg)));
if (IsBold(fg))
attroff(A_BOLD);
}
void NColors::WSetColor(WINDOW *win, int fg, int bg)
{
// set the color pair (ColorNum) and bold/bright (A_BOLD)
wattron(win, COLOR_PAIR(ColorNum(fg, bg)));
if (IsBold(fg))
wattron(win, A_BOLD);
}
void NColors::WUnsetColor(WINDOW *win, int fg, int bg)
{
// unset the color pair (ColorNum) and bold/bright (A_BOLD)
wattroff(win, COLOR_PAIR(ColorNum(fg, bg)));
if (IsBold(fg))
wattroff(win, A_BOLD);
}
} // namespace westonrobot

41
apps/scout_monitor/src/nshapes.cpp
View File

@@ -1,41 +0,0 @@
/*
* nshapers.cpp
*
* Created on: Jun 20, 2019 06:21
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#include "monitor/nshapes.hpp"
namespace westonrobot
{
void NShapes::DrawRectangle(int tl_y, int tl_x, int br_y, int br_x)
{
for (int i = tl_y; i <= br_y; ++i)
{
mvprintw(i, tl_x, "|");
mvprintw(i, br_x, "|");
}
for (int i = tl_x; i <= br_x; ++i)
{
mvprintw(tl_y, i, "-");
mvprintw(br_y, i, "-");
}
}
void NShapes::WDrawRectangle(WINDOW *win, int tl_y, int tl_x, int br_y, int br_x)
{
for (int i = tl_y; i <= br_y; ++i)
{
mvwprintw(win, i, tl_x, "|");
mvwprintw(win, i, br_x, "|");
}
for (int i = tl_x; i <= br_x; ++i)
{
mvwprintw(win, tl_y, i, "-");
mvwprintw(win, br_y, i, "-");
}
}
} // namespace westonrobot

658
apps/scout_monitor/src/scout_monitor.cpp
View File

@@ -1,658 +0,0 @@
/*
* scout_monitor.cpp
*
* Created on: Jun 12, 2019 01:19
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
/*
* Coordinate System:
*
* o --------------------> x
* |
* |
* |
* |
* |
* |
* v
* y
*/
#include "monitor/scout_monitor.hpp"
#include <cmath>
#include <iostream>
#include <sstream>
#include <iomanip>
#include <algorithm>
#include <array>
#include <chrono>
#include <cstdint>
#include <ratio>
#include <thread>
#include "monitor/nshapes.hpp"
#include "monitor/ncolors.hpp"
namespace
{
// reference: https://thispointer.com/c-convert-double-to-string-and-manage-precision-scientific-notation/
std::string ConvertFloatToString(double vel, int digit_num = 3)
{
std::ostringstream streamObj;
streamObj << std::fixed;
streamObj << std::setprecision(digit_num);
streamObj << vel;
return streamObj.str();
}
// source: https://github.com/rxdu/stopwatch
struct StopWatch
{
using Clock = std::chrono::high_resolution_clock;
using time_point = typename Clock::time_point;
using duration = typename Clock::duration;
StopWatch() { tic_point = Clock::now(); };
time_point tic_point;
void tic()
{
tic_point = Clock::now();
};
double toc()
{
return std::chrono::duration_cast<std::chrono::microseconds>(Clock::now() - tic_point).count() / 1000000.0;
};
// for different precisions
double stoc()
{
return std::chrono::duration_cast<std::chrono::seconds>(Clock::now() - tic_point).count();
};
double mtoc()
{
return std::chrono::duration_cast<std::chrono::milliseconds>(Clock::now() - tic_point).count();
};
double utoc()
{
return std::chrono::duration_cast<std::chrono::microseconds>(Clock::now() - tic_point).count();
};
double ntoc()
{
return std::chrono::duration_cast<std::chrono::nanoseconds>(Clock::now() - tic_point).count();
};
// you have to call tic() before calling this function
void sleep_until_ms(int64_t period_ms)
{
int64_t duration = period_ms - std::chrono::duration_cast<std::chrono::milliseconds>(Clock::now() - tic_point).count();
if (duration > 0)
std::this_thread::sleep_for(std::chrono::milliseconds(duration));
};
void sleep_until_us(int64_t period_us)
{
int64_t duration = period_us - std::chrono::duration_cast<std::chrono::microseconds>(Clock::now() - tic_point).count();
if (duration > 0)
std::this_thread::sleep_for(std::chrono::microseconds(duration));
};
};
} // namespace
namespace westonrobot
{
ScoutMonitor::ScoutMonitor()
{
// init ncurses
initscr();
// raw();
cbreak();
noecho();
nonl();
curs_set(FALSE);
intrflush(stdscr, FALSE);
keypad(stdscr, TRUE);
CalcDimensions();
// setup sub-windows
body_info_win_ = newwin(bi_win_sy_, bi_win_sx_, bi_origin_y_, bi_origin_x_);
system_info_win_ = newwin(si_win_sy_, si_win_sx_, si_origin_y_, si_origin_x_);
scout_state_.linear_velocity = 0;
scout_state_.angular_velocity = 0;
NColors::InitColors();
}
ScoutMonitor::~ScoutMonitor()
{
delwin(body_info_win_);
delwin(system_info_win_);
endwin();
}
void ScoutMonitor::UpdateAll()
{
ClearAll();
CalcDimensions();
if (resizing_detected_)
HandleResizing();
UpdateScoutBodyInfo();
UpdateScoutSystemInfo();
}
void ScoutMonitor::SetTestStateData()
{
scout_state_.base_state = BASE_STATE_NORMAL;
scout_state_.battery_voltage = 28.5;
scout_state_.linear_velocity = 1.234;
scout_state_.angular_velocity = -0.6853;
// scout_state_.fault_code |= FAULT_MOTOR_DRV_OVERHEAT_W;
// scout_state_.fault_code |= FAULT_MOTOR_OVERCURRENT_W;
// scout_state_.fault_code |= FAULT_MOTOR_DRV_OVERHEAT_F;
// scout_state_.fault_code |= FAULT_MOTOR_OVERCURRENT_F;
// scout_state_.fault_code |= FAULT_BAT_UNDER_VOL_W;
// scout_state_.fault_code |= FAULT_BAT_UNDER_VOL_F;
scout_state_.fault_code = 0x0000;
// scout_state_.fault_code = 0xffff;
// scout_state_.front_light_state.mode = CONST_ON;
scout_state_.front_light_state.mode = LIGHT_MODE_CUSTOM;
scout_state_.front_light_state.custom_value = 50;
scout_state_.rear_light_state.mode = LIGHT_MODE_CONST_ON;
scout_state_.motor_states[0].current = 10.1;
scout_state_.motor_states[0].rpm = 2000;
scout_state_.motor_states[0].temperature = 35;
scout_state_.motor_states[1].current = 10.1;
scout_state_.motor_states[1].rpm = 2000;
scout_state_.motor_states[1].temperature = 35;
scout_state_.motor_states[2].current = 10.1;
scout_state_.motor_states[2].rpm = 2000;
scout_state_.motor_states[2].temperature = 35;
scout_state_.motor_states[3].current = 10.1;
scout_state_.motor_states[3].rpm = 2000;
scout_state_.motor_states[3].temperature = 35;
}
void ScoutMonitor::Run(std::string device_name, int32_t baud_rate)
{
if (device_name != "")
test_mode_ = false;
if (test_mode_)
SetTestStateData();
else
scout_base_.Connect(device_name, baud_rate);
StopWatch sw;
while (keep_running_)
{
// label starting point of iteration
sw.tic();
// query for latest robot state
if (!test_mode_)
scout_state_ = scout_base_.GetScoutState();
// update window contents
UpdateAll();
// manage window refresh rate
sw.sleep_until_ms(100);
}
}
void ScoutMonitor::CalcDimensions()
{
int sy, sx;
getmaxyx(stdscr, sy, sx);
if (sy != term_sy_ || sx != term_sx_)
{
resizing_detected_ = true;
term_sy_ = sy;
term_sx_ = sx;
bi_win_sy_ = term_sy_;
bi_win_sx_ = term_sx_ * 15 / 24;
bi_origin_y_ = 0;
bi_origin_x_ = 0;
si_win_sy_ = term_sy_;
si_win_sx_ = term_sx_ * 9 / 24;
si_origin_y_ = 0;
si_origin_x_ = bi_win_sx_;
}
}
void ScoutMonitor::HandleResizing()
{
delwin(body_info_win_);
delwin(system_info_win_);
body_info_win_ = newwin(bi_win_sy_, bi_win_sx_, bi_origin_y_, bi_origin_x_);
system_info_win_ = newwin(si_win_sy_, si_win_sx_, si_origin_y_, si_origin_x_);
resizing_detected_ = false;
}
void ScoutMonitor::ClearAll()
{
wclear(body_info_win_);
wclear(system_info_win_);
}
void ScoutMonitor::ShowVehicleState(int y, int x)
{
// show linear velocity
const int linear_axis_x = x + vehicle_fp_offset_x_;
const int linear_axis_tip_y = y + 2;
const int linear_axis_origin_y = linear_axis_tip_y + linear_axis_length_;
const int linear_axis_negative_y = linear_axis_origin_y + linear_axis_length_ + 1;
mvwprintw(body_info_win_, linear_axis_tip_y - 1, linear_axis_x, "^");
for (int i = linear_axis_tip_y; i < linear_axis_origin_y; ++i)
mvwprintw(body_info_win_, i, linear_axis_x, "-");
mvwprintw(body_info_win_, linear_axis_origin_y, linear_axis_x, "x");
for (int i = linear_axis_origin_y + 1; i < linear_axis_negative_y; ++i)
mvwprintw(body_info_win_, i, linear_axis_x, "-");
mvwprintw(body_info_win_, linear_axis_negative_y, linear_axis_x, "v");
double linear_percentage = scout_state_.linear_velocity / ScoutMotionCmd::max_linear_velocity;
int linear_bars = std::abs(static_cast<int>(linear_percentage * 5)) + 1;
if (std::abs(scout_state_.linear_velocity) < 0.001)
linear_bars = 0;
if (linear_bars > 5)
linear_bars = 5;
if (scout_state_.linear_velocity > 0)
{
for (int i = linear_axis_origin_y - linear_bars; i < linear_axis_origin_y; ++i)
{
NColors::WSetColor(body_info_win_, NColors::BLACK, NColors::CYAN);
mvwprintw(body_info_win_, i, linear_axis_x, "-");
NColors::WUnsetColor(body_info_win_, NColors::BLACK, NColors::CYAN);
}
}
else if (scout_state_.linear_velocity < 0)
{
for (int i = linear_axis_origin_y + linear_bars; i > linear_axis_origin_y; --i)
{
NColors::WSetColor(body_info_win_, NColors::BLACK, NColors::CYAN);
mvwprintw(body_info_win_, i, linear_axis_x, "-");
NColors::WUnsetColor(body_info_win_, NColors::BLACK, NColors::CYAN);
}
}
// show angular velocity
const int angular_axis_y = linear_axis_origin_y;
const int angular_axis_origin_x = linear_axis_x;
const int angular_axis_positive_x = angular_axis_origin_x + angular_axis_length_ + 1;
const int angular_axis_negative_x = angular_axis_origin_x - angular_axis_length_;
mvwprintw(body_info_win_, angular_axis_y, angular_axis_negative_x - 1, "<");
for (int i = angular_axis_negative_x; i < angular_axis_origin_x; ++i)
mvwprintw(body_info_win_, angular_axis_y, i, "-");
mvwprintw(body_info_win_, linear_axis_origin_y, linear_axis_x, "x");
for (int i = angular_axis_origin_x + 1; i < angular_axis_positive_x; ++i)
mvwprintw(body_info_win_, angular_axis_y, i, "-");
mvwprintw(body_info_win_, angular_axis_y, angular_axis_positive_x, ">");
double angular_percentage = scout_state_.angular_velocity / ScoutMotionCmd::max_angular_velocity;
int angular_bars = std::abs(static_cast<int>(angular_percentage * 5)) + 1;
if (std::abs(scout_state_.angular_velocity) < 0.001)
angular_bars = 0;
if (angular_bars > 5)
angular_bars = 5;
if (scout_state_.angular_velocity < 0)
{
for (int i = angular_axis_origin_x + angular_bars; i > angular_axis_origin_x; --i)
{
NColors::WSetColor(body_info_win_, NColors::BLACK, NColors::MAGENTA);
mvwprintw(body_info_win_, angular_axis_y, i, "-");
NColors::WUnsetColor(body_info_win_, NColors::BLACK, NColors::MAGENTA);
}
}
else if (scout_state_.angular_velocity > 0)
{
for (int i = angular_axis_origin_x - angular_bars; i < angular_axis_origin_x; ++i)
{
NColors::WSetColor(body_info_win_, NColors::BLACK, NColors::MAGENTA);
mvwprintw(body_info_win_, angular_axis_y, i, "-");
NColors::WUnsetColor(body_info_win_, NColors::BLACK, NColors::MAGENTA);
}
}
// show velocity values
std::string linear_vel_str = "linear : " + ConvertFloatToString(scout_state_.linear_velocity);
mvwprintw(body_info_win_, linear_axis_negative_y + 2, angular_axis_negative_x - 2, linear_vel_str.c_str());
std::string angular_vel_str = "angular: " + ConvertFloatToString(scout_state_.angular_velocity);
mvwprintw(body_info_win_, linear_axis_negative_y + 3, angular_axis_negative_x - 2, angular_vel_str.c_str());
// show vehicle base
NShapes::WDrawRectangle(body_info_win_, linear_axis_tip_y - 2, angular_axis_negative_x - 4,
linear_axis_negative_y + 4, angular_axis_positive_x + 3);
// show vehicle wheels
NShapes::WDrawRectangle(body_info_win_, linear_axis_tip_y - 1, angular_axis_negative_x - 9,
linear_axis_tip_y + 4, angular_axis_negative_x - 5);
NShapes::WDrawRectangle(body_info_win_, linear_axis_negative_y - 2, angular_axis_negative_x - 9,
linear_axis_negative_y + 3, angular_axis_negative_x - 5);
NShapes::WDrawRectangle(body_info_win_, linear_axis_tip_y - 1, angular_axis_positive_x + 4,
linear_axis_tip_y + 4, angular_axis_positive_x + 8);
NShapes::WDrawRectangle(body_info_win_, linear_axis_negative_y - 2, angular_axis_positive_x + 4,
linear_axis_negative_y + 3, angular_axis_positive_x + 8);
// front right motor
ShowMotorInfo(linear_axis_tip_y - 1, angular_axis_positive_x + 4, scout_state_.motor_states[0].current,
scout_state_.motor_states[0].rpm, scout_state_.motor_states[0].temperature, true);
// front left motor
ShowMotorInfo(linear_axis_tip_y - 1, angular_axis_negative_x - 9, scout_state_.motor_states[1].current,
scout_state_.motor_states[1].rpm, scout_state_.motor_states[1].temperature, false);
// rear left motor
ShowMotorInfo(linear_axis_negative_y - 2, angular_axis_negative_x - 9, scout_state_.motor_states[2].current,
scout_state_.motor_states[2].rpm, scout_state_.motor_states[2].temperature, false);
// rear right motor
ShowMotorInfo(linear_axis_negative_y - 2, angular_axis_positive_x + 4, scout_state_.motor_states[3].current,
scout_state_.motor_states[3].rpm, scout_state_.motor_states[3].temperature, true);
// show vehicle lights
std::string front_mode_str = "Mode: ";
if (scout_state_.front_light_state.mode == LIGHT_MODE_CONST_ON)
front_mode_str += "ON";
else if (scout_state_.front_light_state.mode == LIGHT_MODE_CONST_OFF)
front_mode_str += "OFF";
else if (scout_state_.front_light_state.mode == LIGHT_MODE_BREATH)
front_mode_str += "BREATH";
else if (scout_state_.front_light_state.mode == LIGHT_MODE_CUSTOM)
front_mode_str += "CUSTOM";
mvwprintw(body_info_win_, linear_axis_tip_y - 4, angular_axis_origin_x - 13, front_mode_str.c_str());
std::string front_custom_str = "Custom: " + ConvertFloatToString(scout_state_.front_light_state.custom_value, 0);
mvwprintw(body_info_win_, linear_axis_tip_y - 4, angular_axis_origin_x + 3, front_custom_str.c_str());
if (scout_state_.front_light_state.mode != LIGHT_MODE_CONST_OFF &&
!(scout_state_.front_light_state.mode == LIGHT_MODE_CUSTOM && scout_state_.front_light_state.custom_value == 0))
{
NColors::WSetColor(body_info_win_, NColors::BRIGHT_YELLOW);
for (int i = angular_axis_origin_x - 5; i < angular_axis_origin_x - 1; ++i)
mvwprintw(body_info_win_, linear_axis_tip_y - 3, i, "v");
mvwprintw(body_info_win_, linear_axis_tip_y - 3, angular_axis_origin_x, "v");
for (int i = angular_axis_origin_x + 2; i <= angular_axis_origin_x + 5; ++i)
mvwprintw(body_info_win_, linear_axis_tip_y - 3, i, "v");
NColors::WUnsetColor(body_info_win_, NColors::BRIGHT_YELLOW);
}
std::string rear_mode_str = "Mode: ";
if (scout_state_.rear_light_state.mode == LIGHT_MODE_CONST_ON)
rear_mode_str += "ON";
else if (scout_state_.rear_light_state.mode == LIGHT_MODE_CONST_OFF)
rear_mode_str += "OFF";
else if (scout_state_.rear_light_state.mode == LIGHT_MODE_BREATH)
rear_mode_str += "BREATH";
else if (scout_state_.rear_light_state.mode == LIGHT_MODE_CUSTOM)
rear_mode_str += "CUSTOM";
mvwprintw(body_info_win_, linear_axis_negative_y + 6, angular_axis_origin_x - 13, rear_mode_str.c_str());
std::string rear_custom_str = "Custom: " + ConvertFloatToString(scout_state_.rear_light_state.custom_value, 0);
mvwprintw(body_info_win_, linear_axis_negative_y + 6, angular_axis_origin_x + 3, rear_custom_str.c_str());
if (scout_state_.rear_light_state.mode != LIGHT_MODE_CONST_OFF &&
!(scout_state_.rear_light_state.mode == LIGHT_MODE_CUSTOM && scout_state_.rear_light_state.custom_value == 0))
{
NColors::WSetColor(body_info_win_, NColors::BRIGHT_RED);
for (int i = angular_axis_origin_x - 5; i < angular_axis_origin_x - 1; ++i)
mvwprintw(body_info_win_, linear_axis_negative_y + 5, i, "^");
mvwprintw(body_info_win_, linear_axis_negative_y + 5, angular_axis_origin_x, "^");
for (int i = angular_axis_origin_x + 2; i <= angular_axis_origin_x + 5; ++i)
mvwprintw(body_info_win_, linear_axis_negative_y + 5, i, "^");
NColors::WUnsetColor(body_info_win_, NColors::BRIGHT_RED);
}
}
void ScoutMonitor::UpdateScoutBodyInfo()
{
// for (int i = 0; i < bi_win_sx_; i++)
// mvwprintw(body_info_win_, bi_win_sy_ - 1, i, "-");
ShowVehicleState(bi_win_sy_ / 2 - vehicle_fp_offset_y_, bi_win_sx_ / 2 - vehicle_fp_offset_x_);
wrefresh(body_info_win_);
}
void ScoutMonitor::UpdateScoutSystemInfo()
{
for (int i = 0; i < si_win_sy_; i++)
mvwprintw(system_info_win_, i, 0, "|");
const int state_title_col = (si_win_sx_ - 24) / 2;
const int state_value_col = state_title_col + 20;
const int state_div_col = state_value_col - 2;
// system state
const int sec1 = static_cast<int>(std::round((si_win_sy_ - 20) / 2.0));
ShowStatusItemName(sec1, state_title_col, "System state");
if (scout_state_.base_state == BASE_STATE_NORMAL)
{
NColors::WSetColor(system_info_win_, NColors::GREEN);
mvwprintw(system_info_win_, sec1, state_value_col, "NORMAL");
NColors::WUnsetColor(system_info_win_, NColors::GREEN);
}
else if (scout_state_.base_state == BASE_STATE_ESTOP)
{
NColors::WSetColor(system_info_win_, NColors::YELLOW);
mvwprintw(system_info_win_, sec1, state_value_col, "ESTOP");
NColors::WUnsetColor(system_info_win_, NColors::YELLOW);
}
else if (scout_state_.base_state == BASE_STATE_EXCEPTION)
{
NColors::WSetColor(system_info_win_, NColors::RED);
mvwprintw(system_info_win_, sec1, state_value_col, "EXCEPT");
NColors::WUnsetColor(system_info_win_, NColors::RED);
}
// control mode
ShowStatusItemName(sec1 + 1, state_title_col, "Control mode");
if (scout_state_.control_mode == CTRL_MODE_REMOTE)
mvwprintw(system_info_win_, sec1 + 1, state_value_col, "REMOTE");
else if (scout_state_.control_mode == CTRL_MODE_CMD_CAN)
mvwprintw(system_info_win_, sec1 + 1, state_value_col, "CMD");
else if (scout_state_.control_mode == CTRL_MODE_CMD_UART)
mvwprintw(system_info_win_, sec1 + 1, state_value_col, "CMD");
// mvwprintw(system_info_win_, sec1 + 1, state_value_col, std::to_string(scout_state_.control_mode).c_str());
// battery voltage
ShowStatusItemName(sec1 + 2, state_title_col, "Battery voltage");
std::string bat_vol_str = ConvertFloatToString(scout_state_.battery_voltage, 1) + " v";
mvwprintw(system_info_win_, sec1 + 2, state_value_col, bat_vol_str.c_str());
const int fault_col_1 = state_value_col;
const int fault_col_2 = fault_col_1 + 2;
const int fault_col_3 = fault_col_2 + 2;
const int sec2 = sec1 + 4;
mvwprintw(system_info_win_, sec2, state_title_col, "System faults");
// motor driver over heat;
ShowStatusItemName(sec2 + 1, state_title_col, "-Drv over-heat");
if ((scout_state_.fault_code & FAULT_MOTOR_DRV_OVERHEAT_W) == 0 &&
(scout_state_.fault_code & FAULT_MOTOR_DRV_OVERHEAT_F) == 0)
{
NColors::WSetColor(system_info_win_, NColors::GREEN);
mvwprintw(system_info_win_, sec2 + 1, fault_col_1, "N");
NColors::WUnsetColor(system_info_win_, NColors::GREEN);
}
else
{
if (scout_state_.fault_code & FAULT_MOTOR_DRV_OVERHEAT_W)
{
NColors::WSetColor(system_info_win_, NColors::YELLOW);
mvwprintw(system_info_win_, sec2 + 1, fault_col_2, "W");
NColors::WUnsetColor(system_info_win_, NColors::YELLOW);
}
if (scout_state_.fault_code & FAULT_MOTOR_DRV_OVERHEAT_F)
{
NColors::WSetColor(system_info_win_, NColors::RED);
mvwprintw(system_info_win_, sec2 + 1, fault_col_3, "P");
NColors::WUnsetColor(system_info_win_, NColors::RED);
}
}
// motor driver over current
ShowStatusItemName(sec2 + 2, state_title_col, "-Mt over-current");
if ((scout_state_.fault_code & FAULT_MOTOR_OVERCURRENT_W) == 0 &&
(scout_state_.fault_code & FAULT_MOTOR_OVERCURRENT_F) == 0)
{
NColors::WSetColor(system_info_win_, NColors::GREEN);
mvwprintw(system_info_win_, sec2 + 2, fault_col_1, "N");
NColors::WUnsetColor(system_info_win_, NColors::GREEN);
}
else
{
if (scout_state_.fault_code & FAULT_MOTOR_OVERCURRENT_W)
{
NColors::WSetColor(system_info_win_, NColors::YELLOW);
mvwprintw(system_info_win_, sec2 + 2, fault_col_2, "W");
NColors::WUnsetColor(system_info_win_, NColors::YELLOW);
}
if (scout_state_.fault_code & FAULT_MOTOR_OVERCURRENT_F)
{
NColors::WSetColor(system_info_win_, NColors::RED);
mvwprintw(system_info_win_, sec2 + 2, fault_col_3, "P");
NColors::WUnsetColor(system_info_win_, NColors::RED);
}
}
// battery under voltage
ShowStatusItemName(sec2 + 3, state_title_col, "-Bat under volt");
if ((scout_state_.fault_code & FAULT_BAT_UNDER_VOL_W) == 0 &&
(scout_state_.fault_code & FAULT_BAT_UNDER_VOL_F) == 0)
{
NColors::WSetColor(system_info_win_, NColors::GREEN);
mvwprintw(system_info_win_, sec2 + 3, fault_col_1, "N");
NColors::WUnsetColor(system_info_win_, NColors::GREEN);
}
else
{
if (scout_state_.fault_code & FAULT_BAT_UNDER_VOL_W)
{
NColors::WSetColor(system_info_win_, NColors::YELLOW);
mvwprintw(system_info_win_, sec2 + 3, fault_col_2, "W");
NColors::WUnsetColor(system_info_win_, NColors::YELLOW);
}
if (scout_state_.fault_code & FAULT_BAT_UNDER_VOL_F)
{
NColors::WSetColor(system_info_win_, NColors::RED);
mvwprintw(system_info_win_, sec2 + 3, fault_col_3, "F");
NColors::WUnsetColor(system_info_win_, NColors::RED);
}
}
// battery over voltage
ShowStatusItemName(sec2 + 4, state_title_col, "-Bat over volt");
ShowFault(sec2 + 4, fault_col_1, (scout_state_.fault_code & FAULT_BAT_OVER_VOL_F) == 0);
ShowStatusItemName(sec2 + 5, state_title_col, "-RC signal loss");
ShowFault(sec2 + 5, fault_col_1, (scout_state_.fault_code & FAULT_RC_SIGNAL_LOSS) == 0);
const int sec3 = sec2 + 7;
mvwprintw(system_info_win_, sec3, state_title_col, "Comm faults");
// CAN cmd checksum
ShowStatusItemName(sec3 + 1, state_title_col, "-CAN cmd checksum");
ShowFault(sec3 + 1, fault_col_1, (scout_state_.fault_code & FAULT_CAN_CHECKSUM_ERROR) == 0);
// motor comm
ShowStatusItemName(sec3 + 2, state_title_col, "-Motor 1 comm");
ShowFault(sec3 + 2, fault_col_1, (scout_state_.fault_code & FAULT_MOTOR1_COMM_F) == 0);
ShowStatusItemName(sec3 + 3, state_title_col, "-Motor 2 comm");
ShowFault(sec3 + 3, fault_col_1, (scout_state_.fault_code & FAULT_MOTOR2_COMM_F) == 0);
ShowStatusItemName(sec3 + 4, state_title_col, "-Motor 3 comm");
ShowFault(sec3 + 4, fault_col_1, (scout_state_.fault_code & FAULT_MOTOR3_COMM_F) == 0);
ShowStatusItemName(sec3 + 5, state_title_col, "-Motor 4 comm");
ShowFault(sec3 + 5, fault_col_1, (scout_state_.fault_code & FAULT_MOTOR4_COMM_F) == 0);
const int sec4 = sec3 + 8;
NColors::WSetColor(system_info_win_, NColors::GREEN);
mvwprintw(system_info_win_, sec4, state_title_col + 1, "N: Normal");
NColors::WUnsetColor(system_info_win_, NColors::GREEN);
NColors::WSetColor(system_info_win_, NColors::YELLOW);
mvwprintw(system_info_win_, sec4, state_title_col + 12, "W: Warning");
NColors::WUnsetColor(system_info_win_, NColors::YELLOW);
NColors::WSetColor(system_info_win_, NColors::RED);
mvwprintw(system_info_win_, sec4 + 1, state_title_col + 1, "F: Fault P: Protection");
NColors::WUnsetColor(system_info_win_, NColors::RED);
wrefresh(system_info_win_);
}
void ScoutMonitor::ShowStatusItemName(int y, int x, std::string name)
{
const int state_value_col = x + 20;
const int state_div_col = state_value_col - 2;
mvwprintw(system_info_win_, y, x, name.c_str());
mvwprintw(system_info_win_, y, state_div_col, ":");
}
void ScoutMonitor::ShowFault(int y, int x, bool no_fault)
{
const int fault_col_1 = x;
const int fault_col_2 = x + 2;
const int fault_col_3 = fault_col_2 + 2;
if (no_fault)
{
NColors::WSetColor(system_info_win_, NColors::GREEN);
mvwprintw(system_info_win_, y, fault_col_1, "N");
NColors::WUnsetColor(system_info_win_, NColors::GREEN);
}
else
{
NColors::WSetColor(system_info_win_, NColors::RED);
mvwprintw(system_info_win_, y, fault_col_3, "F");
NColors::WUnsetColor(system_info_win_, NColors::RED);
}
}
// (y,x): position of the top left point of corresponding wheel
void ScoutMonitor::ShowMotorInfo(int y, int x, double cur, int rpm, int temp, bool is_right)
{
int col_title = x;
if (is_right)
col_title += 6;
else
col_title -= 9;
std::string cur_str = "CUR:" + ConvertFloatToString(cur, 1);
mvwprintw(body_info_win_, y + 1, col_title, cur_str.c_str());
std::string rpm_str = "RPM:" + ConvertFloatToString(rpm, 0);
mvwprintw(body_info_win_, y + 2, col_title, rpm_str.c_str());
std::string temp_str = "TMP:" + ConvertFloatToString(temp, 0);
mvwprintw(body_info_win_, y + 3, col_title, temp_str.c_str());
}
} // namespace westonrobot