====== Robotino Arm Control ======

**This tutorial is part of the [[robotino_operation|Robotino Operation]] series.**

Purpose: To develop a controller for the robotino arm or Bionic Handling Assistant (BHA) and use it to pick up an object.   

===== Downloads =====

The completed project ready to upload via ftp: 
Poser program:  {{dylanw:poser.zip}}
Grab_n_run program {{dylanw:grab_n_run.zip}}

===== Video =====

Completed manual posing program

{{youtube>pF8ipgjjJZk?large}}

The completed program running.

{{youtube>q3--4yUuwIs?large}}

===== Controlling the arm =====

To control the arm we are going to create a posing program. We need to start with the same basic format from our last tutorial [[robotino_rectangle|Robotino Rectangle Tutorial]]. Please refer to that tutorial for an explanation of the below code.

<code c++ posingBoilerPlate.cpp>
#define _USE_MATH_DEFINES
#include <cmath>
#include <iostream>
#include <stdlib.h>
  
  
#include "rec/robotino/api2/all.h"
  
using namespace rec::robotino::api2;
  
float _rect[2]= {0,0};
  
bool _run = true;
  
  
class MyCom : public Com
{
    public:
  	MyCom()
  		: Com( "rect" )
  	{
  	}
  
  	void errorEvent( const char* errorString )
  	{
  		std::cerr << "Error: " << errorString << std::endl;
  	}
  
  	void connectedEvent()
  	{
  		std::cout << "Connected." << std::endl;
  	}
  
  	void connectionClosedEvent()
  	{
  		std::cout << "Connection closed." << std::endl;
  	}
  
  	void logEvent( const char* message, int level )
  	{
  		std::cout << message << std::endl;
  	}
  
  	void pingEvent( float timeMs )
  	{
  		//std::cout << "Ping: " << timeMs << "ms" << std::endl;
  	}
};
  
class MyBumper : public Bumper
{
    public:
  	MyBumper()
  		: bumped( false )
  	{
  	}
  
  	void bumperEvent( bool hasContact )
  	{
  		bumped |= hasContact;
  		std::cout << "Bumper has " << ( hasContact ? "contact" : "no contact") << std::endl;
  	}
  
  	bool bumped;
};
    
  MyCom com;
  MyBumper bumper;
  
void init( const std::string& hostname )
{
    // Initialize the actors
  
    // Connect
    std::cout << "Connecting... ";
    com.setAddress( hostname.c_str() );
  
    com.connectToServer( true );
  
    if( false == com.isConnected() )
    {
        std::cout << std::endl << "Could not connect to " << com.address() << std::endl;
  
  	rec::robotino::api2::shutdown();
  	exit( 1 );
    }
    
    else
    {
  	std::cout << "success" << std::endl
    }
}
  
void drive()
{
     
}
  
void destroy()
{
    com.disconnectFromServer();
}
  
int main( int argc, char **argv )
{
  
    std::string hostname = "172.26.1.1";
   
    if( argc > 1 )
    {
  	hostname = argv[1];
    }
  
    try
    {
  	init( hostname );
  	drive();
  	destroy();
    }
    
    catch( const rec::robotino::api2::RobotinoException& e )
    {
  	std::cerr << "Com Error: " << e.what() << std::endl;
    }
    
    catch( const std::exception& e )
    {
  	std::cerr << "Error: " << e.what() << std::endl;
    }
    
    catch( ... )
    {
  	std::cerr << "Unknow Error" << std::endl;
    }
  
    rec::robotino::api2::shutdown();
}
</code>

For this tutorial we need to declare a few more variable so just before the init() function declare the following

  CompactBHA cbha;
  float targetValues[6] = { 0.591398, 0.522972, 0.532747, 0.796676, 0.821114, 0.904203};

where the CompactBHA is the control class for the arm.

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.

