The Brachiation Robot

Source: http://www.gibbons.de/main/introduction/pics/5-1.gif

An important aspect of our project was the programming of our processor in order to have a functional robot. In this page, all the aspects of the software development will be discussed and subdivided in five parts: the potentiometer, the accelerometer, the servomotors, the final motion code, and the theoretically working but untested code using the accelerometer.

Methodology

The first obstacle we encountered was the uploading of a program to our processor. An Arduino board was used, but the detailed explanations (given on their website) as to how to use it to upload a program on to an external processor gave no result at all. After many hours of unsuccessful tests, another approach was used: the processor of the Arduino was removed, and replaced by ours. Every time we wanted to modify the program, the processor had to be removed from our PCB, placed on the Arduino for the uploading, and then placed back on our PCB. This method was not very convenient but was the only one that worked.

Our methodology was to first develop programs for each part, that will be detailed below, and then to assemble the different parts. These programs were developed using the Arduino Language (available here) and open source programs available on the internet.

Back.to.top

Potentiometer

The easiest code that we had to write was the one for the potentiometer, since in the examples code given by Arduino there is an example of the control of a servomotor with a potentiometer.

Since this code is really simple and was not used ultimately, as said in the design page, we will not describe it.

Back.to.top

Accelerometer

The communication between our board and the accelerometer was set using a serial port interface (SPI). First, we had to understand how to enable the SPI communication. Fortunately for us, Arduino provides an example illustrating the use of this type of connection.

Once the communication was established, we had to transfer information. To do so, we had to look at the datasheet of our accelerometer (available here). The general principle is the following: a 16 bits word is sent to the accelerometer, containing all the information about the requested data (detailed in comments in the code). Then, the accelerometer sends back a 16 bits word, containing the desired information.

Accelerometer Code

	
	#include "SPI.h" // necessary library
	#include "Servo.h"
	Servo Gripper1; 
	Servo Gripper2; 
        
        word xconf='0011000000000100'; // this word will say to the accelerometer : it's a x acceleration 
                           	       // data request without offest cancelation, we want signed 
                          	       // data and the ARM function is disabled 
        word yconf='0111000000000100';
        word getdatax='0000010000000000'; // this word will allow us to extract the data obtained 
                          		  // by the accelerometer
        word getdatay='0010010000000000';
        word xcmd='0000001000010010';    // if the position along the x axis is near the reaching position
        word ycmd='0000001000010010';
        word SPIerror='0000110000000000';
        word accelx;
        word accely;
        word xresponse;
        word yresponse;
		

        void setup() 
        { 
          pinMode(SS, OUTPUT);       // we use this for SS pin
          SPI.begin();        	     // wake up the SPI bus
          SPI.setBitOrder(MSBFIRST); // Our MMA68xx requires data to be sent MSB (most significant 
                         	     // byte) first. See data sheet page 5
        } 

        void getXAccel() // get the response to the command xconf
        {
          digitalWrite(SS, LOW);
          word xresponse = SPI.transfer(xconf); //send the command and receive the data
          digitalWrite(SS, HIGH);
        }
        
        void getYAccel() // get the response to the command yconf
        {
          digitalWrite(SS, LOW);
          word yresponse = SPI.transfer(yconf); 
          digitalWrite(SS, HIGH);
        }

        void loop() 
        { 
            while(accelx > xcmd && accely > ycmd) 
            // untill we reach the position for the gripper to close, we stay in this loop
            {
              getXAccel();
              getYAccel();
                        
              if((xresponse & SPIerror)!= '00000110000000000' || (yresponse & SPIerror)!= '00000110000000000') 
              // if there are no errors compute the acceleration values
              {
                accelx = xresponse ^ getdatax; // extract in a 16 bits word only the data in x
                accely = yresponse ^ getdatay;
              }
              delay(1); // in order to be sure that the accelerometer has the time to compute the data requests
            }
        }

Back.to.top

Servomotors

This is the key code of our robot. Indeed, if we are able to control the servomotors accurately and with the right timing, then the desired movement will be achieved.

Two ways were available to control our servomotors: either by producing a PWM "by ourselves", or using the Servo library given by Arduino. The latter was chosen.

The first thing to do is to know the range of action of our servomotors. To do so, we checked their datasheets (available here and here), and sweep tests were conducted.

An encountered problem, that made us lose a tremendous amount of time, is that if a position out of the servomotor's range is imposed, the servomotor does not respond at all, instead of going to the nearest position as the requested one. For instance, if the servo motor is at position 50, the maximum position of the servo is 160, and it is asked to go to 180, it will not go to 160, but will not move at all. For this reason, and since the extreme values are not exactly the one given in the datasheet, we wasted much time, thinking there was a problem in the connections or in the PCB.

