task main()
{
initializeRobot();
//waitForStart();
moveDownWrist();
moveRobot();
liftArm();
wait1Msec(500);
rightTurnRobot();
wait1Msec(750);
shortMoveRobot();
wait1Msec(500);
moveUpWrist();
lowerArm();
}
void Oh_Oh_Robot::writingRobot_runMode(){
//readEPROM();
int n=0;
for(int i=0;i<512;i+=2){
if(EEPROM.read(i)==255){
direction[n]=255;
break;
}else{
direction[n]=EEPROM.read(i);
timeSpan[n]=EEPROM.read(i+1)*10;
n++;
}
}
//startRun();
for(int i=0;i<3;i++){
digitalWrite(ledPin,HIGH);
delay(1000);
digitalWrite(ledPin,LOW);
delay(1000);
}
runTimer=millis();
arrayPoint=0;
onOff(true);
liftArm();
runFlag=true;
changeAction();
while(runFlag){
run();
powerFactor;
sizeFactor;
}
}
void DoorDetectorActionsAlgNode::door_centroid_callback(const geometry_msgs::PoseStamped::ConstPtr& door_centroid)
{
ROS_INFO("DoorDetectorActionsAlgNode::door_centroid_callback: New Message Received");
//use appropiate mutex to shared variables if necessary
//this->alg_.lock();
this->door_centroid_mutex_.enter();
bool tf_exists = false;
bool tf_2_exists = false;
if((start==1 || no_simulator) && open_door<voting_rule_approval)
{
open_door++;
ROS_ERROR("open door counter: %d", open_door);
if(open_door==voting_rule_approval)
{
side_open=door_centroid->pose.orientation.w;
//Print result
if (side_open == 0)
ROS_ERROR("Result: the door is fully open!");
if (side_open < 0)
ROS_ERROR("Result: door ajar on the left side!");
if (side_open > 0)
ROS_ERROR("Result: door ajar on the right side!");
//Re-start counter, or send commands to robot
if(no_simulator)
open_door=0;
if(!no_simulator)
{
tf_exists = tf_listener.waitForTransform(tf_target_frame,
door_centroid->header.frame_id, door_centroid->header.stamp, ros::Duration(100));
tf_2_exists = tf_listener.waitForTransform(tf_target_frame_2,
door_centroid->header.frame_id, door_centroid->header.stamp, ros::Duration(100));
if(tf_exists)
{
base_door_centroid=transformGoalBase(*door_centroid, tf_target_frame);
if(tf_2_exists)
{
if (side_open > 0)
{
arm_door_centroid=liftArm(*door_centroid, tf_target_frame_2);
marker.header.frame_id=tf_target_frame_2;
}
if (side_open < 0)
{
arm_door_centroid=liftArm(*door_centroid, tf_target_frame);
marker.header.frame_id=tf_target_frame;
}
if (side_open != 0)
{
marker.pose.position = arm_door_centroid.goal_constraints.position_constraints[0].position;
marker.pose.orientation = arm_door_centroid.goal_constraints.orientation_constraints[0].orientation;
marker = ArrowMarker(door_centroid->header, marker.pose, 1, 1, "arm_poses_marker");
arm_poses_marker_publisher_.publish(marker);
}
}
}
}
}
}
//unlock previously blocked shared variables
//this->alg_.unlock();
this->door_centroid_mutex_.exit();
}
