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; }