JointVariableCommandWidget::JointVariableCommandWidget(
RobotCommandModel* model,
QWidget* parent)
:
QWidget(parent),
m_model(model),
m_ignore_sync(false)
{
auto* glayout = new QGridLayout;
setLayout(glayout);
m_joint_groups_combo_box = new QComboBox;
m_joint_group_states_combo_box = new QComboBox;
updateJointGroupControls();
connect(m_model, SIGNAL(robotLoaded()), this, SLOT(updateRobotModel()));
connect(m_model, SIGNAL(robotStateChanged()), this, SLOT(updateRobotState()));
// TODO: lazily connect these signals when we're certain we have a robot
// model?
connect(m_joint_groups_combo_box,
SIGNAL(currentIndexChanged(const QString&)),
this,
SLOT(setJointGroup(const QString&)));
connect(m_joint_group_states_combo_box,
SIGNAL(currentIndexChanged(const QString&)),
this,
SLOT(setNamedState(const QString&)));
}
/***************** ENTRY-POINT FUNCTION CALL *****************************
* *
**************************************************************************/
void mb_estimator_update(void) {
clear_UI_LED(); // Clears all LEDs that had been active on the previous cycle
if (INITIALIZE_ESTIMATOR) {
// Initialize the filter coefficients:
setFilterCoeff(&FC_FAST, FILTER_CUTOFF_FAST);
setFilterCoeff(&FC_SLOW, FILTER_CUTOFF_SLOW);
setFilterCoeff(&FC_VERY_SLOW, FILTER_CUTOFF_VERY_SLOW);
// Reset the joint angle rate filters
setFilterData(&FD_OUTER_LEG_ANGLE, ID_UI_ROLL);
setFilterData(&FD_UI_ANG_RATE_X, ID_UI_ANG_RATE_X);
setFilterData(&FD_MCH_ANG_RATE, ID_MCH_ANG_RATE);
setFilterData(&FD_MCFO_RIGHT_ANKLE_RATE, ID_MCFO_RIGHT_ANKLE_RATE);
setFilterData(&FD_MCFI_ANKLE_RATE, ID_MCFI_ANKLE_RATE);
// Reset the contact sensor filters
setFilterData(&FD_MCFO_LEFT_HEEL_SENSE, ID_MCFO_LEFT_HEEL_SENSE);
setFilterData(&FD_MCFO_RIGHT_HEEL_SENSE, ID_MCFO_RIGHT_HEEL_SENSE);
setFilterData(&FD_MCFI_LEFT_HEEL_SENSE, ID_MCFI_LEFT_HEEL_SENSE);
setFilterData(&FD_MCFI_RIGHT_HEEL_SENSE, ID_MCFI_RIGHT_HEEL_SENSE);
// Steering motor stuff:
setFilterData(&FD_MCSI_STEER_ANGLE, ID_MCSI_STEER_ANGLE);
// Robot orientation estimation
resetRobotOrientation();
getIntegralRateGyro(); // Run integral to log the current state of the rate gyro
// Set "once per step" variables:
STATE_lastStepLength = 0.0; // Initialize to zero, for lack of a better plan
STATE_lastStepTimeSec = STATE_t; // cpu clock time at last heel strike.
STATE_lastStepDuration = 0.0; // Duration of the last step (seconds)
STATE_lastEstTime = 0.001 * mb_io_get_float(ID_TIMESTAMP);
// Remember that we've initialized everything properly
INITIALIZE_ESTIMATOR = false;
}
STATE_t = 0.001 * mb_io_get_float(ID_TIMESTAMP); // Robot Time (converted to seconds)
runAllFilters();// Run the butterworth filters:
updateRobotOrientation();
sendTotalPower();
updateEnergyUsage(); // Must come after sendTotalPower()
// Update the state variables: (absolute orientation and rate)
updateRobotState();
// Update controller parameters from LabVIEW
updateParameters();
// Check if the robot fell down
checkIfRobotFellDown();
STATE_lastEstTime = STATE_t; // Update previous estimation time.
return;
}
bool BaxterCommander::enableRobot()
{
// Check status
if(!updateRobotState())
{
return false;
}
if(robot_state.stopped == true)
{
ROS_ERROR("enableRobot: Robot is stopped");
return false;
}
if(robot_state.error == true)
{
ROS_ERROR("enableRobot: Robot has an error");
return false;
}
if(robot_state.enabled == true)
{
ROS_WARN("enableRobot: Robot is already enabled");
return true;
}
// Reset robot
if(!resetRobot())
return false;
// Enable robot
robot_enable_pub_.publish(true_msg_);
ros::Time start_time;
while(ros::ok() && ros::Time::now() > start_time + ros::Duration(1.0) && robot_state.enabled == false)
{
if(!updateRobotState())
{
return false;
}
ROS_INFO("enableRobot: Waiting for robot to be enabled ...");
ros::Duration(0.1).sleep();
}
return robot_state.enabled;
}
bool BaxterCommander::resetRobot()
{
if(!updateRobotState())
{
return false;
}
robot_reset_pub_.publish(empty_msg_);
ros::Duration(0.1).sleep();
return true;
}
bool BaxterCommander::stopRobot()
{
if(!updateRobotState())
{
return false;
}
robot_stop_pub_.publish(empty_msg_);
ros::Time start_time;
while(ros::ok() && ros::Time::now() > start_time + ros::Duration(1.0) && robot_state.stopped == false)
{
if(!updateRobotState())
{
return false;
}
ROS_INFO("stopRobot: Waiting for robot to be stopped ...");
ros::Duration(0.1).sleep();
}
return robot_state.stopped;
}
bool BaxterCommander::disableRobot()
{
if(!updateRobotState())
{
return false;
}
// Disable robot
robot_enable_pub_.publish(false_msg_);
ros::Time start_time;
while(ros::ok() && ros::Time::now() > start_time + ros::Duration(1.0) && robot_state.enabled == true)
{
if(!updateRobotState())
{
return false;
}
ROS_INFO("disableRobot: Waiting for robot to be disabled ...");
ros::Duration(0.1).sleep();
}
return !robot_state.enabled;
}
int main(void)
{
int pwm;
int adc_on;
int adc_off;
uint16_t i;
element elem;
robot.rotation_target = 0;
robot.right_motor_pos =0;
robot.left_motor_pos =0;
robot.right_motor_target =0;
robot.left_motor_target =0;
SysTick_Config(168000000/8/1000000); // interrupr 1000000 per secound
initTimer3(1);
initMotors();
initEncoders();
initUsart3();
// enableUSART3RXNEInterrupt();
initADC();
initIRSensors();
initLED();
initSPI2();
LED_OFF;
waitMs(1000);
LED_ON;
initPIDStructures();
stopMotors();
resetEncoders();
updateRobotState();
robot.left_ir_sensor_target = measures.left_ir_sensor;
robot.right_ir_sensor_target = measures.right_ir_sensor;
initQueue(&robot_queue);
//addMove(&robot_queue,FORWARD,2000);
addMove(&robot_queue,ROTATE,20000);
// addMove(&robot_queue,FORWARD,9000);
// addMove(&robot_queue,ROTATE,0);
// addMove(&robot_queue,ROTATE,6000);
// addMove(&robot_queue,FORWARD,9000);
// addMove(&robot_queue,ROTATE,6000);
// addMove(&robot_queue,ROTATE,12000);
// addMove(&robot_queue,FORWARD,9000);
// addMove(&robot_queue,ROTATE,12000);
// addMove(&robot_queue,ROTATE,3000);
// nextMove();
LED_ON;
while(1)
{
if ( isBatteryWeak() ){
stopMotors();
blinkLed();
}
if (flags.update_robot == 1){
moveRobot();
updateRobotState();
flags.update_robot =0;
}
if (flags.usart_custom == 1){
USART3_transmitString(itoa((int) robot.rotation, buf,10));
USART3_transmitString(" ");
USART3_transmitString(itoa((int) robot.rotation_target, buf,10));
USART3_transmitString("\n");
}
}
// flags.usart_custom == 0;
// USART3_transmitString("lewe_kolo_diff ");
// USART3_transmitString(itoa((int) robot.left_motor_target - robot.left_motor_pos , buf,10));
// USART3_transmitByte('\n');
// USART3_transmitString("lewe kolo pwm ");
// USART3_transmitString(itoa((int) getTranslation(LEFT_MOTOR) , buf,10));
// USART3_transmitByte('\n');
//
// USART3_transmitString("prawe kolo diff ");
// USART3_transmitString(itoa((int) robot.right_motor_target - robot.right_motor_pos , buf,10));
// USART3_transmitByte('\n');
// USART3_transmitString("prawe kolo pwm ");
// USART3_transmitString(itoa((int) getTranslation(RIGHT_MOTOR) , buf,10));
// USART3_transmitByte('\n');
// }
//
// if (flags.usart_graph == 1){
// static uint8_t usart_start = 1;
// if (usart_start ){
// usart_start=0;
// USART3_transmitString("\n");
// USART3_transmitString("\n");
// USART3_transmitString("\n");
// USART3_transmitString("__start__\n");
// USART3_transmitString("lewe_kolo_pozycja ");
// USART3_transmitString("lewe_kolo_cel ");
// USART3_transmitString("lewe_kolo_diff ");
// USART3_transmitString("lewe_kolo_pwm_T ");
// USART3_transmitString("prawe_kolo_pozycja ");
// USART3_transmitString("prawe_kolo_cel ");
// USART3_transmitString("prawe_kolo_diff ");
// USART3_transmitString("prawe_kolo_pwm_T \n");
// USART3_transmitString(" \n");
// }
//
// USART3_transmitString(itoa((int) robot.right_motor_pos , buf,10));
// USART3_transmitString(" ");
// USART3_transmitString(itoa((int) robot.right_motor_target , buf,10));
// USART3_transmitString(" ");
// USART3_transmitString(itoa((int) robot.right_motor_target - robot.right_motor_pos , buf,10));
// USART3_transmitString(" ");
// USART3_transmitString(itoa((int) getTranslation(RIGHT_MOTOR) , buf,10));
// USART3_transmitString(" ");
// USART3_transmitString(itoa((int) robot.left_motor_pos , buf,10));
// USART3_transmitString(" ");
// USART3_transmitString(itoa((int) robot.left_motor_target , buf,10));
// USART3_transmitString(" ");
// USART3_transmitString(itoa((int) robot.left_motor_target - robot.left_motor_pos , buf,10));
// USART3_transmitString(" ");
// USART3_transmitString(itoa((int) getTranslation(LEFT_MOTOR) , buf,10));
// USART3_transmitByte('\n');
// flags.usart_graph = 0;
// }
// }
//
return 0;
}