Subversion Repositories Projects

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

  }

}