<code c++>
    char x;

    while(true){
        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;
        
        if(x=='y'){     
            cbha.stringPots( targetValues );     
        }
        
        std::cout << "here is your pose." << std::endl;   // read out the values measured by the potentiometer to read the arm position.
        
        for( unsigned int i = 0; i < 6; ++i )
        {
            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
</code>

Now we are ready to implement out control.  This is done in the drive() function.  

The control works by reading the desired settings of the position measuring potentiometers and then uses a proportional control to achieve that state.  The values are then adjusted to that the average pressure in all 3 bellows is equal to 0.  

This control works for both sets of bellows.  i.e. the upper and lower set.

<code c++>
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 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 lastTime=com.msecsElapsed();  // remember the last time 
    float lowerAvg;  // the average pressure in the lower part of the arm
    float upperAvg;   // the average pressure in the upper part of the arm
  
    while( com.isConnected() && false == bumper.value() && _run )
    {
        // update the string pot values
        cbha.stringPots( values );
          
        // move toward target
          
        //  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[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;
          
        //  now we average up the pressures we calculated
        lowerAvg=(pressures[0]+pressures[1]+pressures[2])/3;
          
        //  subtract the average so the average value of the bellows does not grow over time.  
        pressures[0]-=lowerAvg;
        pressures[1]-=lowerAvg;
        pressures[2]-=lowerAvg;
              
        //  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[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;
   
        upperAvg=(pressures[3]+pressures[4]+pressures[5])/3;
          
        pressures[3]-=upperAvg;
        pressures[4]-=upperAvg;
        pressures[5]-=upperAvg;
          
        //  now we send our calculated pressures to the robotino api.
        cbha.setPressures( pressures );
  
          
          
        // list values
          
        std::cout << std::endl;
          
        std::cout << "avg: " << lowerAvg << " bar" << std::endl;
          
        std::cout << std::endl;
          
        for( unsigned int i = 0; i < 8; ++i )
        {
            std::cout << "B" << i << ": " << pressures[i] << " bar" << std::endl;
        }
          
        std::cout << std::endl;
          
        for( unsigned int i = 0; i < 6; ++i )
        {
            std::cout << "String pot " << i  << ": " << values[i] << std::endl;
        }
  
          
        // syncronize and delay
        lastTime=com.msecsElapsed();
        com.processEvents();
  	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 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.

  OmniDrive omniDrive;
  Odometry odometry;

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() function should look like this

<code c++ init.cpp>
void init( const std::string& hostname )
{
    // Initialize the actors
  
    // Connect
    std::cout << "Connecting... ";
    com.setAddress( hostname.c_str() );
  
    com.connectToServer( true );
  
    if( false == com.isConnected() )
    {
        std::cout << std::endl << "Could not connect to " << com.address() << std::endl;
  
  	rec::robotino::api2::shutdown();
  	exit( 1 );
    }
    
    else
    {
  	std::cout << "success" << std::endl;
          
        cbha.setCompressorsEnabled( true );
        cbha.setGripperValve1( false );
        cbha.setGripperValve2( false );
          
        odometry.set(0,0,0,false);
          
    }
}
</code>

Now lets look at the updated drive() function

<code c++ drive.cpp>
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 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 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 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 vel[3] = {0.0, 0.0, 0.0};  // the velocity see the rectangle tutorial for more info
    unsigned int * seq;
    double x;  // x position
    double y;   // y positon
    double phi;  // rotation 
      
    bool inTolerance=false;  // are we in acceptable tolerance for arm position
    bool closed=false;  // is the claw closed
      
    float stepSize=.5;
    float lastTime=com.msecsElapsed();
    float lowerAvg;
    float upperAvg;
  
    for( unsigned int i = 0; i < 6; ++i ){
        targetValues[i]=pickupPose[i];
    }
    
    while( com.isConnected() && false == bumper.value() && _run )
    {
        // update the string pot values
        cbha.stringPots( values );
          
        // 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[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;
          
        lowerAvg=(pressures[0]+pressures[1]+pressures[2])/3;
          
        pressures[0]-=lowerAvg;
        pressures[1]-=lowerAvg;
        pressures[2]-=lowerAvg;
              
        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[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;
          
        pressures[3]-=upperAvg;
        pressures[4]-=upperAvg;
        pressures[5]-=upperAvg;
          
        pressures[7]=10;
          
        cbha.setPressures( pressures );
          
        // determine if we are in tolerance
        for( unsigned int i = 0; i < 6; ++i ){
            errors[i]=targetValues[i]-values[i];
            std::cout<< errors[i] << std::endl;
            if(tolerance > std::abs(errors[i])   ){      
                std::cout << "in tolerance: " << i <<  std::endl;
            }
            
            else{
              break;
            }
              
            if(i==5){
                 inTolerance=true;
            }
            
            else inTolerance=false;
              
        }
          
          
        // movement
          
        odometry.readings( &x, &y, &phi );
          
          
        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
  
          
        // algorithm
  
  
        // this is the first step.  If we are in tolerance and at the start location start moving.
        if (inTolerance==true && x<=0 && !closed){
            vel[0]=.2;
        }
        // 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; 
            rec::robotino::api2::msleep( 2000 );  // wait 2 seconds 
            cbha.setGripperValve1( true );   // close gripper valves
            cbha.setGripperValve2( true );
            rec::robotino::api2::msleep( 1000 );  // wait 1 second
            closed=true;
            
            for( unsigned int i = 0; i < 6; ++i ){   // switch our pose to the cary pose
                  targetValues[i]=caryPose[i];  
              }
          }
          
          // now that the gripper is closed we want to start rotating.
          if (inTolerance==true && x<=0.51 && x>=.49 && closed){
              vel[2]=.5;
          }
          
          // once you have rotated 2.25 radians stop and wait 5 seconds.
          if (phi>=2.2&&phi<=2.3) {
              vel[2]=0;
              vel[0]=.2;
              rec::robotino::api2::msleep( 5000 );
              break;
          }
          
          
          
          // list values
          
          std::cout << std::endl;
          
          if (inTolerance){
           std::cout << " all in tolerance: "<< std::endl;
          }
          std::cout << std::endl;
          
          for( unsigned int i = 0; i < 8; ++i )
          {
              std::cout << "B" << i << ": " << pressures[i] << " bar" << std::endl;
          }
          
          std::cout << std::endl;
          
          for( unsigned int i = 0; i < 6; ++i )
          {
              std::cout << "String pot " << i  << ": " << values[i] << std::endl;
          }
  
          
          // syncronize and delay
          lastTime=com.msecsElapsed();
          com.processEvents();
  	  rec::robotino::api2::msleep( 100 );
    }
}
</code>

The completed file can be downloaded here {{dylanw:grab_n_run.zip}}