Please update your bookmarks..
Go to my new LEGO website
http://mastincrosbie.com/Marks_LEGO_projects
LEGO Grabber Arm
 The Mindstorms web site
has instructions on how to build a grabber arm that operates from a
fixed base point. I felt that the controls they implemented were very
simplistic, and didn't give a good degree of control of the arm. So
naturally, I had to do one better...
Part of the problem with the original grabber arm code was that is was
binary - the motors were either on or off. In reality, you want a
finer resolution of control over a motor with varying speeds for
different needs.
 A rotation sensor provides an
ideal input device for variable speed control. If the rotation sensor
rotates faster, the motor rotates faster. If the control wheel is
rotated in the opposite direction, the motor operates in the reverse
direction. The operator is given a more intuitive control mechanism.
The code for this project leverages code I wrote for the tachometer,
speed differential and stall detector projects.
Wiring and Construction:
- Left Button on input 3
- Activate claw control. Rotating the wheel input will open or
close the claw.
- Rotation sensor on input 2
- Speed of movement control for either the claw or the elbow.
The rate of rotation of the wheel controls the rate at which the
joint will move. Rotating the wheel in the opposite direction
will change the direction of joint movement.
- Right button on input 1
- Activate elbow joint. Rotating the wheel input will raise or
lower the elbow joint.
The goal is to have the speed of the joint movement be directly
related to the rate of rotation of the sensor on input 2. A sample
loop will count the number of rotation sensor pulses over a time
period and compute a angular velocity value. This value will be used
to adjust the motor speeds.
A linear scale can be used:
16 pulses in 1 second = 1 wheel rotation = Max speed
0 pulses in 1 second = wheel is stopped = Stop motor
 This basic version works
