예제 #1
0
void TutorialUnit::alternateGripper(bool open)
{
    if(!open){
        openGripper();
    } else {
        closeGripper();
    }
}
예제 #2
0
 void moveToPin(Pin &pin) {
   // go to the top of the pin
   goTo(pin.x, pin.y, (3 + ALPHA) * DISC_HEIGHT + BASE_HEIGHT);
   // update pin state
   pin.add();
   // get disc on the middle point
   goTo(pin.x, pin.y,
        (pin.num_discs - 0.5) * DISC_HEIGHT + BASE_HEIGHT);
   // open Gripper!
   openGripper();
 }
예제 #3
0
static Action::Ptr createReleaseAction(GraspingActionContext &ctx, stringstream &ss) {
    // format: release
    FunctionAction::Ptr releaseAnchors(new FunctionAction(
                boost::bind(&GenPR2SoftGripper::releaseAllAnchors, ctx.sbgripper)));
    GripperOpenCloseAction::Ptr openGripper(new GripperOpenCloseAction(
                ctx.robot, ctx.gmanip->baseManip()->manip, true));
    openGripper->setExecTime(0.2);
    ActionChain::Ptr a(new ActionChain);
    *a << releaseAnchors << openGripper;
    return a;
}
예제 #4
0
파일: meArm.cpp 프로젝트: SkipXtian/meArm
void meArm::begin(int pinBase, int pinShoulder, int pinElbow, int pinGripper) {
  _pinBase = pinBase;
  _pinShoulder = pinShoulder;
  _pinElbow = pinElbow;
  _pinGripper = pinGripper;
  _base.attach(_pinBase);
  _shoulder.attach(_pinShoulder);
  _elbow.attach(_pinElbow);
  _gripper.attach(_pinGripper);

  //goDirectlyTo(0, 100, 50);
  goDirectlyToCylinder(0, 100, 50);
  openGripper();
}
예제 #5
0
/**
 * @brief main loop for AVR chip
 *
 */
