[P-Comp] Shy Guy Playtesting

I spent my entire weekend trying to perfect the movement of the servo in order to make it feel convincingly alive and thus am behind on building my state machine.

Below is my working code for Shy Guy’s default state, FEAR. (The distance of a hand from an IR Sensor is mapped to the angle of a servo motor.) Inspired by the Smoothing Tutorial here, I took it a step (or twelve) further and used a library to find the median of every 5 readings from the IR Sensor and mapped that to the servo angle. It eliminated a lot of the jitteriness. 

/*
  
  ShyGuy w/
  Smoothing & QuickStats Library

  Reads repeatedly from an analog input, calculating a running
  median and printing it to the computer. Keeps ten readings in
  an array and continually finds the median and then maps it to
  the servo angle.

  The circuit:
  - IR sensor attached to analog input 0
  - Micro servo attached to pin 9
  - Both powered by 5V and connected to ground

  Inspired by Smoothing Tutorial:
  http://www.arduino.cc/en/Tutorial/Smoothing

  Uses SharpDistSensor Library:
  https://github.com/DrGFreeman/SharpDistSensor

  Based on QuickStats Smoothing example:
  https://github.com/dndubins/QuickStats/blob/master/examples/smoothread/smoothread.ino

  This example code is in the public domain.

*/


// Declare the SERVO and define its values 
#include <Servo.h>

Servo myServo; // TowerPro Micro Servo 99 SG90

int servoAngle = 0; 
// int sensorVal = 0; 

// Declare the IR SENSOR 
#include <SharpDistSensor.h>

// Sharp IR GP2Y0A41SK0F (40-300mm, analog)
// Analog pin to which the sensor is connected
// I don't know what a const byte is! Thanks, example!
const byte sensorPin = A0;

// I don't really know what this means 
// it was just in the SharpDistSensor example:
// Window size of the median filter 
// (odd number 1-655, 1 = no filtering)
const byte medianFilterWindowSize = 5;

// Create an object instance of the SharpDistSensor class
SharpDistSensor sensor(sensorPin, medianFilterWindowSize);

// Set up QUICKSTATS to find MEDIAN of array of IR SENSOR values
#include <QuickStats.h>

QuickStats stats; //initialize an instance of this class

// Define the number of samples to keep track of. 
// The higher the number, the more the readings will be smoothed,
// but the slower the output will respond to the input. 
// Using a constant rather than a normal variable 
// lets us determine the size of the readings array.
// const int numOfReadings = 10;
const int numOfReadings = 5; // this feels more natural

// In the Arduino Smoothing Tutorial, the array is an INT,
// but in QuickStats it is always a FLOAT?
float readings[numOfReadings]; 
float medianDist;

int readingsIndex = 0; // the index of the current reading

void setup() {
  myServo.attach(9); // servo receives data from pin 9

  // Initialize serial communication with computer:
  Serial.begin(9600);
  Serial.println("Reading...");

  // This sets the model of sensor that the SharpDistSensor
  // library is calibrated to (default is GP2Y0A60SZLF_5V)
  sensor.setModel(SharpDistSensor::GP2Y0A41SK0F_5V_DS);

  // Initialize array of readings:
  for (int thisReading = 0; thisReading < numOfReadings; thisReading++) {
    readings[thisReading] = 0;
  }
}

void loop() {

  // Get distance from sensor using SharpDistSensor Library
  unsigned int distance = sensor.getDist();

  // Assign current distance to the current index in the array: 
  readings[readingsIndex] = distance;

  // Advance to the next position in the array:
  readingsIndex = readingsIndex + 1;

  // If we're at the end of the array...
  if (readingsIndex >= numOfReadings) {
    // ...wrap around to the beginning:
    readingsIndex = 0;
  }

  // Calculate the median distance:
  // (see Median filter from QuickStats.h:25)
  medianDist = stats.median(readings, numOfReadings); 

  // Map the median distance to the servo angle
  // The smaller the distance, the larger the angle
  // The servo can get jittery when pushed to 0 and 180
  servoAngle = map(medianDist, 40, 300, 165, 15);

  if (medianDist >= 40 && medianDist <= 300) {
    Serial.print("Distance in MM: ");
    Serial.print(medianDist);
    Serial.print(", ");
    Serial.print(servoAngle);
    Serial.println(" degrees");

    myServo.write(servoAngle);
    delay(50); // wait
  }
  else {
    myServo.write(0);
  }

}

Here is my vision for how I would like the Shy Guy State Machine to work:

Shy Guy is a micro servo that moves according to the position of the user’s hand in relation to an Infrared Proximity Sensor. He will have three states:

