Register | Sign in

Home > Touch Control System (TCS) > TCS Tutorial: Animated Humanoid Robot Head


TCS Tutorial: Animated Humanoid Robot Head

Touch Control System is a great solution for robot brains, but first we need some robots to work with.


You can download and 3D print the parts here:
http://www.thingiverse.com/thing:1037311




This animated humanoid robot head uses 5 servos:

2 micro servos for horizontal and vertical eye movement, 1 micro servo for the mouth, and 2 standard servos for horizontal and vertical neck movement.



Electronics

Arduino Pro Mini 3.3v (3 Pack on Amazon, only need one)
Adafruit 16-Channel 12-bit PWM/Servo Driver
Voltage Sensor
7.4V LiPo Battery
WiFi module
Voltage Regulator 1
Voltage Regulator 2
FTDI Breakout for programming the Arduino
3 Micro Servos
2 Standard Servos



Printing and Assembly

We recommend printing all parts with 50% or more fill. For the enclosure, neck, and internal head pieces, we used UV reactive plastic (orange and blue) with a couple UV LEDs to make it glow. Clear plastic was used for the eyes and outside parts so you can see the internals glow through. There are two full-color RGB LEDs in the eyes that are controlled using the same PWM driver board used for the servos.

You should be able to use 4-40 screws or 1.75mm plastic to attach most parts. Many of the holes will need to be drilled out for the screws to fit. Some parts will require larger screws. Most parts will need to be sanded down to fit properly

The little moving white gear pieces that come with the servos are attached to the model using paperclips or with metal from sewing needles.

It is recommended that you get the servos moving and calibrated BEFORE attaching them to the parts they move. Every servo is different and you’ll need to adjust the min and max values in the Arduino code for each servo. Using min/max values that exceed the servo’s range can damage the servo and the parts attached to it.

The neck uses two bearings: http://www.amazon.com/gp/product/B002BBGTK6



Driving RGB LEDs with the PWM driver

We used Anode RGB LEDs for the eyes. You can also use Cathode RGB LEDs and we’ll show you the difference between the two.

Adafruit provides a library for interfacing with their PWM driver.

With an Anode RGB LED, attach the common pin to +5V.

With a Cathode RGB LED, attach the common pin to Ground.

Using the Adafruit library, you can set a PWM frequency on a channel like so:

// this is for Anode RGB LEDs
pwm.setPWM(1, 0, 4095); // full brightness
pwm.setPWM(1, 4095, 4095); // minimum brightness

Cathode RGB LEDs are reversed:

// this is for Cathode RGB LEDs
pwm.setPWM(1, 4095, 4095); // full brightness
pwm.setPWM(1, 0, 4095); // minimum brightness

You can then convert 0-255 RGB values to the 0-4095 range using the Arduino Map function. Keeping the 0-4095 value above 0 worked well for us, we set the minimum to 5. The map function takes in the value you want to convert, the source range, the destination range, and outputs a new adjusted value:

//RGB 0-255 values
int redPin = 1;
int red = 100;
//Anode:
pwm.setPWM(redPin, map(red, 0, 255, 5, 4095), 4095);
//Cathode:
pwm.setPWM(redPin, map(red, 255, 0, 5, 4095), 4095);

Here is a function we are using to set the eye colors:

// The eye RGB LEDs are attached to the PWM driver on pins 5, 6, 7, and 8, 9, 10.
// Accepts 0-255 RGB values
void setEyeColorRGB(int r, int g, int b){
  pwm.setPWM(5, map(r, 0, 255, 5, 4095), 4095);
  pwm.setPWM(6, map(g, 0, 255, 5, 4095), 4095);
  pwm.setPWM(7, map(b, 0, 255, 5, 4095), 4095); 
  
  pwm.setPWM(8, map(r, 0, 255, 5, 4095), 4095);
  pwm.setPWM(9, map(g, 0, 255, 5, 4095), 4095);
  pwm.setPWM(10, map(b, 0, 255, 5, 4095), 4095);  
}


Servo Calibration

At the beginning of our Arduino sketch, we have min/max values for each servo. The default range used in the Adafruit documentation is 150-600. You’ll want to start with that range and adjust accordingly for each servo.

