Line Robot - Gripper

Concept

Purpose of this gripper is to catch balls in Robocup Rescue Line competition, put them in the container, and carry them to the discharge area. It uses 4 robotic servo motors to perform the actions.

Code

Servo motors have to move the gripper's jaws in a couple positions. We will have to enter some code, if not already present in Your mrm-robot-line.cpp and mrm-robot-line-h files.

First, change mrm-robot-line.h by adding some constants in "#define" area, in the beginning of the file, after "#include" section:

// Change these values to get optimal robot's behaviour.

// CATCH_SERVO drives jaws that catch ball.
#define CATCH_SERVO_CLOSE 260 // Closed position, ball caught.
#define CATCH_SERVO_OPEN_FULL 50 // Open position, ready to catch a ball.
#define CATCH_SERVO_OPEN_MIN 150 // Open just to drop the ball

// LIFT_SERVO lifts catch servo and the rest of mechanism.
#define LIFT_SERVO_BACK 70 // Top (idle) position.
#define LIFT_SERVO_DOWN 150 // Lowest position, catching a ball.
#define LIFT_SERVO_PUT_BACK 50
#define LIFT_SERVO_PUT_FRONT 80 // Middle position, for caught ball dropping.
#define LIFT_SERVO_UP 10 // Top (idle) position.

// ROTATE_SERVO rotates gripper
#define ROTATE_SERVO_DOWN 300
#define ROTATE_SERVO_LEFT 90
#define ROTATE_SERVO_RIGHT 125

// BLOCK_SERVO blocks balls
#define BLOCK_SERVO_BOTH 150

If not already present, add some function headers, again in mrm-robot-line-h, this time after "public:" designation:

	/** Arm will go to ball-catch position.
	*/
	void armCatch();

	/** Arm will go to ball-catch ready position.
	*/
	void armCatchReady();

	/** Arm will go to idle (top) position.
	*/
	void armIdle();

	/** Arm will put the ball left
	*/
	void armLeftPut();

	/** Arm will go to top left position
	*/
	void armLeftReady();

	/** Arm will drop the ball.
	*/
	void armPut();

	/** Arm will lift the caught ball in the position where will be ready to drop it.
	*/
	void armPutReady();

	/** Arm will put the ball right
	*/
	void armRightPut();

	/** Arm will go to top right position
	*/
	void armRightReady();

Now, mrm-robot-line.cpp. Add some functions:

/** Arm will go to ball-catch position.
*/
void RobotLine::armCatch() {
	mrm_servo->write(LIFT_SERVO_DOWN, 0); // Lower the arm. Parameter 0 defines servo, in this case lift-servo. LIFT_SERVO_DOWN is angle.
	mrm_servo->write(CATCH_SERVO_CLOSE, 1); // Catch the ball. Parameter 1 - catch-servo. CATCH_SERVO_CLOSE is angle.
	mrm_servo->write(BLOCK_SERVO_BOTH, 2); // Block both.
	mrm_servo->write(ROTATE_SERVO_DOWN, 3); // Rotate down.
}

/** Arm will go to ball-catch ready position.
*/
void RobotLine::armCatchReady() {
	mrm_servo->write(LIFT_SERVO_DOWN, 0); // Lower the arm.
	mrm_servo->write(CATCH_SERVO_OPEN_FULL, 1); // Open jaws.
	mrm_servo->write(BLOCK_SERVO_BOTH, 2); // Block both.
	mrm_servo->write(ROTATE_SERVO_DOWN, 3); // Rotate down.
}

/** Arm will go to idle (top) position.
*/
void RobotLine::armIdle() {
	mrm_servo->write(LIFT_SERVO_UP, 0); // Lift the arm.
	mrm_servo->write(CATCH_SERVO_OPEN_FULL, 1); // Open jaws.
	mrm_servo->write(BLOCK_SERVO_BOTH, 2); // Block both.
	mrm_servo->write(ROTATE_SERVO_DOWN, 3); // Rotate down.
} 

/** Arm will put the ball left
*/
void RobotLine::armLeftPut() {
	mrm_servo->write(LIFT_SERVO_PUT_BACK, 0); // Lift the arm.
	mrm_servo->write(CATCH_SERVO_OPEN_MIN, 1); // Open jaws.
	mrm_servo->write(BLOCK_SERVO_BOTH, 2); // Block both.
	mrm_servo->write(ROTATE_SERVO_LEFT, 3); // Rotate left.
}

/** Arm will go to top left position
*/
void RobotLine::armLeftReady() {
	mrm_servo->write(LIFT_SERVO_BACK, 0); // Lift the arm.
	mrm_servo->write(CATCH_SERVO_CLOSE, 1); // Close jaws.
	mrm_servo->write(BLOCK_SERVO_BOTH, 2); // Block both.
	mrm_servo->write(ROTATE_SERVO_LEFT, 3); // Rotate left.
}

/** Arm will drop the ball.
*/
void RobotLine::armPut() {
	mrm_servo->write(LIFT_SERVO_PUT_FRONT, 0); // Lift the arm halfways.
	mrm_servo->write(CATCH_SERVO_OPEN_FULL, 1); // Open jaws.
	mrm_servo->write(BLOCK_SERVO_BOTH, 2); // Block both.
}