Ultimately, after number of tests, we established that the range of the small servomotors was from 20 to 160, and the range of the large servo motor from 25 to 160.

In order to obtain reproducible results, we had to make an initialisation program bringing the servomotors to the desired initial position. As you can see on the program given below, we place the gripper's servo at 90, which is the middle range value, and the central servomotor to 25 which is his minimal value. Those values are going to be explained a little farther below.

Initial Position Code

Once the servomotors are at the right initial position, the next step is to make the robot move. The corresponding code is given below.

How does it work?

First, we close the two grippers and in order to counter the gravity force, we need a high torque so we ask the servomotor to reach the 80 degree position (keep in mind that in our initial close configuration the angle value is 90). As the gripper is already closed when the servo is in position 90, it will try to close it further to go to 80, and as a result, the gripper will be tightly closed.
Then, after 3.5 seconds (this value was arbitrarily chosen) the gripper on the left will open, then the central motor will fully rotate and finally, at the right time (determined experimentally), the opened gripper will close. The code is then repeated for the second movement, but this time, it is the other gripper that has to open and close.
In order to cross the ladder, a counter is used and once the end of the ladder is reached, we place an infinite loop, in which the two grippers remain closed.

As you can see we place a time variable called "time", which allows us to use the appropriate delay between two actions.

Movement Code

		
	#include "Servo.h"
	
	Servo Gripper1; 
	Servo Central;
	Servo Gripper2; 
			 
	int time = 0; // this variable is used to introduce the necessary delays between two actions.
	
			
	void setup() // the servomotors are associated to the corresponding pins
	{ 
	  Gripper1.attach(2); 
	  Central.attach(3);
	  Gripper2.attach(5);
	} 
			
			 
	void loop() 
	{	  
	  while(millis() < time+2500) // a delay of 2.5 second is introduced before the first opening of the 
				      // gripper in order to have time to place the robot.
	  { 
	    Gripper1.write(80);
	    Gripper2.write(80);
	    Central.write(25);
	  } 
	  
	  Gripper1.write(140); // after one second, gripper1 is opened
	  time=millis();
	  
	  // we wait 150ms, during which gripper2 has to remain closed. these 150ms are necessary
	  // because if the central servo is activated too early, 
	  // the gripper collides with the bar and the movement is hindered.
	  while(millis() < time+150)
	  {
	    Gripper2.write(80);
	  }
		  
	  Central.write(160); //after 150 ms, we activate the central servo and send it to its maximal position
	  delay(15);
	  time=millis();
	
	  while(millis() < time+500) // during the next 500 ms (determined experimentally), the only 
	  			     // action to perform is to maintain gripper2 closed.
	  {
	    Gripper2.write(80);
	  }
	  
	  Gripper1.write(80); // after 500ms, gripper1 is closed (and gripper2 is maintained closed)
	  Gripper2.write(80);
	  
	  
	  time=millis();
	  
	  // after grabbing a bar, we wait one second, then perform the same motion. 
	  // the only difference is that it is now gripper2 that opens and gripper1 that remains closed.
	  while(millis()< time+1000)
	  { 
	    Gripper1.write(80);
	    Gripper2.write(80);
	    Central.write(160);
	  }
	 
	  Gripper2.write(140);
	  time=millis();
	  
	  while(millis() < time+150)
	  {
	    Gripper1.write(80);
	  }
	  
	  Central.write(25);
	  delay(15);
	  time=millis();
	  
	  while(millis() < time+500)
	  {
	    Gripper1.write(80);
	  }
	  
	  while(1) //stops after 2 bars.
	  {
	    Gripper2.write(80);
	    Gripper1.write(80);
	  }  
	}

Back.to.top

Overall Code

In the global code given below, the use of the accelerometer was implemented. Theoretically, if we find the right value for the x and y references (xcmd and ycmd), this code should work, but due to lack of time, we were not able to test it.

You can see in this code that we have a safety measure. Indeed if there is an error with the SPI transfer or the data given by the accelerometer, if the timing is reached then the gripper will close, this allows the robot to cross the ladder even with an accelerometer problem.