• Fear
• Curiosity
• Affection

FEAR

Shy Guy moves away from the hand as it gets closer.

If the user keeps their hand still for 10 seconds and within 100-150mm away from the sensor…

CURIOSITY

…Shy Guy moves towards the hand, haltingly every few seconds.

If the user moves their hand beyond 100-150mm at all, Shy Guy reverts to FEAR Mode. (Or retreats entirely until a certain time has passed?)

If the user keeps their hand still until Shy Guy is within 40-90cm of it…

AFFECTION

Shy Guy will then follow the hand forward, but not back beyond 90 degrees

Here is my attempt at creating a State Machine that would transition from FEAR to CURIOSITY. I clearly did not understand the mechanisms of the State Machine as well as I thought. (I used this and this to help me wrap my mind around it.)

/***SERVO**************************/
#include <Servo.h>
Servo myServo;      // TowerPro Micro Servo 99 SG90
int servoAngle = 0; 
int sensorVal = 0;  


/***IR SENSOR**********************/
#include <SharpDistSensor.h>

// Sharp IR GP2Y0A41SK0F (4-30cm, analog)
// Analog pin to which the sensor is connected
const byte sensorPin = A0;

// Larger windows means slower processing time
const byte medianFilterWindowSize = 5;

// Create an object instance of the SharpDistSensor class
SharpDistSensor sensor(sensorPin, medianFilterWindowSize);


/* QUICKSTATS *********************/
// QuickStats is a library to help do math to sensor readings
#include <QuickStats.h>
QuickStats stats; // initialize an instance of this class

// Define the number of samples to keep track of. 
// The higher the number, the more smooth, 
// but the slower the output will respond to the input.
// Using a constant rather than a normal variable lets us 
// determine the size of the readings array.
const int numOfReadings = 5;

// I'm not sure why we use float here:
float readings[numOfReadings];  
float medianDist;               
int readingsIndex = 0;         

/* STATE MACHINE ******************/
static unsigned int state;
static unsigned long time;
int pos = 0; // case 1 Curiosity


/* RUNS ONCE **********************/
void setup() {

  time = 0; //millis()?
  state = 0;

  myServo.attach(9); 
  Serial.begin(9600);
  Serial.println("Reading..."); 

  // Sets the model of sensor that the library is calibrated to
  // The default is GP2Y0A60SZLF_5V
  sensor.setModel(SharpDistSensor::GP2Y0A41SK0F_5V_DS);

  // Initialize the array of readings:
  for (int thisReading = 0; thisReading < numOfReadings; thisReading++) {
    readings[thisReading] = 0;
  }

}

/* RUNS CONTINUOUSLY *************/
void loop() {

  // Get distance from sensor
  unsigned int distance = sensor.getDist();

  // Assign current distance to the current index in the array of readings
  readings[readingsIndex] = distance;

  // Advance to the next position in the array:
  readingsIndex = readingsIndex + 1;

  // If we're at the end of the array...
  if (readingsIndex >= numOfReadings) {
    // ...wrap around to the beginning:
    readingsIndex = 0;
  }

  // Calculate the median distance:
  medianDist = stats.median(readings, numOfReadings); // Median filter from QuickStats.h:25

  // Map the distance to the servo angle
  // The smaller the distance, the larger the angle
  // The servo can get jittery when pushed to its limits so I've limited it
  servoAngle = map(medianDist, 40, 300, 165, 15);

  // FEAR
  // If the distance from IR sensor is between its given limits,
  // print the distance and angle in Serial Monitor
  // and move the servo to the angle
  if (medianDist >= 40 && medianDist <= 300) {
    Serial.print("Distance in MM: ");
    Serial.print(medianDist);
    Serial.print(", ");
    Serial.print(servoAngle);
    Serial.println(" degrees");

    myServo.write(servoAngle);
    delay(50); // wait the time it takes to move the servo plus the time to take the median (22ms)

  }
  else {
    myServo.write(0);
  }

  if (medianDist >= 100 && medianDist <= 120) {
    state = 1;
  }

  switch (state) {
    // CURIOSITY
    case 1:
      // 165degrees is the furthest back the servo goes
      // 15degrees is the furthest forward
      // would rather this be timed unevenly
      for (pos = 165; pos >= 15; pos -= 5) { 
        myServo.write(pos);
      }
      break;
  }

}

I’m hoping that by play testing tomorrow I will have at least the states in separate sketches that people could play with.

I’m also still not totally sure what Shy Guy will look like and how I will position the sensor so as to make it feel like you are communicating with Shy Guy directly. Here’s what he looked like during the Paper Prototype Phases while I was still using an Ultrasonic sensor: