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