Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with HTTPS or Subversion.

Download ZIP
Browse files

Robotic arm sketch showing inverse kinematics and USB HID interface

  • Loading branch information...
commit afa275fe49296279f56e7d2e1a886cde7e6235d6 1 parent 9533460
@felis authored
Showing with 284 additions and 0 deletions.
  1. +284 −0 examples/arm_mouse.pde
View
284 examples/arm_mouse.pde
@@ -0,0 +1,284 @@
+/* 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 );
+ }
+}
+
Please sign in to comment.
Something went wrong with that request. Please try again.