Permalink
Find file
afa275f Jul 16, 2010
285 lines (255 sloc) 8.85 KB
/* AL5D robotic arm manual control using USB mouse. Servo controller by Renbotics with some pins swapped, USB Host Shield by Circuits At Home */
#include <ServoShield.h>
#include <Spi.h>
#include <Max3421e.h>
#include <Usb.h>
#define DEVADDR 1
#define CONFVALUE 1
/* Arm dimensions( mm ) */
#define BASE_HGT 67.31 //base hight 2.65"
#define HUMERUS 146.05 //shoulder-to-elbow "bone" 5.75"
#define ULNA 187.325 //elbow-to-wrist "bone" 7.375"
#define GRIPPER 100.00 //gripper (incl.heavy duty wrist rotate mechanism) length 3.94"
#define ftl(x) ((x)>=0?(long)((x)+0.5):(long)((x)-0.5)) //float to long conversion
/* Servo names/numbers */
/* Base servo HS-485HB */
#define BAS_SERVO 0
/* Shoulder Servo HS-5745-MG */
#define SHL_SERVO 1
/* Elbow Servo HS-5745-MG */
#define ELB_SERVO 2
/* Wrist servo HS-645MG */
#define WRI_SERVO 3
/* Wrist rotate servo HS-485HB */
#define WRO_SERVO 4
/* Gripper servo HS-422 */
#define GRI_SERVO 5
//#define ARM_PARK set_arm( -50, 140, 100, 0 ) //arm parking position
/* pre-calculations */
float hum_sq = HUMERUS*HUMERUS;
float uln_sq = ULNA*ULNA;
void setup();
void loop();
ServoShield servos; //ServoShield object
MAX3421E Max;
USB Usb;
//ServoShield servos; //ServoShield object
/* Arm data structure */
struct {
float x_coord; // X coordinate of the gripper tip
float y_coord; // Y coordinate of the gripper tip
float z_coord; //Z coordinate of the gripper tip
float gripper_angle; //gripper angle
int16_t gripper_servo; //gripper servo pulse duration
int16_t wrist_rotate; //wrist rotate servo pulse duration
} armdata;
void setup()
{
/* set servo end points */
servos.setbounds( BAS_SERVO, 900, 2100 );
servos.setbounds( SHL_SERVO, 1000, 2100 );
servos.setbounds( ELB_SERVO, 900, 2100 );
servos.setbounds( WRI_SERVO, 600, 2400 );
servos.setbounds( WRO_SERVO, 600, 2400 );
servos.setbounds( GRI_SERVO, 890, 2100 );
/**/
// servo_park();
arm_park();
servos.start(); //Start the servo shield
Max.powerOn();
Serial.begin( 115200 );
Serial.println("Start");
delay( 500 );
//ARM_PARK;
}
void loop()
{
byte rcode;
//delay( 10 );
set_arm( armdata.x_coord, armdata.y_coord, armdata.z_coord, armdata.gripper_angle );
servos.setposition( WRO_SERVO, armdata.wrist_rotate );
servos.setposition( GRI_SERVO, armdata.gripper_servo );
//ARM_PARK;
// circle();
Max.Task();
Usb.Task();
if( Usb.getUsbTaskState() == USB_STATE_CONFIGURING ) {
mouse_init();
}//if( Usb.getUsbTaskState() == USB_STATE_CONFIGURING...
if( Usb.getUsbTaskState() == USB_STATE_RUNNING ) { //poll the keyboard
rcode = mouse_poll();
if( rcode ) {
Serial.print("Mouse Poll Error: ");
Serial.println( rcode, HEX );
}//if( rcode...
}//if( Usb.getUsbTaskState() == USB_STATE_RUNNING...
//Serial.println( armdata.gripper_servo, DEC );
}
/* Initialize mouse */
void mouse_init( void )
{
byte rcode = 0; //return code
/**/
Usb.setDevTableEntry( 1, Usb.getDevTableEntry( 0,0 ) ); //copy device 0 endpoint information to device 1
/* Configure device */
rcode = Usb.setConf( DEVADDR, 0, CONFVALUE );
if( rcode ) {
Serial.print("Error configuring mouse. Return code : ");
Serial.println( rcode, HEX );
while(1); //stop
}//if( rcode...
Usb.setUsbTaskState( USB_STATE_RUNNING );
return;
}
/* Poll mouse using Get Report and fill arm data structure */
byte mouse_poll( void )
{
byte rcode;
char buf[ 4 ]; //mouse buffer
static uint16_t delay = 500; //delay before park
/* poll mouse */
rcode = Usb.getReport( DEVADDR, 0, 4, 0, 1, 0, buf );
if( rcode ) { //error
return( rcode );
}
// todo: add arm limit check
armdata.x_coord += ( buf[ 1 ] * -0.1 );
armdata.y_coord += ( buf[ 2 ] * -0.1 );
switch( buf[ 0 ] ) { //read buttons
case 0x00: //no buttons pressed
armdata.z_coord += ( buf[ 3 ] * -2 ) ;
break;
case 0x01: //button 1 pressed. Wheel sets gripper angle
armdata.gripper_servo += ( buf[ 3 ] * -20 );
/* check gripper boundaries */
if( armdata.gripper_servo < 1000 ) {
armdata.gripper_servo = 1000;
}
if( armdata.gripper_servo > 2100 ) {
armdata.gripper_servo = 2100;
}
break;
case 0x02: //button 2 pressed. Wheel sets wrist rotate
armdata.wrist_rotate += ( buf[ 3 ] * -10 );
/* check wrist rotate boundaries */
if( armdata.wrist_rotate < 600 ) {
armdata.wrist_rotate = 600;
}
if( armdata.wrist_rotate > 2400 ) {
armdata.wrist_rotate = 2400;
}
break;
case 0x04: //wheel button pressed. Wheel controls gripper
armdata.gripper_angle += ( buf[ 3 ] * -1 );
/* check gripper angle boundaries */
if( armdata.gripper_angle < -90 ) {
armdata.gripper_angle = -90;
}
if( armdata.gripper_angle > 90 ) {
armdata.gripper_angle = 90;
}
break;
case 0x07: //all 3 buttons pressed. Park the arm
arm_park();
break;
}//switch( buf[ 0 ...
Serial.println( armdata.wrist_rotate, DEC );
}
/* arm positioning routine utilizing inverse kinematics */
/* z is height, y is distance from base center out, x is side to side. y,z can only be positive */
//void set_arm( uint16_t x, uint16_t y, uint16_t z, uint16_t grip_angle )
void set_arm( float x, float y, float z, float grip_angle_d )
{
float grip_angle_r = radians( grip_angle_d ); //grip angle in radians for use in calculations
/* Base angle and radial distance from x,y coordinates */
float bas_angle_r = atan2( x, y );
float rdist = sqrt(( x * x ) + ( y * y ));
/* rdist is y coordinate for the arm */
y = rdist;
/* Grip offsets calculated based on grip angle */
float grip_off_z = ( sin( grip_angle_r )) * GRIPPER;
float grip_off_y = ( cos( grip_angle_r )) * GRIPPER;
/* Wrist position */
float wrist_z = ( z - grip_off_z ) - BASE_HGT;
float wrist_y = y - grip_off_y;
/* Shoulder to wrist distance ( AKA sw ) */
float s_w = ( wrist_z * wrist_z ) + ( wrist_y * wrist_y );
float s_w_sqrt = sqrt( s_w );
/* s_w angle to ground */
//float a1 = atan2( wrist_y, wrist_z );
float a1 = atan2( wrist_z, wrist_y );
/* s_w angle to humerus */
float a2 = acos((( hum_sq - uln_sq ) + s_w ) / ( 2 * HUMERUS * s_w_sqrt ));
/* shoulder angle */
float shl_angle_r = a1 + a2;
float shl_angle_d = degrees( shl_angle_r );
/* elbow angle */
float elb_angle_r = acos(( hum_sq + uln_sq - s_w ) / ( 2 * HUMERUS * ULNA ));
float elb_angle_d = degrees( elb_angle_r );
float elb_angle_dn = -( 180.0 - elb_angle_d );
/* wrist angle */
float wri_angle_d = ( grip_angle_d - elb_angle_dn ) - shl_angle_d;
/* Servo pulses */
float bas_servopulse = 1500.0 - (( degrees( bas_angle_r )) * 11.11 );
float shl_servopulse = 1500.0 + (( shl_angle_d - 90.0 ) * 6.6 );
float elb_servopulse = 1500.0 - (( elb_angle_d - 90.0 ) * 6.6 );
float wri_servopulse = 1500 + ( wri_angle_d * 11.1 );
/* Set servos */
servos.setposition( BAS_SERVO, ftl( bas_servopulse ));
servos.setposition( WRI_SERVO, ftl( wri_servopulse ));
servos.setposition( SHL_SERVO, ftl( shl_servopulse ));
servos.setposition( ELB_SERVO, ftl( elb_servopulse ));
}
/* moves the arm to parking position */
void arm_park()
{
set_arm( armdata.x_coord = -50, armdata.y_coord = 140, armdata.z_coord = 100, armdata.gripper_angle = 0 );
servos.setposition( WRO_SERVO, armdata.wrist_rotate = 600 );
servos.setposition( GRI_SERVO, armdata.gripper_servo = 900 );
}
/* move servos to parking position */
void servo_park()
{
servos.setposition( BAS_SERVO, 1715 );
servos.setposition( SHL_SERVO, 2100 );
servos.setposition( ELB_SERVO, 2100 );
servos.setposition( WRI_SERVO, 1800 );
servos.setposition( WRO_SERVO, 600 );
servos.setposition( GRI_SERVO, 900 );
return;
}
void zero_x()
{
for( double yaxis = 150.0; yaxis < 356.0; yaxis += 1 ) {
set_arm( 0, yaxis, 127.0, 0 );
delay( 10 );
}
for( double yaxis = 356.0; yaxis > 150.0; yaxis -= 1 ) {
set_arm( 0, yaxis, 127.0, 0 );
delay( 10 );
}
}
/* moves arm in a straight line */
void line()
{
for( double xaxis = -100.0; xaxis < 100.0; xaxis += 0.5 ) {
set_arm( xaxis, 250, 100, 0 );
delay( 10 );
}
for( float xaxis = 100.0; xaxis > -100.0; xaxis -= 0.5 ) {
set_arm( xaxis, 250, 100, 0 );
delay( 10 );
}
}
void circle()
{
#define RADIUS 80.0
//float angle = 0;
float zaxis,yaxis;
for( float angle = 0.0; angle < 360.0; angle += 1.0 ) {
yaxis = RADIUS * sin( radians( angle )) + 200;
zaxis = RADIUS * cos( radians( angle )) + 200;
set_arm( 0, yaxis, zaxis, 0 );
delay( 1 );
}
}