int main(void) {
	// ==== do initializations!! ===
	initRBELib(); // allows printf + more to work
	debugUSARTInit(OUR_BAUD_RATE); // intialize USART communications
	printf("Starting...\n\r"); // so we know if we freeze in setup

	initSPI(); // initialize SPI communications
	initArm(); // initialize the arm'

	stopConveyor(); // initialize servo positions
	openGripper();

	// ==== end initializations ====

	printf("I am alive... Looking for blocks to pickup.\n\r");

	// ===== main loop ====
	while (1) {
		finiteStateMachine(); // run FSM to determine what arm needs to do
		serviceArm(); // allow arm to react to changes and service PID if needed
	}
	return 0;
}
예제 #6
0
bool doTask(int task) {
	switch (task) {
		case GO_FORWARD: 		
			return goForward(false);
		case GO_BACKWARD:	
			return goBackward(); 
		case CALC_NEXT_LINE:	
			return calcNextLine();
		case GO_FORWARD_AND_COUNT:
			return goForwardAndCount();
		case TURN_LEFT:		
			return turn90(LEFT);	
		case TURN_RIGHT:		
			return turn90(RIGHT);
		case TURN_90:		
			// turn this based on the side
			return true;
		case TURN_180:
			return turn180();
		case TO_VERTICAL:
			return craneToVer();
		case TO_HORIZONTAL:
			return craneToHor();
		case SLIDE_OUT:
			return slideOut();
		case SLIDE_IN:
			return slideIn();
		case OPEN_GRIPPER:
			return openGripper();
		case CLOSE_GRIPPER:
			return closeGripper();
		case SWITCH_SIDE:
			return switchSide();
		default:		
			return true;
	}
}
// Actual routine code
void autonomousB() {
	// Drive until parallel with IR beacon
	driveWithSensorGoal(100, 100, sensorIR, 7, 0);

	// Record distance travelled
	int distLeft  = nMotorEncoder[motorDriveLeft];
	int distRight = nMotorEncoder[motorDriveRight];

	// Move forward a bit more
	driveWithEncoderGoal(50, 50, ENCODER_RATIO_DRIVE_LEFT*0.2, ENCODER_RATIO_DRIVE_RIGHT*0.2, ENCODER_RATIO_DRIVE_LEFT/100, ENCODER_RATIO_DRIVE_RIGHT/100);
	wait1Msec(200);

	wait1Msec(5000);

	// Add to the distance
	//distLeft  += nMotorEncoder[motorDriveLeft];
	//distRight += nMotorEncoder[motorDriveRight];

	// Rotate CW to face basket
	driveWithSensorGoal(-50, 50, sensorIR, 4, 0);
	wait1Msec(200);

	// Record distance from basket
	int distance = SensorValue[sensorSonar];

	// Drive up to basket
	driveWithSensorGoal(50, 50, sensorSonar, 25, 3); // 22 for 360
	wait1Msec(200);

	// Open gripper
	openGripper();
	wait1Msec(500);

	// Drive away from basket
	driveWithSensorGoal(-50, -50, sensorSonar, distance, 5);
	wait1Msec(200);

	// Rotate CW to face start position
	driveWithEncoderGoal(-50, 50, -ENCODER_RATIO_DRIVE_LEFT*0.13340*2.5, ENCODER_RATIO_DRIVE_RIGHT*0.13340*2.5, ENCODER_RATIO_DRIVE_LEFT/100, ENCODER_RATIO_DRIVE_RIGHT/100);
	//driveWithSensorGoal(-50, 50, sensorIR, 1, 0);
	wait1Msec(200);

	wait1Msec(5000);

	// Drive to the approximate start position
	driveWithEncoderGoal(100, 100, distRight, distLeft, ENCODER_RATIO_DRIVE_LEFT/100, ENCODER_RATIO_DRIVE_RIGHT/100);
	wait1Msec(200);

	while (true) {}

	// Rotate CCW
	driveWithEncoderGoal(50, -50, ENCODER_RATIO_DRIVE_LEFT*0.13340*2, -ENCODER_RATIO_DRIVE_RIGHT*0.13340*2, ENCODER_RATIO_DRIVE_LEFT/100, ENCODER_RATIO_DRIVE_RIGHT/100);
	wait1Msec(200);

	// Drive forward
	driveWithEncoderGoal(100, 100, ENCODER_RATIO_DRIVE_LEFT, ENCODER_RATIO_DRIVE_RIGHT, ENCODER_RATIO_DRIVE_LEFT/100, ENCODER_RATIO_DRIVE_RIGHT/100);
	wait1Msec(200);

	// Rotate CCW
	driveWithEncoderGoal(50, -50, ENCODER_RATIO_DRIVE_LEFT*0.13340*2, -ENCODER_RATIO_DRIVE_RIGHT*0.13340*2, ENCODER_RATIO_DRIVE_LEFT/100, ENCODER_RATIO_DRIVE_RIGHT/100);
	wait1Msec(200);

	// Drive onto ramp
	driveWithEncoderGoal(100, 100, ENCODER_RATIO_DRIVE_LEFT, ENCODER_RATIO_DRIVE_RIGHT, ENCODER_RATIO_DRIVE_LEFT/100, ENCODER_RATIO_DRIVE_RIGHT/100);
}
예제 #8
0
 Robot(Pin p_a, Pin p_b, Pin p_c) : p_a(p_a), p_b(p_b), p_c(p_c) {
   // get pins positions
   openGripper();
 }
