SyncErrorsTableModel::SyncErrorsTableModel(QObject *parent)
: QAbstractTableModel(parent),
repo_name_column_width_(kRepoNameColumnWidth),
path_column_width_(kPathColumnWidth),
error_column_width_(kErrorColumnWidth)
{
update_timer_ = new QTimer(this);
connect(update_timer_, SIGNAL(timeout()), this, SLOT(updateErrors()));
update_timer_->start(kUpdateErrorsIntervalMSecs);
updateErrors();
}
static void poseCommanderTask(void* param) {
uint32_t lastWakeTime;
//Wait for the system to be fully started to start pose controller loop
systemWaitStart();
lastWakeTime = xTaskGetTickCount ();
while(1) {
vTaskDelayUntil(&lastWakeTime, F2T(POSECOMMANDERFREQUENCY)); // 100Hz
// read actual pose value
actualPoseGetPose(&actualPose);
// get desired pose value
if(desiredPoses.front != 0) {
desiredPose = *(Pose*)(desiredPoses.front->item);
}
// update data for logging
updateErrors(&desiredPose, &actualPose);
// test for goal reached and shift
if(reached(&desiredPose, &actualPose)) {
if(desiredPoses.front != 0) {
if(desiredPoses.front->next != 0) {
linkedlistItem front = *linkedListPopFront(&desiredPoses);
deleteLinkedlistItem(&front);
}
}
}
}
}
void LoginPage::loginResultAttachPhone(int64_t _seq, int result)
{
if (_seq != send_seq_)
return;
updateErrors(result);
if (result == 0)
{
emit attached();
}
}
void LoginPage::loginResult(int64_t _seq, int result)
{
if (_seq != send_seq_)
return;
updateErrors(result);
if (result == 0)
{
if (login_staked_widget_->currentIndex() == SUBPAGE_PHONE_CONF_INDEX)
{
code_edit_->setText("");
initLoginSubPage(SUBPAGE_PHONE_LOGIN_INDEX);
}
GetDispatcher()->post_stats_to_core(login_staked_widget_->currentIndex() == SUBPAGE_UIN_LOGIN_INDEX
? core::stats::stats_event_names::reg_login_uin
: core::stats::stats_event_names::reg_login_phone);
clearErrors();
emit loggedIn();
}
}
void FourWheelOmniPilot::defaultDrive (Robot* rob, float x_goal, float y_goal, float theta_goal, float x_goal2, float y_goal2)
{
// Used to clear accumulated errors if goal changes significantly
prev_goal_target = Point(x_goal, y_goal);
Point rp = rob->getPosition();
Point gp = Point(x_goal,y_goal);
distance_error = Measurements::distance(rp,gp);
float angle_to_goal = Measurements::angleBetween(rp, gp);
angle_error = Measurements::angleDiff(rob->getOrientation(), theta_goal);
// Calulate error integral component
updateErrors(x_goal, y_goal);
// Inertial Frame Velocities
double x_vel = (TRANS_P_K*distance_error + TRANS_I_K*dist_error_integral)*cos(angle_to_goal);
double y_vel = (TRANS_P_K*distance_error + TRANS_I_K*dist_error_integral)*sin(angle_to_goal);
double theta_vel = ANGULAR_P_K*angle_error + ANGULAR_I_K*angle_error_integral;
// Adjust angular velocity to traverse the minor turn angle
double theta_current = rob->getOrientation();
if (abs(Measurements::angleDiff(theta_goal,theta_current))<
abs(Measurements::angleDiff(theta_goal,theta_current+theta_vel)))
theta_vel=-theta_vel;
// Reduce speed near target
bool is_final_target = Measurements::distance(gp, Point(x_goal2, y_goal2)) < 0.01;
if (is_final_target && distance_error < 1000)
{
x_vel *= 0.4;
y_vel *= 0.4;
}
// Robot Frame Velocities
double y_vel_robot = cos(theta_current)*x_vel+sin(theta_current)*y_vel;
double x_vel_robot = sin(theta_current)*x_vel-cos(theta_current)*y_vel;
// Apply acceleration ramp
const double ACC_PER_FRAME = 1;
double speed = rob->getSpeedMillimetersPerFrame();
double requested_speed = sqrt(x_vel_robot*x_vel_robot + y_vel_robot*y_vel_robot);
if(requested_speed > speed + ACC_PER_FRAME) {
x_vel_robot = (speed + ACC_PER_FRAME) * 100 * x_vel_robot/requested_speed;
y_vel_robot = (speed + ACC_PER_FRAME) * 100 * y_vel_robot/requested_speed;
}
// Wheel Velocity Calculations
double RF = (-sin(RF_OFFSET) * x_vel_robot + cos(RF_OFFSET)*y_vel_robot - TRANS_OFFSET*speed*cos(RF_OFFSET) + WHEEL_RADIUS*theta_vel);
double LF = -(-sin(LF_OFFSET) * x_vel_robot + cos(LF_OFFSET)*y_vel_robot - TRANS_OFFSET*speed*cos(LF_OFFSET) + WHEEL_RADIUS*theta_vel);
double LB = -(-sin(LB_OFFSET) * x_vel_robot + cos(LB_OFFSET)*y_vel_robot - TRANS_OFFSET*speed*cos(LB_OFFSET) + WHEEL_RADIUS*theta_vel);
double RB = (-sin(RB_OFFSET) * x_vel_robot + cos(RB_OFFSET)*y_vel_robot - TRANS_OFFSET*speed*cos(RB_OFFSET) + WHEEL_RADIUS*theta_vel);
// Normalize wheel velocities
normalizeSpeeds(LF, LB, RF, RB, 100);
// Set velocities on robot object
rob->setLF(LF);
rob->setLB(LB);
rob->setRF(RF);
rob->setRB(RB);
}
void FourWheelOmniPilot::facePointDrive(Robot* rob, float x_goal, float y_goal, float theta_goal)
{
//Current Position
double x_current = rob->getPosition().x;
double y_current = rob->getPosition().y;
double theta_current = rob->getOrientation();
prev_goal_target = Point(x_goal, y_goal);
Point rp = Point(x_current,y_current);
Point gp = Point(x_goal,y_goal);
distance_error = Measurements::distance(rp,gp);
float angle_to_goal = Measurements::angleBetween(rp, gp);
angle_error = Measurements::angleDiff(rob->getOrientation(), theta_goal);
// PID
updateErrors(x_goal, y_goal);
// Unlike defaultCalc, we always use the same XY prop/int multipliers
float xy_prop_used = TRANS_P_K;
float xy_int_used = TRANS_I_K;
//Interial Frame Velocities
double x_vel =
(xy_prop_used * distance_error +
xy_int_used * dist_error_integral)*cos(angle_to_goal);
double y_vel =
(xy_prop_used * distance_error +
xy_int_used * dist_error_integral)*sin(angle_to_goal);
double theta_vel =
ANGULAR_P_K * angle_error
+ ANGULAR_I_K * angle_error_integral;
if (abs(Measurements::angleDiff(theta_goal,theta_current))<
abs(Measurements::angleDiff(theta_goal,theta_current+theta_vel)))
theta_vel=-theta_vel;
// Reduce speed near target
if (distance_error < 700)
{
x_vel *= 0.1;
y_vel *= 0.1;
}
// Focus on rotation
double vel = sqrt(x_vel*x_vel+y_vel*y_vel);
if (abs(Measurements::angleDiff(theta_goal,theta_current)) > ROT_TOLERANCE*3 && vel > 50)
{
x_vel = (abs(x_vel)>=abs(y_vel))? 50 * x_vel/abs(x_vel) : 50*x_vel/abs(y_vel);
y_vel = (abs(x_vel)<=abs(y_vel))? 50 * y_vel/abs(y_vel) : 50*y_vel/abs(x_vel);
theta_vel *= 2;
}
// Robot Frame Velocities
double y_vel_robot = cos(theta_current)*x_vel+sin(theta_current)*y_vel;
double x_vel_robot = sin(theta_current)*x_vel-cos(theta_current)*y_vel;
// Wheel Velocity Calculations
double RF = (-sin(RF_OFFSET) * x_vel_robot + cos(RF_OFFSET)*y_vel_robot + WHEEL_RADIUS*theta_vel);
double LF = -(-sin(LF_OFFSET) * x_vel_robot + cos(LF_OFFSET)*y_vel_robot + WHEEL_RADIUS*theta_vel);
double LB = -(-sin(LB_OFFSET) * x_vel_robot + cos(LB_OFFSET)*y_vel_robot + WHEEL_RADIUS*theta_vel);
double RB = (-sin(RB_OFFSET) * x_vel_robot + cos(RB_OFFSET)*y_vel_robot + WHEEL_RADIUS*theta_vel);
// Normalize wheel velocities
normalizeSpeeds(LF, LB, RF, RB, 100);
// Set velocities on robot object
rob->setLF(LF);
rob->setLB(LB);
rob->setRF(RF);
rob->setRB(RB);
}
void FourWheelOmniPilot::dribbleDrive
(Robot* rob, float x_goal, float y_goal, float theta_goal)
{
//Current Position
double x_current = rob->getPosition().x;
double y_current = rob->getPosition().y;
double theta_current = rob->getOrientation();
prev_goal_target = Point(x_goal, y_goal);
Point rp = Point(x_current,y_current);
Point gp = Point(x_goal,y_goal);
distance_error = Measurements::distance(rp,gp);
float angle_to_goal = Measurements::angleBetween(rp, gp);
angle_error = Measurements::angleDiff(rob->getOrientation(), theta_goal);
//Calulate error integral component
updateErrors(x_goal, y_goal);
//Inertial Frame Velocities
double x_vel =
(TRANS_P_K * distance_error +
TRANS_I_K * dist_error_integral)*cos(angle_to_goal);
double y_vel =
(TRANS_P_K * distance_error +
TRANS_I_K * dist_error_integral)*sin(angle_to_goal);
double theta_vel =
ANGULAR_P_K * angle_error +
ANGULAR_I_K * angle_error_integral;
if (abs(Measurements::angleDiff(theta_goal,theta_current))<
abs(Measurements::angleDiff(theta_goal,theta_current+theta_vel)))
theta_vel=-theta_vel;
// Robot Frame Velocities
double y_vel_robot = cos(theta_current)*x_vel+sin(theta_current)*y_vel;
double x_vel_robot = sin(theta_current)*x_vel-cos(theta_current)*y_vel;
double vel_robot = sqrt(x_vel_robot*x_vel_robot + y_vel_robot * y_vel_robot);
// Apply acceleration ramp
if(vel_robot > prev_speed)
{
x_vel_robot = x_vel_robot * (prev_speed + 2) / vel_robot;
y_vel_robot = y_vel_robot * (prev_speed + 2) / vel_robot;
vel_robot = prev_speed + 2;
}
prev_speed = vel_robot;
// Cap velocities for dribbling
y_vel_robot = fmin(y_vel_robot, DRIBBLE_FRWD_SPD);
y_vel_robot = fmax(y_vel_robot, -DRIBBLE_BACK_SPD);
theta_vel -= x_vel_robot;
theta_vel = fmin(theta_vel, DRIBBLE_TURN_RATE);
theta_vel = fmax(theta_vel, -DRIBBLE_TURN_RATE);
x_vel_robot = 0;
// Wheel Velocity Calculations
double RF = (-sin(RF_OFFSET) * x_vel_robot + cos(RF_OFFSET)*y_vel_robot - TRANS_OFFSET*vel_robot*cos(RF_OFFSET) + WHEEL_RADIUS*theta_vel);
double LF = -(-sin(LF_OFFSET) * x_vel_robot + cos(LF_OFFSET)*y_vel_robot - TRANS_OFFSET*vel_robot*cos(LF_OFFSET) + WHEEL_RADIUS*theta_vel);
double LB = -(-sin(LB_OFFSET) * x_vel_robot + cos(LB_OFFSET)*y_vel_robot - TRANS_OFFSET*vel_robot*cos(LB_OFFSET) + WHEEL_RADIUS*theta_vel);
double RB = (-sin(RB_OFFSET) * x_vel_robot + cos(RB_OFFSET)*y_vel_robot - TRANS_OFFSET*vel_robot*cos(RB_OFFSET) + WHEEL_RADIUS*theta_vel);
// Normalize wheel velocities
normalizeSpeeds(LF,LB,RF,RB, 100);
// Set velocities on robot object
rob->setLF(LF);
rob->setLB(LB);
rob->setRF(RF);
rob->setRB(RB);
}
MainWindow::MainWindow(QWidget *parent) :
QMainWindow(parent),
ui(new Ui::MainWindow)
{
m_commandLine = new CommandLine(qApp->arguments());
if (m_commandLine->processed())
{
// don't start GUI and retrieve the return code
m_guiActive = false;
m_returnCode = m_commandLine->returnCode();
return;
}
// start GUI here
ui->setupUi(this);
m_recentPrograms = new RecentFiles(ui->menuOpenRecent, this);
connect(m_recentPrograms, SIGNAL(openFile(QString)),
this, SLOT(programOpen(const QString)));
settingsRestore();
// TODO settings will eventually have info about edit boxes that were open
// TODO that will need to be restored, for now there is a single edit box
//==================
// SETUP EDIT BOX
//==================
// create the starting program edit box
m_editBox = new EditBox(this);
setCentralWidget(m_editBox);
m_statusReady = false;
connect(m_editBox->document(), SIGNAL(modificationChanged(bool)),
this, SLOT(setWindowModified(bool)));
connect(m_editBox->document(), SIGNAL(modificationChanged(bool)),
ui->actionSave, SLOT(setEnabled(bool)));
// connect available signals to the appropriate edit actions
connect(m_editBox, SIGNAL(undoAvailable(bool)),
ui->actionUndo, SLOT(setEnabled(bool)));
connect(m_editBox, SIGNAL(redoAvailable(bool)),
ui->actionRedo, SLOT(setEnabled(bool)));
connect(m_editBox, SIGNAL(copyAvailable(bool)),
ui->actionCut, SLOT(setEnabled(bool)));
connect(m_editBox, SIGNAL(copyAvailable(bool)),
ui->actionCopy, SLOT(setEnabled(bool)));
connect(m_editBox, SIGNAL(copyAvailable(bool)),
ui->actionDelete, SLOT(setEnabled(bool)));
connect(m_editBox, SIGNAL(errorsAvailable(bool)),
ui->actionGoNextError, SLOT(setEnabled(bool)));
connect(m_editBox, SIGNAL(errorsAvailable(bool)),
ui->actionGoPrevError, SLOT(setEnabled(bool)));
// create actions for edit box context menu
QList<QAction *> actions;
actions.append(ui->actionUndo);
actions.append(ui->actionRedo);
actions.append(new QAction(m_editBox));
actions.last()->setSeparator(true);
actions.append(ui->actionCut);
actions.append(ui->actionCopy);
actions.append(ui->actionPaste);
actions.append(ui->actionDelete);
actions.append(new QAction(m_editBox));
actions.last()->setSeparator(true);
actions.append(ui->actionSelectAll);
m_editBox->addActions(actions);
m_editBox->setContextMenuPolicy(Qt::ActionsContextMenu);
//==============================================
// SETUP PROGRAM MODEL, LINE DELEGATE AND VIEW
//==============================================
// setup program model (connect to edit box changes)
m_programModel = new ProgramModel(this);
connect(m_editBox, SIGNAL(linesChanged(int, int, int, QStringList)),
m_programModel, SLOT(update(int, int, int, QStringList)));
connect(m_programModel, SIGNAL(errorListChanged(ErrorList)),
m_editBox, SLOT(updateErrors(ErrorList)));
// setup program line delegate (connect to model line count changes)
m_programLineDelegate = new ProgramLineDelegate(EditBox::BaseLineNumber,
ui->programView, this);
connect(m_programModel, SIGNAL(lineCountChanged(int)),
m_programLineDelegate, SLOT(lineNumberWidthUpdate(int)));
// setup program view
ui->programView->setItemDelegate(m_programLineDelegate);
ui->programView->setModel(m_programModel);
ui->programView->setFont(m_editBox->font());
//=================
// SETUP PROGRAM
//=================
// if a file name was specified on the command line
// then it overrides the restored program
if (!m_commandLine->fileName().isEmpty())
{
setCurProgram(m_commandLine->fileName());
}
else
{
// make sure window title is set before continuing
setCurProgram(m_curProgram);
}
// load program if one was saved or specified on command line
if (!m_curProgram.isEmpty() // load a program?
&& (!QFile::exists(m_curProgram) // program not found?
|| !programLoad(m_curProgram)) // program not loaded?
&& m_commandLine->fileName().isEmpty()) // no program argument
{
setCurProgram(""); // clear program path that was restored/set
// TODO should an warning message be issued here?
}
statusBarCreate();
m_guiActive = true;
statusBarUpdate();
}
