summaryrefslogtreecommitdiff
path: root/USB_Host_Shield/examples/arm_mouse.pde
diff options
context:
space:
mode:
Diffstat (limited to 'USB_Host_Shield/examples/arm_mouse.pde')
-rw-r--r--USB_Host_Shield/examples/arm_mouse.pde284
1 files changed, 0 insertions, 284 deletions
diff --git a/USB_Host_Shield/examples/arm_mouse.pde b/USB_Host_Shield/examples/arm_mouse.pde
deleted file mode 100644
index e934354..0000000
--- a/USB_Host_Shield/examples/arm_mouse.pde
+++ /dev/null
@@ -1,284 +0,0 @@
-/* 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 );
- }
-}
-
generated by cgit v1.2.3 (git 2.25.1) at 2024-06-09 21:56:41 +0000