Robot shield for Arduino. Part 3 - Analysis of the firmware, functions that are common to all the robots

Part 1 - Hardware and Schematic

Part 2 - Robots Filippo, Bipe and Spider

Let us see now the analysis of the firmware describing those functions that are common to all the robots such as the procedure that reads the distance from obstacles by means of the sensor SRF05:

Code 1

long readDistance()
  {
     long duration, cm;
     pinMode(PING_PIN, OUTPUT);
     digitalWrite(PING_PIN, LOW);
     delayMicroseconds(2);
     digitalWrite(PING_PIN, HIGH);
     delayMicroseconds(5);
     digitalWrite(PING_PIN, LOW);
     pinMode(PING_PIN, INPUT);
     duration = pulseIn(PING_PIN, HIGH);        // ritorna distanza ostacolo in cm
     return duration / 29 / 2;
  }

The procedure foresees to send a pulse duration of 2usec to the sensor, which, proceeds generating a series of ultrasound to the outside. The same pin is subsequently used as input for reading, via the function PulseIn, the time taken by the sound to reach the obstacle, bounce and return to the sensor, the period during which the sensor maintains a high level of the line. The time taken by the ultrasound to go and return to the sensor is proportional to the distance of the object.

Here also the procedure for reading the voltage on the battery:

Code 2

void levelBat()
  {
       int VbatRAW = analogRead(A0);
       float Vbat = float(VbatRAW)/50;
       Serial.print("Vbat= ");
       Serial.println(Vbat, DEC);
           if (Vbat<6.0)
          {
      Serial.println("Livello batteria basso!");
      inOff();
           }
}

The procedure is simple, plan to read the analog channel A0 of Arduino and save the result into a variable VbatRAW. As explained above, by dividing this value by 50, is obtained by the voltage in volts of the battery. Since excessively discharge the battery can damage it, we expected an alarm to the level below which the robot is immediately stopped and all the servos are disabled. The threshold value is defined in 6.0 V ideal for use with a battery pack consisting of two cell LiPo or six if you use NiMh or NiCd cells, eight cells used instead if you can increase this value up to 8.0 volts or, one volt per used cell.

Another common procedure is called to all the robots initServo () to initialize the servos:

Code 3

void initServo()
   {
    // posizione iniziale dei servi
       servo0.attach(2);  // connette servo
       delay(300);
    ........
       servo7.attach(9);  // connette servo
       delay(300);
   }

The function Servo.attach() associates each object provides the respective servo output, from this moment the servant reaches the PWM signal.

The procedure inOff () performs the opposite function by removing the pin from their association with the servos and leaving them with no control.

Code 4

void inOff()
 {
   leg[0] = 90;
   .....
   leg[7] = 90;
   setServo();
   servo0.detach();  // connette servo
   ....
   servo7.detach();  // connette servo
   RobotMode = MODE_OFF;
 }

The function to put in sleep mode the robot is called Standby() and simply place all the servos in the central position. In the case of the robot Filippo and BIPE the servos are positioned in their respective neutral points whose values are contained in the vector named leg_ntr [].

Code 5

void inStandby()
 {
   leg[0] = 90;
   ....
   leg[7] = 90;
   setServo();
   RobotMode = MODE_STANDBY;
   Serial.println("Sono in Standby...");
}

A very important procedure is to ensure that the servos have to place to desired coordinates:

Code 6

void setServo()
 {
     for (i=0; i<18; i++)
        {
        servo0.write(leg_old[0] + i*(leg[0]-leg_old[0])/18);
        servo1.write(leg_old[1] + i*(leg[1]-leg_old[1])/18);
        servo2.write(leg_old[2] + i*(leg[2]-leg_old[2])/18);
        servo3.write(leg_old[3] + i*(leg[3]-leg_old[3])/18);
        servo4.write(leg_old[4] + i*(leg[4]-leg_old[4])/18);
        servo5.write(leg_old[5] + i*(leg[5]-leg_old[5])/18);
        servo6.write(leg_old[6] + i*(leg[6]-leg_old[6])/18);
        servo7.write(leg_old[7] + i*(leg[7]-leg_old[7])/18);
        delay(TimeOneStep/4/18);
        }
     leg_old[0]=leg[0];
     leg_old[1]=leg[1];
     leg_old[2]=leg[2];
     leg_old[3]=leg[3];
     leg_old[4]=leg[4];
     leg_old[5]=leg[5];
     leg_old[6]=leg[6];
     leg_old[7]=leg[7];
}

If we set directly the servo, it was a sudden movement and the robot would move in spurts. He prefers, instead, gradually controlling the servo to bring it to the desired position in a defined time. To do this you must define a vector leg[] that contains the coordinates to be achieved by each servo and a vector leg_old[] that contains the actual coordinates. The procedure controls the servos by sending them all coordinated by the intermediate value at the target value in a time defined by the variable TimeOneStep. This system allows us to obtain fluid movements.

Only the robot spider there is a procedure called setServoWalk() because it is necessary that some servos are moving at different speeds than the other. During the walk the three legs in contact with the floor moving slowly, the fourth foot must lift and move fast in the opposite direction to progress.

Demonstration video

Downloads

Source Code for robots Filippo, BIPE, SPIDER - download

open-electronics.org