예제 #9
0
/**
*@brief サインスマート製4自由度ロボットアーム制御クラスのコンストラクタ
*/
RobotArm::RobotArm()
{
	jl = new Vector3d[4];
	pl = new Vector3d[4];
	l[0] = ArmLength0;
	l[1] = ArmLength1;
	l[2] = ArmLength2;
	l[3] = ArmLength3;
	lh = HandLength;
	lf = FingerLength;
	m[0] = ArmMath0;
	m[1] = ArmMath1;
	m[2] = ArmMath2;
	m[3] = ArmMath3;
	mh = HandMath;
	mf = FingerMath;
	wi = ArmWidth;
	wf = FingerWidth;
	hi = ArmHeight;
	hf = FingerHeight;
	rh = HandRadius;
	jl[0](0) = 0;
	jl[0](1) = 0;
	jl[0](2) = 0;
	pl[0](0) = jl[0](0);
	pl[0](1) = jl[0](1);
	pl[0](2) = jl[0](2)+l[0]/2;
	jl[1](0) = pl[0](0);
	jl[1](1) = pl[0](1);
	jl[1](2) = pl[0](2)+l[0]/2;
	pl[1](0) = jl[1](0);
	pl[1](1) = jl[1](1);
	pl[1](2) = jl[1](2)+l[1]/2;
	jl[2](0) = pl[1](0);
	jl[2](1) = pl[1](1);
	jl[2](2) = pl[1](2)+l[1]/2;
	pl[2](0) = jl[2](0);
	pl[2](1) = jl[2](1);
	pl[2](2) = jl[2](2)+l[2]/2;
	jl[3](0) = pl[2](0);
	jl[3](1) = pl[2](1);
	jl[3](2) = pl[2](2)+l[2]/2;
	pl[3](0) = jl[3](0);
	pl[3](1) = jl[3](1)+l[3]/2;
	pl[3](2) = jl[3](2);
	jh(0) = pl[3](0);
	jh(1) = pl[3](1)+l[3]/2;
	jh(2) = pl[3](2);
	ph(0) = jh(0);
	//pyh = jyh+lh/2;
	ph(1) = jh(1);
	ph(2) = jh(2);
	jf(0) = ph(0);
	//jyf = pyh+lh/2;
	jf(1) = ph(1);
	jf(2) = ph(2);
	pf(0) = jf(0);
	pf(1) = jf(1);
	pf(2) = jf(2)-lf/2;
	hw = 0.02;

	setAngle(0, 0, 0, 0);


	dt = 0.01;
	//endTime = -1;
	//time = 0;
	/*targetPoint(0) = 0;
	targetPoint(1) = 0;
	targetPoint(2) = 0;

	startPoint(0) = 0;
	startPoint(1) = 0;
	startPoint(2) = 0;*/

	setOffset(0,0,0,0);

	Kp = 10;
	Kjp = 10;
	
	openGripper();

	maxSpeedCartesian = Vector3d(1000, 1000, 1000);
	maxSpeedJoint[0] = 1000;
	maxSpeedJoint[1] = 1000;
	maxSpeedJoint[2] = 1000;
	maxSpeedJoint[3] = 1000;

	softUpperLimitCartesian = Vector3d(1000, 1000, 1000);
	softLowerLimitCartesian = Vector3d(-1000, -1000, -1000);

	pauseFalg = false;
	stopFalg = false;

	//homePosition = Vector3d(0, 0, 0);
	


	softUpperLimitJoint[0] = MOTOR_UPPER__LIMIT_0;
	softUpperLimitJoint[1] = MOTOR_UPPER__LIMIT_1;
	softUpperLimitJoint[2] = MOTOR_UPPER__LIMIT_2;
	softUpperLimitJoint[3] = MOTOR_UPPER__LIMIT_3;

	softLowerLimitJoint[0] = MOTOR_LOWER__LIMIT_0;
	softLowerLimitJoint[1] = MOTOR_LOWER__LIMIT_1;
	softLowerLimitJoint[2] = MOTOR_LOWER__LIMIT_2;
	softLowerLimitJoint[3] = MOTOR_LOWER__LIMIT_3;

	serbo = true;

	manifactur = "SainSmart";
	type = "DIY 4-Axis Servos Control Palletizing Robot Arm";
	axisNum = 4;
	cmdCycle = 50;
	isGripper = false;

	//speedPoint = 10;
	//speedJointPos = 1;

	MaxSpeedJoint[0] = 2;
	MaxSpeedJoint[1] = 2;
	MaxSpeedJoint[2] = 2;
	MaxSpeedJoint[3] = 2;
	MaxSpeedCartesianTrans = 0.5;
	MaxSpeedCartesianRot = 2;

	MinTime = dt;

	jointOffset[0] = MOTOR_OFFSET_0;
	jointOffset[1] = MOTOR_OFFSET_1;
	jointOffset[2] = MOTOR_OFFSET_2;
	jointOffset[3] = MOTOR_OFFSET_3;
	

}