well. I can actually guide it to pick up a LEGO wheel from my desk!
But the gearing and design on the elbow portion leaves a lot to be
desired. The arm needs to be strengthened and the claw needs to be
made to close straight.
Click here to download the source code.
/*
* grabberarm.c
*
* I built the grabber arm as described on the Mindstorm's web site. Of
* course, the software that they provide only works with the RCX
* programming software bundled with the Mindstorms box. So I changed the
* light sensor for a rotation sensor and wrote this code.
*
* Left Button on input 3 Activate claw
* Rot sensor on input 2 Speed of movement
* Right button on input 1 Activate elbow joint
*
* The goal is to have the speed of the joint movement be directly
* related to the rate of rotation of the sensor on input 2. A sample
* loop will count the number of rotation sensor pulses over a time
* period and compute a angular velocity value. This value will be used
* to adjust the motor speeds.
*
* A linear scale can be used:
* 16 pulses in 1 second = 1 wheel rotation = Max speed
* 0 pulses in 1 second = wheel is stopped = Stop motor
*
* Directions:
* Press and hold a motor button
* Rotate the rotation sensor to move the motor
* Release the button and the motor stops
*
* Mark Crosbie 9/25/00 mark@mastincrosbie.com
*
*/
#include
#include
#include
#include
#include
#include
/****************** GLOBAL DEFINES ***********************************/
#define SPEEDSENSOR ROTATION_2
#define BUTTONCLAW TOUCH_3
#define BUTTONELBOW TOUCH_1
#define MOTORCLAW MOTOR_C
#define MOTORELBOW MOTOR_A
/* time interval between each sample point in ms */
#define SAMPLE_INTERVAL 250
/* time to apply brakes for in ms */
#define BRAKE_INTERVAL 500
/* a handy abs() function */
#define ABS(x) ((x)>=0)?(x):-(x)
/***************** FUNCTION PROTOTYPES ******************************/
void initAll(void);
int rotationThread(int argc, char **argv);
int clawThread(int argc, char **argv);
int elbowThread(int argc, char **argv);
int setSpeed(int rotVal);
/***************** GLOBAL VARIABLES *********************************/
/* boolean - is the claw button or the elbow button pressed, or both? */
int clawPressed, elbowPressed;
/* current velocity vector for each motor. Negative implies that the
* wheel is being rotated backwards, so reverse motor direction
*/
int clawVelocity, elbowVelocity;
/* current speed setting of each motor */
int clawSpeed, elbowSpeed;
/* what was read from the rotation sensor */
int rotationValue;
int main(int argc, char *argv[]) {
initAll();
while(1) {
lcd_int(rotationValue);
msleep(SAMPLE_INTERVAL);
}
return 0;
}
/*
* Name:
* initAll
*
* Description:
* Initialize the sensor and motor ports
* Reset all variables
*
* Parameters:
* None
* Returns:
* None
*/
void initAll(void) {
/* start rotation sensor processing */
ds_active(&SENSOR_2);
/* reset the rotation value */
ds_rotation_set(&SENSOR_2, 0);
ds_rotation_on(&SENSOR_2);
/* and turn off the motor */
motor_a_dir(off);
motor_c_dir(off);
clawPressed = elbowPressed = 0;
clawVelocity = elbowVelocity = 0;
/* start the thread that constantly monitors the rotation sensor */
execi(rotationThread, 0, (char **)NULL, PRIO_NORMAL,DEFAULT_STACK_SIZE);
/* start the tasks that monitors the claw button and adjusts the
* claw motor speed
*/
execi(clawThread, 0, (char **)NULL, PRIO_NORMAL,DEFAULT_STACK_SIZE);
/* start the tasks that monitors the elbow button and adjusts the
* elbow motor speed
*/
execi(elbowThread, 0, (char **)NULL, PRIO_NORMAL, DEFAULT_STACK_SIZE);
/* start task processing */
tm_start();
}
/*
* Name:
* rotationThread
*
* Description:
* Continually update the rotationValue global
*
* Parameters:
* data: (in) ignored
*
* Returns:
* None
*
* Globals modified:
* rotationValue
*
* Algorithm:
* If either sensor has changed then update val1 and val2
* otherwise leave them be. Flag if we detect a change and return
*/
int rotationThread(int argc, char **argv) {
int val1=0, val2 = 0;
while(1) {
/* reset the rotation value */
ds_rotation_set(&SENSOR_2, 0);
/* take the readings */
val1 = SPEEDSENSOR;
msleep(SAMPLE_INTERVAL);
val1 = SPEEDSENSOR;
rotationValue = (val1 - val2);
msleep(SAMPLE_INTERVAL);
}
}
/*
* Name:
* clawThread
*
* Description:
* Watch the claw button and move the claw
*
* Parameters:
* data: (in) ignored
*
* Returns:
* None
*
* Globals modified:
* None
*
* Algorithm:
*/
int clawThread(int argc, char **argv) {
while(1){
/* if the claw activate button is pressed then monitor the roation */
/* sensor. */
if(BUTTONCLAW == 1) {
/* if the rotation sensor is not moving then don't bother with
* the next set of checks
*/
if(rotationValue == 0) {
/* stop the claw motor */
motor_c_dir(off);
continue;
}
/* if the sign on the rotationValue is negative, then reverse
* the motor direction, otherwise keep it moving forward
*/
if(rotationValue < 0) {
motor_c_dir(rev);
motor_c_speed(ABS(setSpeed(rotationValue)));
} else {
motor_c_dir(fwd);
motor_c_speed(setSpeed(rotationValue));
}
} else {
/* otherwise stop the claw motor */
motor_c_dir(off);
}
}
}
/*
* Name:
* elbowThread
*
* Description:
* Watch the claw button and move the claw
*
* Parameters:
* data: (in) ignored
*
* Returns:
* None
*
* Globals modified:
* None
*
* Algorithm:
*/
int elbowThread(int argc, char **argv) {
while(1){
/* if the elbow activate button is pressed then monitor the roation */
/* sensor. */
if(BUTTONELBOW == 1) {
/* if the rotation sensor is not moving then don't bother with
* the next set of checks
*/
if(rotationValue == 0) {
/* stop the claw motor */
motor_a_dir(off);
continue;
}
/* if the sign on the rotationValue is negative, then reverse
* the motor direction, otherwise keep it moving forward
*/
if(rotationValue < 0) {
motor_a_dir(rev);
motor_a_speed(ABS(setSpeed(rotationValue)));
} else {
motor_a_dir(fwd);
motor_a_speed(setSpeed(rotationValue));
}
} else {
/* otherwise stop the claw motor */
motor_a_dir(off);
}
}
}
int setSpeed(int rotValue) {
return(rotValue * 16);
}
|
