/* * 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); }