Overall Code

		
	#include "Servo.h"
	#include "SPI.h" // necessary library
	
	Servo Gripper1; 
        Servo Central;
        Servo Gripper2; 
        
        word xconf='0011000000000100'; // this word will say to the accelerometer : it's a x acceleration 
                           	       // data request without offest cancelation, we want signed 
                          	       // data and the ARM function is disabled 
        word yconf='0111000000000100';
        word getdatax='0000010000000000'; // this word will allow us to extract the data obtained 
                          		  // by the accelerometer
        word getdatay='0010010000000000';
        word xcmd='0000001000010010';    // if the position along the x axis is near the reaching position
        word ycmd='0000001000010010';
        word SPIerror='0000110000000000';
        word accelx;
        word accely;
        word xresponse;
        word yresponse;
        
        int time = 0;
        int bar = 0;  // this variable represents the bar that has just been grabbed.

        void setup() 
        { 
          pinMode(8,INPUT);
          Gripper1.attach(2); // attaches the servo on pin 2 to the servo object 
          Central.attach(3);
          Gripper2.attach(5); 
          pinMode(SS, OUTPUT);       // we use this for SS pin
          SPI.begin();        	     // wake up the SPI bus
          SPI.setBitOrder(MSBFIRST); // Our MMA68xx requires data to be sent MSB (most significant 
                         	     // byte) first. See data sheet page 5
        } 

        void getXAccel() // get the response to the command xconf
        {
          digitalWrite(SS, LOW);
          word xresponse = SPI.transfer(xconf);
          digitalWrite(SS, HIGH);
        }
        
        void getYAccel() // get the response to the command yconf
        {
          digitalWrite(SS, LOW);
          word yresponse = SPI.transfer(yconf);
          digitalWrite(SS, HIGH);
        }

        void loop() 
        { 
          if(digitalRead(8)==LOW) // if start is "off" then we initialize every position
          {
            Gripper1.write(90);
            delay(15);
            Central.write(25);
            delay(15);
            Gripper2.write(90);
            delay(15);
          }
          else // we start the displacement
          {
            if(bar==0) // at the beginning, a larger delay (2.5 seconds) is introduced, so we have the time 
                       // to position the robot and let it go.
            {
              while(millis()< 2500)
              { 
                // the two grippers are closed, and the central servo is set to its minimal position
                Gripper1.write(80);
                Gripper2.write(80);
                Central.write(25);
              } 
            }
            time=millis(); // millis() send the number of ms elapsed since the beginning of the program
                  
            while(millis() < time+1000)
            { 
              Gripper1.write(80);
              Gripper2.write(80);
              Central.write(25);
            } 
            
            Gripper1.write(140);
            time=millis();
                  
            while(millis() < time+150)
            {
              Gripper2.write(80);
            }
            
            Central.write(160);
            delay(15);
            time=millis();
                  
            while((millis() < time+500) || (accelx > xcmd && accely > ycmd)) 
            // untill we reach the position for the gripper to close, we stay in this loop, if there 
            // is a problem with the accelerometer, we still have a security to close the gripper with 
            // the timing
            {
              Gripper2.write(80);
              getXAccel();
              getYAccel();
                        
              if((xresponse & SPIerror)!= '00000110000000000' || (yresponse & SPIerror)!= '00000110000000000') 
              // if there are no errors compute the acceleration values
              {
                accelx = xresponse ^ getdatax; // extract in a 16 bits word only the data in x
                accely = yresponse ^ getdatay;
              }
              delay(1); // in order to be sure that the accelerometer has the time to compute the data requests
            }
            
            Gripper1.write(80);
            Gripper2.write(80);
            bar++; // and a bar has just been caught.
                  
            if(bar==5) // if the end of the ladder (5bars) is reached, the two grippers remain closed until 
	  	       // the power is shut down.
            {
              while(1)
              {
                Gripper1.write(80);
                Gripper2.write(80);
              } 
            }
            time=millis();
                  
            while(millis() < time+2500)
            { 
              Gripper1.write(80);
              Gripper2.write(80);
              Central.write(160);
            }
            
            Gripper1.write(80);
            Gripper2.write(140);
            time=millis();
                  
            while(millis() < time+150)
            {
              Gripper1.write(80);
            }
            
            Central.write(25);
            delay(15);
            time=millis();
          
            while((millis() < time+500) || (accelx > xcmd && accely > ycmd))
            {
              Gripper2.write(80);
              getXAccel();
              getYAccel();
            
              if((xresponse & SPIerror)!= '00000110000000000' || (yresponse & SPIerror)!= '00000110000000000')
              {
                accelx=xresponse ^ getdatax;
                accely=yresponse ^ getdatay;
                delay(1);
              }    
            }
            bar++;
          
            if(bar==5) // if the end of the ladder (5bars) is reached, the two grippers remain closed until 
                       // the power is shut down.
            {
              while(1)
              {
                Gripper1.write(80);
    	        Gripper2.write(80);
              } 
            }
          }
        }

Back.to.top