Check out the Adafruit PWM Driver documentation here: https://learn.adafruit.com/16-channel-pwm-servo-driver/using-the-adafruit-library

Here are the values we ended up with:

// neck horizontal
#define SERVOMIN_NH  200 // this is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX_NH  584 // this is the 'maximum' pulse length count (out of 4096)
#define SERVOPIN_NH  0
// neck vertical
#define SERVOMIN_NV  230 // this is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX_NV  520 // this is the 'maximum' pulse length count (out of 4096)
#define SERVOPIN_NV  1
// eyes vertical
#define SERVOMIN_EV  150 // this is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX_EV  430 // this is the 'maximum' pulse length count (out of 4096)
#define SERVOPIN_EV  2
// eyes horizontal
#define SERVOMIN_EH  160 // this is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX_EH  320 // this is the 'maximum' pulse length count (out of 4096)
#define SERVOPIN_EH  3
// mouth
#define SERVOMIN_M  160 // this is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX_M  300 // this is the 'maximum' pulse length count (out of 4096)
#define SERVOPIN_M  4

You can then set a servo to min, max, or figure out where the middle of the range would be:

pwm.setPWM(SERVOPIN_M, 0, SERVOMIN_M); //min
pwm.setPWM(SERVOPIN_M, 0, SERVOMAX_M); //max
int middleVal = SERVOMIN_M + ((SERVOMAX_M - SERVOMIN_M) / 2);
pwm.setPWM(SERVOPIN_M, 0, middleVal); // middle

Using a percentage:

int percentVal = 50; // 50%
// map converts 0-100 range to min-max range
pwm.setPWM(SERVOPIN_M, 0, map(percentVal, 0, 100, SERVOMIN_M, SERVOMAX_M));


Voltage Sensor

We are using a 7.4 LiPo battery to power everything and we need to make sure the battery doesn’t get too low. A 7.4V battery should stay above 6V under load to prevent damage.

This function will report the battery level:

double battery_level(){
  int sensorValue = analogRead(A0);
  // map the analog values to min/max battery voltage range (6v to 8.4v)
  // with these numbers you can go a bit outside of the 6-8.4v range and it will still be fairly accurate.
  double mapped_volt = map(sensorValue, 366, 511, 600, 840);
  double dvolt = mapped_volt / 100;
  return dvolt;
}

Example usage:

void loop(){
  // If battery is below 7.0V then go into battery low mode
  while (battery_level() < 7.0){
    battery_low();
  }
  // the rest of your loop code here...
}
void battery_low(){
  // turn off eye LEDs
  setEyeColorRGB(0,0,0);
  // we have UV LEDs attached to channels 11 and 12 on the PWM driver
  // toggle UV LEDs
  pwm.setPWM(11, 4095, 4095);
  pwm.setPWM(12, 4095, 4095);
  // report to serial console
  Serial.println("Battery too low (< 7V):");
  Serial.println(battery_level());
  delay(1000);
  // toggle UV LEDs
  pwm.setPWM(11, 0, 4095);
  pwm.setPWM(12, 0, 4095);
  delay(1000);
}


Dual Voltage Regulators

We are using two voltage regulators, one to supply 5v to the Arduino board and PWM driver, and another to supply 3.3v to the WiFi module.

The WiFi module (esp8266), while only 3.3v, requires more power than the Arduino’s regulator can provide. This means we need to supply 3.3v separately. We went with a massive 10A regulator to ensure the WiFi chip gets all the power it needs and to provide lots of room for additional 3.3v sensors.

This document was extremely helpful in getting the WiFi module to work: http://rancidbacon.com/files/kiwicon8/ESP8266_WiFi_Module_Quick_Start_Guide_v_1.0.4.pdf



Complete Arduino Code

Below is the code we are using so far to animate the head. We are still working on it, so check back for updates!

