DASL Wiki

User Tools

Site Tools

Trace: • here_you_go_read_carefully
robotino_arm_control

Differences

This shows you the differences between two versions of the page.

Link to this comparison view

robotino_arm_control [2016/10/25 15:32] – created dwallacerobotino_arm_control [2016/10/28 17:51] (current) – dwallace
Line 8: Line 8:
  
 The completed project ready to upload via ftp:  The completed project ready to upload via ftp: 
-Poser program:  [[dylanw:poser.zip] +Poser program:  {{dylanw:poser.zip}} 
-Grab_n_run program [[dylanw:grab_n_run.zip]]+Grab_n_run program {{dylanw:grab_n_run.zip}}
  
 ===== Video ===== ===== Video =====
Line 111: Line 111:
   rec::robotino::api2::shutdown();   rec::robotino::api2::shutdown();
   exit( 1 );   exit( 1 );
-   
     }     }
          
     else     else
     {     {
-  std::cout << "success" << std::endl; +  std::cout << "success" << std::endl
-          +
     }     }
 } }
Line 176: Line 174:
 Now we need to ask the user for the desired pose. We can do this just after we print out "success" in the init class. Now we need to ask the user for the desired pose. We can do this just after we print out "success" in the init class.
  
-          char x; +<code c++> 
-          while(true){ +    char x; 
-              std::cout << "please pose the arm in the position you would like it to stay in. type 'y' to accept pose  " << std::endl;  // ask user to enter "y" when happy with the arm position + 
-              std::cin>> x; +    while(true){ 
-              if(x=='y'){      +        std::cout << "please pose the arm in the position you would like it to stay in. type 'y' to accept pose  " << std::endl;  // ask user to enter "y" when happy with the arm position 
-                  cbha.stringPots( targetValues );      +        std::cin>> x; 
-              } +         
-              std::cout << "here is your pose." << std::endl;   // read out the values measured by the potentiometer to read the arm position. +        if(x=='y'){      
-              for( unsigned int i = 0; i < 6; ++i ) +            cbha.stringPots( targetValues );      
-              { +        } 
-                  std::cout << "String pot " << i  << ": " << targetValues[i] << std::endl; +         
-              } +        std::cout << "here is your pose." << std::endl;   // read out the values measured by the potentiometer to read the arm position. 
-              std::cout << "accept y/n" << std::endl;  // ask user if they are happy with this pose +         
-              std::cin>> x; +        for( unsigned int i = 0; i < 6; ++i ) 
-              if(x=='y'){ +        { 
-                  break; +            std::cout << "String pot " << i  << ": " << targetValues[i] << std::endl; 
-              } +        } 
-          }+         
 +        std::cout << "accept y/n" << std::endl;  // ask user if they are happy with this pose 
 +        std::cin>> x; 
 +         
 +        if(x=='y'){ 
 +            break; 
 +        } 
 +    }
                      
-          cbha.setCompressorsEnabled( true );  // enable the compressor +    cbha.setCompressorsEnabled( true );  // enable the compressor 
 +</code>
  
 Now we are ready to implement out control.  This is done in the drive() function.   Now we are ready to implement out control.  This is done in the drive() function.  
Line 204: Line 209:
 This control works for both sets of bellows.  i.e. the upper and lower set. This control works for both sets of bellows.  i.e. the upper and lower set.
  