/** Arm will lift the caught ball in the position where will be ready to drop it.
*/
void RobotLine::armPutReady() {
	mrm_servo->write(LIFT_SERVO_PUT_FRONT, 0); // Lift the arm halfways.
	mrm_servo->write(CATCH_SERVO_CLOSE, 1); // Keep the ball in jaws.
	mrm_servo->write(BLOCK_SERVO_BOTH, 2); // Block both.
}

/** Arm will put the ball right
*/
void RobotLine::armRightPut() {
	mrm_servo->write(LIFT_SERVO_PUT_BACK, 0); // Lift the arm.
	mrm_servo->write(CATCH_SERVO_OPEN_MIN, 1); // Open jaws.
	mrm_servo->write(BLOCK_SERVO_BOTH, 2); // Block both.
	mrm_servo->write(ROTATE_SERVO_RIGHT, 3); // Rotate right.
}

/** Arm will go to top right position
*/
void RobotLine::armRightReady() {
	mrm_servo->write(LIFT_SERVO_BACK, 0); // Lift the arm.
	mrm_servo->write(CATCH_SERVO_CLOSE, 1); // Close jaws.
	mrm_servo->write(BLOCK_SERVO_BOTH, 2); // Block both.
	mrm_servo->write(ROTATE_SERVO_RIGHT, 3); // Rotate right.
}

Explanation

Let's examine one example:

void RobotLine::armCatch() {
	servo(LIFT_SERVO_DOWN, 0); // Lower the arm. Parameter 0 defines servo, in this case lift-servo. LIFT_SERVO_DOWN is angle.
	servo(CATCH_SERVO_CLOSE, 1); // Catch the ball. Parameter 1 - catch-servo. CATCH_SERVO_CLOSE is angle.
	servo(BLOCK_SERVO_BOTH, 2); // Block both.
	servo(ROTATE_SERVO_DOWN, 3); // Rotate down.
}

#define LIFT_SERVO_DOWN 130 // Lowest position, catching a ball.

	servo(130, 0); 

servo(degrees, servoOrdinalNumber);

Examine the other functions that move servo motors:

• armCatchReady
• armIdle
• armLeftPut
• Etc.

You can test the functions in loop():

void RobotLine::loop() {
	if (setup())
		armCatchReady();
}

To test a more complex action, try this code:

void RobotLine::loop() {
	if (setup()) {
		armCatchReady();
		delayMs(500);
		armCatch();
		delayMs(500);
		armPutReady();
		delayMs(500);
		armPutLeft();
		delayMs(500);
		armIdle();
	}
}

Barrier

As the gripper opens its jaws rather wide, the most appropriate method to check if a ball is in a catch position is a barrier that we connected to pin 35. Here is the code to check for a ball:

void RobotLine::loop() {
	print("%i - %i\n\r", analogRead(35), barrier());
}
...
bool RobotLine::barrier() {
	return analogRead(35) < 2000; // 2000 is an example. Test to find Your best value
}

barrier() is an already existing function. You will have to enter loop().

Explanation:

1. print() in loop() again uses C-style format to display 2 integers. The second one is returned result from barrier() function.
2. barrier() reads pin 35 as analog value, which values can be between 0 and 4095 and returns boolean value (true/false or 0/1).
3. 2000 is a value that You will have to adjust to Your setup. It must be halfways between interrupted and uninterrupted light.

Dark balls

void RobotLine::loop() {
	print("%i - %i\n\r", analogRead(36), dark());
}
...
bool RobotLine::dark() {
	return analogRead(36) < 2000; // 2000 is an example. Test to find Your best value
}

Evacuation area

In the rest of this lesson we will not show additional code, but just discuss strategy in evacuation area.

There are 2 ways to detect entrance in the evacuation area.

• The robot can recognize a pattern: no line for a certain time, a wall, and the rest a big space.
• The robot can read reflective stripe at the entrance. This is a better solution. Fortunately, MRMS reflectance sensors 9x, CAN, analog, I2C (mrm-ref-can) is based on a sensor developed specifically to recognize reflective surface in RoboCup Rescue Maze, where it is much more important to do so. Therefore, if positioned at a right distance from the surface, it will be able to read its value as a distinct one from black and white. You should use a distance that gives You roughly equal difference in readings between reflective surface and white as between white and black.

Finding balls

This time 3 possibilities.

• A random walk. Just drive the robot around with the jaws in catch-ready position and catch whatever enters jaws (hopefully, balls).
• Ball's recognition before collection with distance sensors (lidars). When balls are in the middle of the area, it is possible to find them by using side distance sensors. However, the ones close to walls, especially in corners are more difficult, some of them most probably impossible. A little random walk can help in this case. Another problem is that black balls have a very small reflectance footprint and MRMS LIDAR 2m VL53L0X, CAN Bus (mrm-lid-can-b) will mostly not recognize them. You can try with MRMS LIDAR 4m VL53L1X, CAN Bus (mrm-lid-can-b2). They are more sensitive and can be configured.
• Ball's recognition before collection with a camera. This model doesn't have one but there may be another one in the future that will have it. The camera will have no problems with black balls, but will struggle to recognize reflective ones. It is difficult to make a difference between white and silver.

Next steps

The part in which we presented various hardware elements is finished. Next lessons will be more advanced.