void TutorialUnit::alternateGripper(bool open)
{
if(!open){
openGripper();
} else {
closeGripper();
}
}
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();
}
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;
}
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();
}
/**
* @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;
}
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);
}
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();
}
/**
*@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;
}