- +<code c++> 
-  void drive() +void drive() 
-  { +{ 
-      float pressures[8] = { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f };  // the pressures in our bellows +    float pressures[8] = { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f };  // the pressures in our bellows 
-      float values[6] = { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};   // the values read by the potentiometers+    float values[6] = { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};   // the values read by the potentiometers
              
-      float stepSize=.5;   // the step size for our proportional control  +    float stepSize=.5;   // the step size for our proportional control  
-      float lastTime=com.msecsElapsed();  // remember the last time  +    float lastTime=com.msecsElapsed();  // remember the last time  
-      float lowerAvg;  // the average pressure in the lower part of the arm +    float lowerAvg;  // the average pressure in the lower part of the arm 
-      float upperAvg;   // the average pressure in the upper part of the arm+    float upperAvg;   // the average pressure in the upper part of the arm
      
-  while( com.isConnected() && false == bumper.value() && _run ) +    while( com.isConnected() && false == bumper.value() && _run ) 
-   { +    { 
-          // update the string pot values +        // update the string pot values 
-          cbha.stringPots( values );+        cbha.stringPots( values );
                      
-          // move toward target+        // move toward target
                      
-          //  This is our control for the lower arm.   +        //  This is our control for the lower arm.   
-          pressures[0]=pressures[0]+(-values[0]+targetValues[0])*stepSize+(values[1]-targetValues[1])*stepSize+(values[2]-targetValues[2])*stepSize; +        pressures[0]=pressures[0]+(-values[0]+targetValues[0])*stepSize+(values[1]-targetValues[1])*stepSize+(values[2]-targetValues[2])*stepSize; 
-          pressures[1]=pressures[1]+(values[0]-targetValues[0])*stepSize+(-values[1]+targetValues[1])*stepSize+(values[2]-targetValues[2])*stepSize; +        pressures[1]=pressures[1]+(values[0]-targetValues[0])*stepSize+(-values[1]+targetValues[1])*stepSize+(values[2]-targetValues[2])*stepSize; 
-          pressures[2]=pressures[2]+(values[0]-targetValues[0])*stepSize+(values[1]-targetValues[1])*stepSize+(-values[2]+targetValues[2])*stepSize;+        pressures[2]=pressures[2]+(values[0]-targetValues[0])*stepSize+(values[1]-targetValues[1])*stepSize+(-values[2]+targetValues[2])*stepSize;
                      
-          //  now we average up the pressures we calculated +        //  now we average up the pressures we calculated 
-          lowerAvg=(pressures[0]+pressures[1]+pressures[2])/3;+        lowerAvg=(pressures[0]+pressures[1]+pressures[2])/3;
                      
-          //  subtract the average so the average value of the bellows does not grow over time.   +        //  subtract the average so the average value of the bellows does not grow over time.   
-          pressures[0]-=lowerAvg; +        pressures[0]-=lowerAvg; 
-          pressures[1]-=lowerAvg; +        pressures[1]-=lowerAvg; 
-          pressures[2]-=lowerAvg;+        pressures[2]-=lowerAvg;
                              
-          //  repeat for the upper arm +        //  repeat for the upper arm 
-          pressures[3]=pressures[3]+(-values[3]+targetValues[3])*stepSize+(values[4]-targetValues[4])*stepSize+(values[5]-targetValues[5])*stepSize; +        pressures[3]=pressures[3]+(-values[3]+targetValues[3])*stepSize+(values[4]-targetValues[4])*stepSize+(values[5]-targetValues[5])*stepSize; 
-          pressures[4]=pressures[4]+(values[3]-targetValues[3])*stepSize+(-values[4]+targetValues[4])*stepSize+(values[5]-targetValues[5])*stepSize; +        pressures[4]=pressures[4]+(values[3]-targetValues[3])*stepSize+(-values[4]+targetValues[4])*stepSize+(values[5]-targetValues[5])*stepSize; 
-          pressures[5]=pressures[5]+(values[3]-targetValues[3])*stepSize+(values[4]-targetValues[4])*stepSize+(-values[5]+targetValues[5])*stepSize;+        pressures[5]=pressures[5]+(values[3]-targetValues[3])*stepSize+(values[4]-targetValues[4])*stepSize+(-values[5]+targetValues[5])*stepSize;
        
-          upperAvg=(pressures[3]+pressures[4]+pressures[5])/3;+        upperAvg=(pressures[3]+pressures[4]+pressures[5])/3;
                      
-          pressures[3]-=upperAvg; +        pressures[3]-=upperAvg; 
-          pressures[4]-=upperAvg; +        pressures[4]-=upperAvg; 
-          pressures[5]-=upperAvg;+        pressures[5]-=upperAvg;
                      
-          //  now we send our calculated pressures to the robotino api. +        //  now we send our calculated pressures to the robotino api. 
-          cbha.setPressures( pressures );+        cbha.setPressures( pressures );
      
                      
                      
-          // list values+        // list values
                      
-          std::cout << std::endl;+        std::cout << std::endl;
                      
-          std::cout << "avg: " << lowerAvg << " bar" << std::endl;+        std::cout << "avg: " << lowerAvg << " bar" << std::endl;
                      
-          std::cout << std::endl;+        std::cout << std::endl;
                      
-          for( unsigned int i = 0; i < 8; ++i ) +        for( unsigned int i = 0; i < 8; ++i ) 
-          { +        { 
-              std::cout << "B" << i << ": " << pressures[i] << " bar" << std::endl; +            std::cout << "B" << i << ": " << pressures[i] << " bar" << std::endl; 
-          }+        }
                      
-          std::cout << std::endl;+        std::cout << std::endl;
                      
-          for( unsigned int i = 0; i < 6; ++i ) +        for( unsigned int i = 0; i < 6; ++i ) 
-          { +        { 
-              std::cout << "String pot " << i  << ": " << values[i] << std::endl; +            std::cout << "String pot " << i  << ": " << values[i] << std::endl; 
-          }+        }
      
                      
-          // syncronize and delay +        // syncronize and delay 
-          lastTime=com.msecsElapsed(); +        lastTime=com.msecsElapsed(); 
-   com.processEvents(); +        com.processEvents(); 
-   rec::robotino::api2::msleep( 100 ); +   rec::robotino::api2::msleep( 100 ); 
-   } +    }       
-       +} 
-       +</code>
-  }+
  
 +Now the program is ready to compile and run.  You can download the completed program and it's makefile here {{dylanw:poser.zip}}
  