#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#include <SoftwareSerial.h>
#include <stdlib.h>
// connect 9 to TX of Serial USB
// connect 8 to RX of serial USB
SoftwareSerial ser(8, 9); // RX, TX
// called this way, it uses the default address 0x40
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
// you can also call it with a different address you want
//Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x41);
// Depending on your servo make, the pulse width min and max may vary, you 
// want these to be as small/large as possible without hitting the hard stop
// for max range. You'll have to tweak them as necessary to match the servos you
// have!
//150 - 600
#define SERVOMIN_NH  200 // this is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX_NH  584 // this is the 'maximum' pulse length count (out of 4096)
#define SERVOPIN_NH  0
#define SERVOMIN_NV  230 // this is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX_NV  520 // this is the 'maximum' pulse length count (out of 4096)
#define SERVOPIN_NV  1
#define SERVOMIN_EV  150 // this is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX_EV  430 // this is the 'maximum' pulse length count (out of 4096)
#define SERVOPIN_EV  2
#define SERVOMIN_EH  160 // this is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX_EH  320 // this is the 'maximum' pulse length count (out of 4096)
#define SERVOPIN_EH  3
#define SERVOMIN_M  160 // this is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX_M  300 // this is the 'maximum' pulse length count (out of 4096)
#define SERVOPIN_M  4
// Color arrays
int black[3]  = { 0, 0, 0 };
int white[3]  = { 100, 100, 100 };
int red[3]    = { 100, 0, 0 };
int green[3]  = { 0, 100, 0 };
int blue[3]   = { 0, 0, 100 };
int yellow[3] = { 40, 95, 0 };
int dimWhite[3] = { 30, 30, 30 };
// etc.
// Set initial color
int redVal = black[0];
int grnVal = black[1]; 
int bluVal = black[2];
int wait = 0;      // 10ms internal crossFade delay; increase for slower fades
int hold = 0;       // Optional hold when a color is complete, before the next crossFade
int DEBUG = 1;      // DEBUG counter; if set to 1, will write values back via serial
int loopCount = 60; // How often should DEBUG report?
int repeat = 0;     // How many times should we loop before stopping? (0 for no stop)
int j = 0;          // Loop counter for repeat
// Initialize color variables
int prevR = redVal;
int prevG = grnVal;
int prevB = bluVal;
int rdir = 0;
int r = 1;
int g = 1;
int b = 1;
int clr = 0;
int analogInput = 0;
float vout = 0.0;
float vin = 0.0;
float R1 = 30000.0; // resistance of R1 (100K) 
float R2 = 7500.0; // resistance of R2 (10K)
int value = 0;
void setup() {
  Serial.begin(9600);
  Serial.println("Robot Head Starting Up...");
  pwm.begin();
  
  pwm.setPWMFreq(60);  // Analog servos run at ~60 Hz updates
  pinMode(analogInput, INPUT);
  while (battery_level() < 7.0){
    battery_low();
  }
  pwm.setPWM(11, 0, 4095);
  pwm.setPWM(12, 0, 4095);
  pwm.setPWM(SERVOPIN_M, 0, SERVOMIN_M);
  setEyeColorRGB(0,0,0);
  // enable software serial
  ser.begin(9600);
  
  connectWiFi();
  // fade the UV LEDs on
  for (int p = 0; p < 4095; p++) {
    pwm.setPWM(11, p, 4095);
    pwm.setPWM(12, p, 4095);
    delay(1);
  }
  Serial.println("Robot Head Started.");
}
void battery_low(){
  setEyeColorRGB(0,0,0);
  pwm.setPWM(11, 4095, 4095);
  pwm.setPWM(12, 4095, 4095);
  Serial.println("Battery too low (<7V):");
  Serial.println(battery_level());
  delay(1000);
  pwm.setPWM(11, 0, 4095);
  pwm.setPWM(12, 0, 4095);
  delay(1000);
}
void loop() {
  while (battery_level() < 7.0){
    battery_low();
  }
  pwm.setPWM(11, 4095, 4095);
  pwm.setPWM(12, 4095, 4095);
  Serial.println("Battery Level:");
  Serial.println(battery_level());
  while (ser.available())
  {
    char c = ser.read();
    Serial.write(c);
    if (c == '\r') Serial.print('\n');
  }
  delay(2000);
  crossFade(red);
  crossFade(green);
  crossFade(blue);
  crossFade(yellow);
  motor_animation();
}
int calculateStep(int prevValue, int endValue) {
  int step = endValue - prevValue; // What's the overall gap?
  if (step) {                      // If its non-zero, 
    step = 1020/step;              //   divide by 1020
  } 
  return step;
}
/* The next function is calculateVal. When the loop value, i,
*  reaches the step size appropriate for one of the
*  colors, it increases or decreases the value of that color by 1. 
*  (R, G, and B are each calculated separately.)
*/
int calculateVal(int step, int val, int i) {
  if ((step) && i % step == 0) { // If step is non-zero and its time to change a value,
    if (step > 0) {              //   increment the value if step is positive...
      val += 1;           
    } 
    else if (step < 0) {         //   ...or decrement it if step is negative
      val -= 1;
    } 
  }
  // Defensive driving: make sure val stays in the range 0-255
  if (val > 255) {
    val = 255;
  } 
  else if (val < 0) {
    val = 0;
  }
  return val;
}
/* crossFade() converts the percentage colors to a 
*  0-255 range, then loops 1020 times, checking to see if  
*  the value needs to be updated each time, then writing
*  the color values to the correct pins.
*/
void crossFade(int color[3]) {
  // Convert to 0-255
  int R = (color[0] * 255) / 100;
  int G = (color[1] * 255) / 100;
  int B = (color[2] * 255) / 100;
  int stepR = calculateStep(prevR, R);
  int stepG = calculateStep(prevG, G); 
  int stepB = calculateStep(prevB, B);
  for (int i = 0; i <= 1020; i++) {
    redVal = calculateVal(stepR, redVal, i);
    grnVal = calculateVal(stepG, grnVal, i);
    bluVal = calculateVal(stepB, bluVal, i);
    setEyeColorRGB(redVal, grnVal, bluVal);
    delay(wait); // Pause for 'wait' milliseconds before resuming the loop
    }
  
  // Update current values for next loop
  prevR = redVal; 
  prevG = grnVal; 
  prevB = bluVal;
  delay(hold); // Pause for optional 'wait' milliseconds before resuming the loop
}
void setEyeColorRGB(int r, int g, int b){
  pwm.setPWM(5, map(r, 0, 255, 5, 4095), 4095);
  pwm.setPWM(6, map(g, 0, 255, 5, 4095), 4095);
  pwm.setPWM(7, map(b, 0, 255, 5, 4095), 4095);
  
  pwm.setPWM(8, map(r, 0, 255, 5, 4095), 4095);
  pwm.setPWM(9, map(g, 0, 255, 5, 4095), 4095);
  pwm.setPWM(10, map(b, 0, 255, 5, 4095), 4095);
  
}
// used for WiFi module
void readSerial(String cmd){
  ser.println(cmd);
  Serial.print("Sent: ");
  Serial.println(cmd);
  
  while (ser.available())
  {
    char c = ser.read();
    Serial.write(c);
    if (c == '\r') Serial.print('\n');
  }
   delay(2000);
}
boolean connectWiFi()
{
  // reset ESP8266
  readSerial("AT+RST");
  readSerial("AT+CWMODE=3");
  readSerial("AT+CWJAP=\"SSID\",\"PASSWORD\"");
  delay(10000);
  readSerial("AT+CIFSR");
  readSerial("AT+CIPMUX=1");
  // start server on port 1336
  readSerial("AT+CIPSERVER=1,1336");
}
void motor_animation(){
  setEyeColorRGB(0,0,255);
  motion_range(SERVOPIN_NH, SERVOMIN_NH, SERVOMAX_NH);
  motion_range(SERVOPIN_NV, SERVOMIN_NV, SERVOMAX_NV);
  motion_range(SERVOPIN_EH, SERVOMIN_EH, SERVOMAX_EH);
  motion_range(SERVOPIN_EV, SERVOMIN_EV, SERVOMAX_EV);
  motion_range(SERVOPIN_M, SERVOMIN_M, SERVOMAX_M);
  setEyeColorRGB(0,255, 0);
  // set last color for crossfade function
  prevR = 0; 
  prevG = 255; 
  prevB = 0;
  motion(SERVOPIN_NH, SERVOMIN_NH, middle(SERVOMIN_NH, SERVOMAX_NH));
  motion(SERVOPIN_NV, SERVOMIN_NV, middle(SERVOMIN_NV, SERVOMAX_NV));
  motion(SERVOPIN_EH, SERVOMIN_EH, middle(SERVOMIN_EH, SERVOMAX_EH));
  motion(SERVOPIN_EV, SERVOMIN_EV, middle(SERVOMIN_EV, SERVOMAX_EV));
  delay(10000);
  crossFade(red);
  pwm.setPWM(SERVOPIN_M, 0, middle(SERVOMIN_M, SERVOMAX_M));
  delay(150);
  pwm.setPWM(SERVOPIN_M, 0, SERVOMIN_M);
  delay(150);
  pwm.setPWM(SERVOPIN_M, 0, middle(SERVOMIN_M, SERVOMAX_M));
  delay(150);
  pwm.setPWM(SERVOPIN_M, 0, SERVOMIN_M);
  delay(150);
  pwm.setPWM(SERVOPIN_M, 0, middle(SERVOMIN_M, SERVOMAX_M));
  delay(150);
  pwm.setPWM(SERVOPIN_M, 0, SERVOMIN_M);
  delay(150);
  delay(10000);
  setEyeColorRGB(255,0,255);
  delay(1000);
  motion(SERVOPIN_NH, middle(SERVOMIN_NH, SERVOMAX_NH), SERVOMIN_NH);
  motion(SERVOPIN_NV, middle(SERVOMIN_NV, SERVOMAX_NV), SERVOMIN_NV);
  motion(SERVOPIN_EH, middle(SERVOMIN_EH, SERVOMAX_EH), SERVOMIN_EH);
  motion(SERVOPIN_EV, middle(SERVOMIN_EV, SERVOMAX_EV), SERVOMIN_EV);
  delay(1000);
  pwm.setPWM(SERVOPIN_NH, 0, middle(SERVOMIN_NH, SERVOMAX_NH));
  pwm.setPWM(SERVOPIN_NV, 0, middle(SERVOMIN_NV, SERVOMAX_NV));
  pwm.setPWM(SERVOPIN_EH, 0, middle(SERVOMIN_EH, SERVOMAX_EH));
  pwm.setPWM(SERVOPIN_EV, 0, middle(SERVOMIN_EV, SERVOMAX_EV));
  pwm.setPWM(SERVOPIN_M, 0, SERVOMIN_M);
}
// you can use this function if you'd like to set the pulse length in seconds
// e.g. setServoPulse(0, 0.001) is a ~1 millisecond pulse width. its not precise!
void setServoPulse(uint8_t n, double pulse) {
  double pulselength;
  
  pulselength = 1000000;   // 1,000,000 us per second
  pulselength /= 60;   // 60 Hz
  Serial.print(pulselength); Serial.println(" us per period"); 
  pulselength /= 4096;  // 12 bits of resolution
  Serial.print(pulselength); Serial.println(" us per bit"); 
  pulse *= 1000;
  pulse /= pulselength;
  Serial.println(pulse);
  pwm.setPWM(n, 0, pulse);
  delay(15);
  
}
double battery_level(){
  int sensorValue = analogRead(A0);
  double mapped_volt = map(sensorValue, 366, 511, 600, 840);
  double dvolt = mapped_volt / 100;
  return dvolt;
}
void motion_range(uint8_t n, uint16_t mn, uint16_t mx){
  for (uint16_t pulselen = mn; pulselen < mx; pulselen++) {
    pwm.setPWM(n, 0, pulselen);
    delay(2);
  }
  for (uint16_t pulselen = mx; pulselen > mn; pulselen--) {
    pwm.setPWM(n, 0, pulselen);
    delay(2);
  }
}
uint16_t middle (uint16_t mn, uint16_t mx){
  return mn + ((mx - mn) / 2);
}
void motion(uint8_t n, uint16_t mn, uint16_t mx){
  if (mn < mx){
    for (uint16_t pulselen = mn; pulselen < mx; pulselen++) {
      pwm.setPWM(n, 0, pulselen);
      delay(2);
    }
  }else{
    for (uint16_t pulselen = mn; pulselen > mx; pulselen--) {
      pwm.setPWM(n, 0, pulselen);
      delay(2);
    }
  }
}



< TCS Home