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&)));
}
Exemplo n.º 2
0
/***************** 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;
}
Exemplo n.º 7
0
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;
}