-  +===== Moving to an object and grabbing it =====
-Now the program is ready to compile and run.  You can download the completed program and it's makefile here [[:File:poser.zip]]+
  
 +Now that we can move the arm we want to move to a known location and grab an object there.  We will modify the poser program wrote above to do just that.
  
- +First we need to declare a few more things above our init() function.
-== Moving to an object and grabbing it == +
- +
-Now that we can move the arm we want to move to a known location and grab an object there.  We will modify the proser program wrote above to do just that. +
- +
-First we need to declare a few more things above our init() funciton.+
  
   OmniDrive omniDrive;   OmniDrive omniDrive;
Line 298: Line 298:
 we will use the odometry class to measure the robots position. we will use the odometry class to measure the robots position.
  
-We no longer need the posing code in our init class.  Additionally we will need to open the grippers and set the odometer to 0.  Your init() funciton should look like this+We no longer need the posing code in our init class.  Additionally we will need to open the grippers and set the odometer to 0.  Your init() function should look like this
  
-  void init( const std::string& hostname ) +<code c++ init.cpp> 
-  { +void init( const std::string& hostname ) 
-   // Initialize the actors+{ 
 +    // Initialize the actors
      
-  // Connect +    // Connect 
-   std::cout << "Connecting... "; +    std::cout << "Connecting... "; 
-   com.setAddress( hostname.c_str() );+    com.setAddress( hostname.c_str() );
      
-  com.connectToServer( true );+    com.connectToServer( true );
      
-  if( false == com.isConnected() ) +    if( false == com.isConnected() ) 
-   { +    { 
-   std::cout << std::endl << "Could not connect to " << com.address() << std::endl;+        std::cout << std::endl << "Could not connect to " << com.address() << std::endl;
      
-  rec::robotino::api2::shutdown(); +  rec::robotino::api2::shutdown(); 
-   exit( 1 ); +   exit( 1 ); 
-   } +    } 
-   else +     
-   { +    else 
-   std::cout << "success" << std::endl;+    { 
 +   std::cout << "success" << std::endl;
                      
-          cbha.setCompressorsEnabled( true ); +        cbha.setCompressorsEnabled( true ); 
-          cbha.setGripperValve1( false ); +        cbha.setGripperValve1( false ); 
-          cbha.setGripperValve2( false );+        cbha.setGripperValve2( false );
                      
-          odometry.set(0,0,0,false);+        odometry.set(0,0,0,false);
                      
-  } +    } 
-  }+} 
 +</code>
  
 +Now lets look at the updated drive() function
  
-Now lets look at the updated drive() funciton +<code c++ drive.cpp> 
- +void drive() 
-  void drive() +{ 
-  { +    float targetValues[6] = { 0.591398, 0.522972, 0.532747, 0.796676, 0.821114, 0.904203};   // this is where we will store the current position we want  
-      float targetValues[6] = { 0.591398, 0.522972, 0.532747, 0.796676, 0.821114, 0.904203};   // this is where we will store the current position we want  +    float caryPose[6] = { 0.591398, 0.522972, 0.532747, 0.796676, 0.821114, 0.904203};   //  this is the pose to cary an object.  Use your poser program from earlier to create your own values 
-      float caryPose[6] = { 0.591398, 0.522972, 0.532747, 0.796676, 0.821114, 0.904203};   //  this is the pose to cary an object.  Use your poser program from earlier to create your own values +    float pressures[8] = { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f }; 
-      float pressures[8] = { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f }; +    float values[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; 
-      float values[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; +    float pickupPose[6]={0.777126,0.469208,0.454545,0.938416,0.782014,0.826002};  //  this is the pose to pick up an object.  Use your poser program from earlier to create your own values 
-      float pickupPose[6]={0.777126,0.469208,0.454545,0.938416,0.782014,0.826002};  //  this is the pose to pick up an object.  Use your poser program from earlier to create your own values +    float errors[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};  // we want to know how far away we are from the desired values we will store that error here 
-      float errors[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};  // we want to know how far away we are from the desired values we will store that error here +    float tolerance=.02;  // this is how far we are willing to be off from our desired values
-      float tolerance=.02;  // this is how far we are willing to be off from our desired values+
              
              
-      float vel[3] = {0.0, 0.0, 0.0};  // the velocity see the rectangle tutorial for more info +    float vel[3] = {0.0, 0.0, 0.0};  // the velocity see the rectangle tutorial for more info 
-      unsigned int * seq; +    unsigned int * seq; 
-      double x;  // x position +    double x;  // x position 
-      double y;   // y positon +    double y;   // y positon 
-      double phi;  // rotation +    double phi;  // rotation 
              
-      bool inTolerance=false;  // are we in acceptable tolerance for arm position +    bool inTolerance=false;  // are we in acceptable tolerance for arm position 
-      bool closed=false;  // is the claw closed+    bool closed=false;  // is the claw closed
              
-      float stepSize=.5; +    float stepSize=.5; 
-      float lastTime=com.msecsElapsed(); +    float lastTime=com.msecsElapsed(); 
-      float lowerAvg; +    float lowerAvg; 
-      float upperAvg;+    float upperAvg;
      
-      for( unsigned int i = 0; i < 6; ++i ){ +    for( unsigned int i = 0; i < 6; ++i ){ 
-          targetValues[i]=pickupPose[i]; +        targetValues[i]=pickupPose[i]; 
-      } +    } 
-   while( com.isConnected() && false == bumper.value() && _run ) +     
-   { +    while( com.isConnected() && false == bumper.value() && _run ) 
-          // update the string pot values +    { 
-          cbha.stringPots( values );+        // update the string pot values 
 +        cbha.stringPots( values );
                      
-          // this is the same as the poser program+        // this is the same as the poser program
                      
-          pressures[0]=pressures[0]+(-values[0]+targetValues[0])*stepSize+(values[1]-targetValues[1])*stepSize+(values[2]-targetValues[2])*stepSize; +        pressures[0]=pressures[0]+(-values[0]+targetValues[0])*stepSize+(values[1]-targetValues[1])*stepSize+(values[2]-targetValues[2])*stepSize; 
-          pressures[1]=pressures[1]+(values[0]-targetValues[0])*stepSize+(-values[1]+targetValues[1])*stepSize+(values[2]-targetValues[2])*stepSize; +        pressures[1]=pressures[1]+(values[0]-targetValues[0])*stepSize+(-values[1]+targetValues[1])*stepSize+(values[2]-targetValues[2])*stepSize; 
-          pressures[2]=pressures[2]+(values[0]-targetValues[0])*stepSize+(values[1]-targetValues[1])*stepSize+(-values[2]+targetValues[2])*stepSize;+        pressures[2]=pressures[2]+(values[0]-targetValues[0])*stepSize+(values[1]-targetValues[1])*stepSize+(-values[2]+targetValues[2])*stepSize;
                      
-          lowerAvg=(pressures[0]+pressures[1]+pressures[2])/3;+        lowerAvg=(pressures[0]+pressures[1]+pressures[2])/3;
                      
-          pressures[0]-=lowerAvg; +        pressures[0]-=lowerAvg; 
-          pressures[1]-=lowerAvg; +        pressures[1]-=lowerAvg; 
-          pressures[2]-=lowerAvg;+        pressures[2]-=lowerAvg;
                              
-          pressures[3]=pressures[3]+(-values[3]+targetValues[3])*stepSize+(values[4]-targetValues[4])*stepSize+(values[5]-targetValues[5])*stepSize; +        pressures[3]=pressures[3]+(-values[3]+targetValues[3])*stepSize+(values[4]-targetValues[4])*stepSize+(values[5]-targetValues[5])*stepSize; 
-          pressures[4]=pressures[4]+(values[3]-targetValues[3])*stepSize+(-values[4]+targetValues[4])*stepSize+(values[5]-targetValues[5])*stepSize; +        pressures[4]=pressures[4]+(values[3]-targetValues[3])*stepSize+(-values[4]+targetValues[4])*stepSize+(values[5]-targetValues[5])*stepSize; 
-          pressures[5]=pressures[5]+(values[3]-targetValues[3])*stepSize+(values[4]-targetValues[4])*stepSize+(-values[5]+targetValues[5])*stepSize;+        pressures[5]=pressures[5]+(values[3]-targetValues[3])*stepSize+(values[4]-targetValues[4])*stepSize+(-values[5]+targetValues[5])*stepSize;
        
-          upperAvg=(pressures[3]+pressures[4]+pressures[5])/3;+        upperAvg=(pressures[3]+pressures[4]+pressures[5])/3;
                      
-          pressures[3]-=upperAvg; +        pressures[3]-=upperAvg; 
-          pressures[4]-=upperAvg; +        pressures[4]-=upperAvg; 
-          pressures[5]-=upperAvg;+        pressures[5]-=upperAvg;
                      
-          pressures[7]=10;+        pressures[7]=10;
                      
-          cbha.setPressures( pressures );+        cbha.setPressures( pressures );
                      
-          // determine if we are in tolerance +        // determine if we are in tolerance 
-          for( unsigned int i = 0; i < 6; ++i ){ +        for( unsigned int i = 0; i < 6; ++i ){ 
-              errors[i]=targetValues[i]-values[i]; +            errors[i]=targetValues[i]-values[i]; 
-              std::cout<< errors[i] << std::endl; +            std::cout<< errors[i] << std::endl; 
-              if(tolerance > std::abs(errors[i])   ){ +            if(tolerance > std::abs(errors[i])   ){       
-                   +                std::cout << "in tolerance: " << i <<  std::endl; 
-                  std::cout << "in tolerance: " << i <<  std::endl; +            } 
-              } +             
-              else{ +            else{ 
-                  break; +              break; 
-              }+            }
                              
-              if(i==5){ +            if(i==5){ 
-                   inTolerance=true; +                 inTolerance=true; 
-              } +            } 
-              else inTolerance=false;+             
 +            else inTolerance=false;
                              
-          }+        }
                      
                      
-          // movement+        // movement
                      
-          odometry.readings( &x, &y, &phi );+        odometry.readings( &x, &y, &phi );
                      
                      
-          std::cout << "Pos:" << x << "," << y << ","<< phi << std::endl;+        std::cout << "Pos:" << x << "," << y << ","<< phi << std::endl;
                      
-  omniDrive.setVelocity( vel[0], vel[1], vel[2] );   // update our velocity vector using the robotino api omni drive+  omniDrive.setVelocity( vel[0], vel[1], vel[2] );   // update our velocity vector using the robotino api omni drive
      
                      
-          // algorithm+        // algorithm
      
      
-          // this is the first step.  If we are in tolerance and at the start location start moving. +        // this is the first step.  If we are in tolerance and at the start location start moving. 
-          if (inTolerance==true && x<=0 && !closed){ +        if (inTolerance==true && x<=0 && !closed){ 
-              vel[0]=.2; +            vel[0]=.2; 
-          } +        } 
-          // seccond step.  stop when we get .5 meters away from start  then close the gropper  +        // second step.  stop when we get .5 meters away from start  then close the gripper  
-          if (inTolerance==true && x<=0.51 && x>=.49 && !closed){ +         
-              vel[0]=0;  +        if (inTolerance==true && x<=0.51 && x>=.49 && !closed){ 
-              rec::robotino::api2::msleep( 2000 );  // wait 2 seconds  +            vel[0]=0;  
-              cbha.setGripperValve1( true );   // close gripper valves +            rec::robotino::api2::msleep( 2000 );  // wait 2 seconds  
-              cbha.setGripperValve2( true ); +            cbha.setGripperValve1( true );   // close gripper valves 
-              rec::robotino::api2::msleep( 1000 );  // wait 1 second +            cbha.setGripperValve2( true ); 
-              closed=true; +            rec::robotino::api2::msleep( 1000 );  // wait 1 second 
-              for( unsigned int i = 0; i < 6; ++i ){   // switch our pose to the cary pose+            closed=true; 
 +             
 +            for( unsigned int i = 0; i < 6; ++i ){   // switch our pose to the cary pose
                   targetValues[i]=caryPose[i];                     targetValues[i]=caryPose[i];  
               }               }
           }           }
 +          
           // now that the gripper is closed we want to start rotating.           // now that the gripper is closed we want to start rotating.
           if (inTolerance==true && x<=0.51 && x>=.49 && closed){           if (inTolerance==true && x<=0.51 && x>=.49 && closed){
               vel[2]=.5;               vel[2]=.5;
           }           }
 +          
           // once you have rotated 2.25 radians stop and wait 5 seconds.           // once you have rotated 2.25 radians stop and wait 5 seconds.
           if (phi>=2.2&&phi<=2.3) {           if (phi>=2.2&&phi<=2.3) {
Line 479: Line 488:
           // syncronize and delay           // syncronize and delay
           lastTime=com.msecsElapsed();           lastTime=com.msecsElapsed();
-  com.processEvents(); +          com.processEvents(); 
-   rec::robotino::api2::msleep( 100 ); +     rec::robotino::api2::msleep( 100 ); 
-   } +    } 
-       +} 
-       +</code>
-  } +
- +
  
-The completed file can be downloaded here [[:File:grab_n_run.zip]]+The completed file can be downloaded here {{dylanw:grab_n_run.zip}}
robotino_arm_control.1477434763.txt.gz · Last modified: 2016/10/25 15:32 by dwallace