Proj 4 Robot Behavior

Post Reply
glegrady
Posts: 203
Joined: Wed Sep 22, 2010 12:26 pm

Proj 4 Robot Behavior

Post by glegrady » Thu May 11, 2017 12:34 pm

Describe the sequence of actions of robot's behavior.

How will it use:
. Forward, and backward movement
. Left and right direction
. Speed
. When will it stop
. Will it use Pan and Tilt? How?
. What will it do when it sees a face?
. When will it trigger sound and lights
. Anything else?
George Legrady
legrady@mat.ucsb.edu

fangfang
Posts: 4
Joined: Wed Apr 12, 2017 5:14 pm

This is the draft- We put our code in other post

Post by fangfang » Mon May 15, 2017 10:36 am

How will it use:
. Forward, and backward movement
. Left and right direction
. Speed
. When will it stop
. Will it use Pan and Tilt? How?
. What will it do when it sees a face?

It will make a sound once it sees a face, and light would be turned on as a notice.
And once it closes to the borderline, the light would be turned up as well.
Last edited by fangfang on Mon May 22, 2017 9:30 am, edited 2 times in total.

christinepang
Posts: 4
Joined: Wed Apr 12, 2017 5:10 pm

Re: Proj 4 Robot Behavior

Post by christinepang » Mon May 15, 2017 10:38 am

TEAM 6

. Left and right direction
When it sees a line, it will turn intelligently i.e. if left sensor is triggered, it will turn to the right

. Speed
We will increase the speed to make it turn faster

. When will it stop
It will pause momentarily when it sees a face and then it will "dance"

. Will it use Pan and Tilt? How?
It will try to center on the face

. What will it do when it sees a face?
It will "dance", play sounds, and flash its lights

Codename: Fabio

Code:
/**
The code controls robot movement.
It has two situations: moving and stop.
In moving situation, the robot moves autonomously and makes turns if it
detects a white line on the ground or an object in front.
As soon as it receives a message from Python, it stops for five seconds.
If no new message coming in those five seconds, the robot changes back to moving situation.

The code was initially written for serving EOYS MAT 2017.

Rodger (Jieliang) Luo
May 11th, 2017
*/

#include <avr/pgmspace.h>
#include <Wire.h>
#include <Zumo32U4.h>

#define DIST_THRESHOLD 9 // brightness level, for infraed sensors
#define QTR_THRESHOLD 500 // microseconds, for line sensors

//----- Editing Area -----
#define REVERSE_SPEED 180 // 0 is stopped, 400 is full speed
#define TURN_SPEED 280
#define FORWARD_SPEED 250
#define REVERSE_DURATION 300 // ms
#define TURN_DURATION 500 // ms
#define LONG_TURN_DURATION 1000
//----- End of Editing Area -----

// Accelerometer Settings
#define RA_SIZE 3 // number of readings to include in running average of accelerometer readings
#define XY_ACCELERATION_THRESHOLD 2400 // for detection of contact (~16000 = magnitude of acceleration due to gravity)

Zumo32U4LCD lcd;
Zumo32U4ButtonA buttonA;
Zumo32U4Buzzer buzzer;
Zumo32U4Motors motors;
Zumo32U4LineSensors lineSensors;
Zumo32U4ProximitySensors proxSensors;

#define NUM_SENSORS 3
unsigned int lineSensorValues[NUM_SENSORS];

unsigned long loop_start_time;
unsigned long last_turn_time;
#define MIN_DELAY_AFTER_TURN 1000 // ms = min delay before detecting contact event

boolean keep_moving = true;

// RunningAverage class
// based on RunningAverage library for Arduino
// source: http://playground.arduino.cc/Main/RunningAverage
template <typename T>
class RunningAverage
{
public:
RunningAverage(void);
RunningAverage(int);
~RunningAverage();
void clear();
void addValue(T);
T getAverage() const;
void fillValue(T, int);
protected:
int _size;
int _cnt;
int _idx;
T _sum;
T * _ar;
static T zero;
};

// Accelerometer Class -- extends the LSM303 class to support reading and averaging the x-y acceleration
// vectors from the onboard LSM303DLHC accelerometer/magnetometer
class Accelerometer : public LSM303
{
typedef struct acc_data_xy
{
unsigned long timestamp;
int x;
int y;
float dir;
} acc_data_xy;

public:
Accelerometer() : ra_x(RA_SIZE), ra_y(RA_SIZE) {};
~Accelerometer() {};
void enable(void);
void getLogHeader(void);
void readAcceleration(unsigned long timestamp);
float len_xy() const;
float dir_xy() const;
int x_avg(void) const;
int y_avg(void) const;
long ss_xy_avg(void) const;
float dir_xy_avg(void) const;
private:
acc_data_xy last;
RunningAverage<int> ra_x;
RunningAverage<int> ra_y;
};

Accelerometer lsm303;

void waitForButtonAndCountDown()
{
ledYellow(1);
lcd.clear();
lcd.print(F("Press A"));

buttonA.waitForButton();

ledYellow(0);
lcd.clear();

// Play audible countdown.
for (int i = 0; i < 3; i++)
{
delay(1000);
buzzer.playNote(NOTE_G(3), 200, 15);
}
delay(1000);
buzzer.playNote(NOTE_G(4), 500, 15);
delay(1000);

last_turn_time = millis();
}

void setup()
{
// Uncomment if necessary to correct motor directions:
//motors.flipLeftMotor(true);
//motors.flipRightMotor(true);

//Initial two groups of sensors
lineSensors.initThreeSensors();
proxSensors.initThreeSensors();

// Initialize the Wire library and join the I2C bus as a master
Wire.begin();
Serial.begin(9600);

// Initialize LSM303
lsm303.init();
lsm303.enable();

waitForButtonAndCountDown();
}

void loop()
{
if (buttonA.isPressed())
{
// If button is pressed, stop and wait for another press to go again.
motors.setSpeeds(0, 0);
buttonA.waitForRelease();
waitForButtonAndCountDown();
}


//----- Editing Area -----
//Listening from Python
if(Serial.read() == '1'){
keep_moving = false; //Stop move if get a message from Python
}
//----- End of Editing Area -----

loop_start_time = millis();
lsm303.readAcceleration(loop_start_time);

proxSensors.read();
lineSensors.read(lineSensorValues);

//----- Editing Area -----
uint8_t sum[4] = {
proxSensors.countsFrontWithRightLeds() + proxSensors.countsFrontWithLeftLeds(),
proxSensors.countsLeftWithLeftLeds() + proxSensors.countsFrontWithLeftLeds(),
proxSensors.countsRightWithRightLeds() + proxSensors.countsFrontWithRightLeds(),
proxSensors.countsFrontWithRightLeds() + proxSensors.countsRightWithRightLeds()
};

//If the robot sees a line or an obstacle, make a turn
//Put line and infraed sensors in separate statements
//so we can assign different behaviors in the future
if(lineSensorValues[0] < QTR_THRESHOLD ){
turnRight();
}
else if (lineSensorValues[NUM_SENSORS - 1] < QTR_THRESHOLD){
turnLeft();
}
else if (sum[0] > DIST_THRESHOLD || sum[1] > DIST_THRESHOLD || sum[2] > DIST_THRESHOLD || sum[3] > DIST_THRESHOLD)
{
turn180();
}


// Otherwise, go straight.
else{
if(keep_moving)
{
if(check_for_contact()) turn180();
motors.setSpeeds(FORWARD_SPEED, FORWARD_SPEED);
}

else if(!keep_moving){
motors.setSpeeds(0, 0);
delay(1000);
Dance();
keep_moving = true;
}
}
}

void turnLeft(){
motors.setSpeeds(-REVERSE_SPEED, -REVERSE_SPEED);
delay(REVERSE_DURATION);
motors.setSpeeds(-TURN_SPEED, TURN_SPEED);
delay(TURN_DURATION);
last_turn_time = millis();
}
void turnRight(){
motors.setSpeeds(-REVERSE_SPEED, -REVERSE_SPEED);
delay(REVERSE_DURATION);
motors.setSpeeds(TURN_SPEED, -TURN_SPEED);
delay(TURN_DURATION);
last_turn_time = millis();
}
void turn180(){
motors.setSpeeds(-REVERSE_SPEED, -REVERSE_SPEED);
delay(REVERSE_DURATION);
motors.setSpeeds(TURN_SPEED, -TURN_SPEED);
delay(LONG_TURN_DURATION);
last_turn_time = millis();
}
void Dance(){
buzzer.playNote(NOTE_E(4), 1000, 15);
LEDon();
motors.setSpeeds(200, 400);
delay(500);
LEDoff();
motors.setSpeeds(-200, -400);
delay(500);
buzzer.playNote(NOTE_E_FLAT(4), 1000, 15);
LEDon();
motors.setSpeeds(400, 200);
delay(500);
LEDoff();
motors.setSpeeds(-400, -200);
delay(500);
buzzer.playNote(NOTE_D(4), 1500, 15);
LEDon();
motors.setSpeeds(-350, 350);
delay(1500);
buzzer.playNote(NOTE_D_FLAT(4), 3000, 15);
motors.setSpeeds(350, -350);
delay(3000);
LEDoff();
}
void LEDon(){
ledRed(1);
ledYellow(1);
ledGreen(1);
}
void LEDoff(){
ledRed(0);
ledYellow(0);
ledGreen(0);
}
//----- End of Editing Area -----

// check for contact, but ignore readings immediately after turning or losing contact
bool check_for_contact()
{
static long threshold_squared = (long) XY_ACCELERATION_THRESHOLD * (long) XY_ACCELERATION_THRESHOLD;
return (lsm303.ss_xy_avg() > threshold_squared) && (loop_start_time - last_turn_time > MIN_DELAY_AFTER_TURN);
}

// class Accelerometer -- member function definitions

// enable accelerometer only
// to enable both accelerometer and magnetometer, call enableDefault() instead
void Accelerometer::enable(void)
{
// Enable Accelerometer
// 0x27 = 0b00100111
// Normal power mode, all axes enabled
writeAccReg(LSM303::CTRL_REG1_A, 0x27);

if (getDeviceType() == LSM303::device_DLHC)
writeAccReg(LSM303::CTRL_REG4_A, 0x08); // DLHC: enable high resolution mode
}

void Accelerometer::getLogHeader(void)
{
Serial.print("millis x y len dir | len_avg dir_avg | avg_len");
Serial.println();
}

void Accelerometer::readAcceleration(unsigned long timestamp)
{
readAcc();
if (a.x == last.x && a.y == last.y) return;

last.timestamp = timestamp;
last.x = a.x;
last.y = a.y;

ra_x.addValue(last.x);
ra_y.addValue(last.y);
}

float Accelerometer::len_xy() const
{
return sqrt(last.x*a.x + last.y*a.y);
}

float Accelerometer::dir_xy() const
{
return atan2(last.x, last.y) * 180.0 / M_PI;
}

int Accelerometer::x_avg(void) const
{
return ra_x.getAverage();
}

int Accelerometer::y_avg(void) const
{
return ra_y.getAverage();
}

long Accelerometer::ss_xy_avg(void) const
{
long x_avg_long = static_cast<long>(x_avg());
long y_avg_long = static_cast<long>(y_avg());
return x_avg_long*x_avg_long + y_avg_long*y_avg_long;
}

float Accelerometer::dir_xy_avg(void) const
{
return atan2(static_cast<float>(x_avg()), static_cast<float>(y_avg())) * 180.0 / M_PI;
}



// RunningAverage class
// based on RunningAverage library for Arduino
// source: http://playground.arduino.cc/Main/RunningAverage
// author: Rob.Tillart@gmail.com
// Released to the public domain

template <typename T>
T RunningAverage<T>::zero = static_cast<T>(0);

template <typename T>
RunningAverage<T>::RunningAverage(int n)
{
_size = n;
_ar = (T*) malloc(_size * sizeof(T));
clear();
}

template <typename T>
RunningAverage<T>::~RunningAverage()
{
free(_ar);
}

// resets all counters
template <typename T>
void RunningAverage<T>::clear()
{
_cnt = 0;
_idx = 0;
_sum = zero;
for (int i = 0; i< _size; i++) _ar = zero; // needed to keep addValue simple
}

// adds a new value to the data-set
template <typename T>
void RunningAverage<T>::addValue(T f)
{
_sum -= _ar[_idx];
_ar[_idx] = f;
_sum += _ar[_idx];
_idx++;
if (_idx == _size) _idx = 0; // faster than %
if (_cnt < _size) _cnt++;
}

// returns the average of the data-set added so far
template <typename T>
T RunningAverage<T>::getAverage() const
{
if (_cnt == 0) return zero; // NaN ? math.h
return _sum / _cnt;
}

// fill the average with a value
// the param number determines how often value is added (weight)
// number should preferably be between 1 and size
template <typename T>
void RunningAverage<T>::fillValue(T value, int number)
{
clear();
for (int i = 0; i < number; i++)
{
addValue(value);
}
}
Last edited by christinepang on Mon May 22, 2017 10:38 am, edited 2 times in total.

anniewong
Posts: 4
Joined: Wed Apr 12, 2017 5:11 pm

Re: Proj 4 Robot Behavior

Post by anniewong » Mon May 15, 2017 10:41 am

Group 2: Leah Armer, Margie Fargas, Annie Wong

GOALS FOR ROBOT BEHAVIOUR: Our robot, Leonard, will use the following features in these modified behavioral ways:

1. Forward, and backward movement: Both speeds are at 300.
2. Left and right direction: Turn speed is at 200 while his turn duration is 500 ms.
3. Speed: Leonard will go a moderate speed of 300, reverse and acceleration are also 300.
4. When will it stop: Leonard is an incessant beast who will not stop unless controlled by "control + c" by his mothers. Leonard will only pause momentarily when retracting from tape, taking a picture, and avoiding objects.
5. Will it use Pan and Tilt? How?: Leonard will be panning and tilting constantly; it will be his main goal to search continuously to find the maximum amount of faces possible, therefore, his face will be in constant movement.
6. What will it do when it sees a face?: Leonard will play a song when he finds a face and snap a picture.
7. When will it trigger sound and lights?: Leonard will be triggered to play a sound when he finds a face/ takes a picture. Leonard will flash his lights whenever he runs into a white tape.

Group 2 participants and their robot endeavors to successfully complete these tasks, and to program the robot correctly so that it might accurately fulfill this agenda.
----------------------------------------------------------------------------------------------------------------------------------
Summary: At MAT night, we took Leonard out for a spin in the arena and maximized his speeds to 300. We were abe to program the R2D2 beep function and pan tilt functions as well. However, somehow, Leonard become relentlessly rebellious, revving at unimaginable speeds unpredicted by us. He was so fast, we was doing wheelies and rearing on his back wheels. He was so rambunctious that Rodger had to remove him and place him in time out for a little while, in which he sat in the middle of the playground, furiously panning and tilting. The biggest problem we ran into with Leonard that he is not very good at detecting faces and as a consequence, snapping pictures. If he doesn't take pictures, we can not hear the buzzer because Leonard will not play the R2D2 sound. We still need to program the light feature when he approaches white tape and we'll need to refine his rough manners. He even at times, went so fast, betrayed his built-in sensors and code and went over and beyond the white tape. But I think we can solve that by dialing down his speeds of pan and forward movement.

Code: Select all

#include <avr/pgmspace.h>
#include <Wire.h>
#include <Zumo32U4.h>

#define DIST_THRESHOLD     9  // brightness level, for infraed sensors
#define QTR_THRESHOLD     500  // microseconds, for line sensors

//----- Editing Area -----
#define REVERSE_SPEED     200  // 0 is stopped, 400 is full speed
#define TURN_SPEED        200
#define FORWARD_SPEED     300
#define REVERSE_DURATION  300  // ms
#define TURN_DURATION     500  // ms
//----- End of Editing Area -----

// Accelerometer Settings
#define RA_SIZE 3  // number of readings to include in running average of accelerometer readings
#define XY_ACCELERATION_THRESHOLD 2400  // for detection of contact (~16000 = magnitude of acceleration due to gravity)

Zumo32U4LCD lcd;
Zumo32U4ButtonA buttonA;
Zumo32U4Buzzer buzzer;
Zumo32U4Motors motors;
Zumo32U4LineSensors lineSensors;
Zumo32U4ProximitySensors proxSensors;

#define NUM_SENSORS 3
unsigned int lineSensorValues[NUM_SENSORS];

unsigned long loop_start_time;
unsigned long last_turn_time;
#define MIN_DELAY_AFTER_TURN          1000  // ms = min delay before detecting contact event

boolean keep_moving = true;

// RunningAverage class
// based on RunningAverage library for Arduino
// source:  http://playground.arduino.cc/Main/RunningAverage
template <typename T>
class RunningAverage
{
  public:
    RunningAverage(void);
    RunningAverage(int);
    ~RunningAverage();
    void clear();
    void addValue(T);
    T getAverage() const;
    void fillValue(T, int);
  protected:
    int _size;
    int _cnt;
    int _idx;
    T _sum;
    T * _ar;
    static T zero;
};

// Accelerometer Class -- extends the LSM303 class to support reading and averaging the x-y acceleration
//   vectors from the onboard LSM303DLHC accelerometer/magnetometer
class Accelerometer : public LSM303
{
  typedef struct acc_data_xy
  {
    unsigned long timestamp;
    int x;
    int y;
    float dir;
  } acc_data_xy;

  public:
    Accelerometer() : ra_x(RA_SIZE), ra_y(RA_SIZE) {};
    ~Accelerometer() {};
    void enable(void);
    void getLogHeader(void);
    void readAcceleration(unsigned long timestamp);
    float len_xy() const;
    float dir_xy() const;
    int x_avg(void) const;
    int y_avg(void) const;
    long ss_xy_avg(void) const;
    float dir_xy_avg(void) const;
  private:
    acc_data_xy last;
    RunningAverage<int> ra_x;
    RunningAverage<int> ra_y;
};

Accelerometer lsm303;

void waitForButtonAndCountDown()
{
  ledYellow(1);
  lcd.clear();
  lcd.print(F("Press A"));

  buttonA.waitForButton();

  ledYellow(0);
  lcd.clear();

  // Play audible countdown.
  for (int i = 0; i < 3; i++)
  {
    delay(1000);
    buzzer.playNote(NOTE_A(7),100, 15); //A 
    buzzer.playNote(NOTE_G(7),100, 15); //G 
    buzzer.playNote(NOTE_E(7),100, 15); //E 
    buzzer.playNote(NOTE_C(7),100, 15); //C
    buzzer.playNote(NOTE_D(7),100, 15); //D 
    buzzer.playNote(NOTE_B(7),100, 15); //B 
    buzzer.playNote(NOTE_F(7),100, 15); //F 
    buzzer.playNote(NOTE_C(8),100, 15); //C 
    buzzer.playNote(NOTE_A(7),100, 15); //A 
    buzzer.playNote(NOTE_G(7),100, 15); //G 
    buzzer.playNote(NOTE_E(7),100, 15); //E 
    buzzer.playNote(NOTE_C(7),100, 15); //C
    buzzer.playNote(NOTE_D(7),100, 15); //D 
    buzzer.playNote(NOTE_B(7),100, 15); //B 
    buzzer.playNote(NOTE_F(7),100, 15); //F 
    buzzer.playNote(NOTE_C(8),100, 15); //C 
  }
  last_turn_time = millis(); 
}

void setup()
{
  // Uncomment if necessary to correct motor directions:
  //motors.flipLeftMotor(true);
  //motors.flipRightMotor(true);

  //Initial two groups of sensors
  lineSensors.initThreeSensors();
  proxSensors.initThreeSensors();

  // Initialize the Wire library and join the I2C bus as a master
  Wire.begin();
  Serial.begin(9600);

 // Initialize LSM303
  lsm303.init();
  lsm303.enable();

  waitForButtonAndCountDown();
}

void loop()
{
  if (buttonA.isPressed())
  {
    // If button is pressed, stop and wait for another press to go again.
    motors.setSpeeds(0, 0);
    buttonA.waitForRelease();
    waitForButtonAndCountDown();
  }

  
//----- Editing Area -----
  //Listening from Python
  if(Serial.read() == '1'){
    keep_moving = false; //Stop move if get a message from Python
    delay(1000);
    buzzer.playNote(NOTE_A(7),100, 15); //A 
    buzzer.playNote(NOTE_G(7),100, 15); //G 
    buzzer.playNote(NOTE_E(7),100, 15); //E 
    buzzer.playNote(NOTE_C(7),100, 15); //C
    buzzer.playNote(NOTE_D(7),100, 15); //D 
    buzzer.playNote(NOTE_B(7),100, 15); //B 
    buzzer.playNote(NOTE_F(7),100, 15); //F 
    buzzer.playNote(NOTE_C(8),100, 15); //C 
    buzzer.playNote(NOTE_A(7),100, 15); //A 
    buzzer.playNote(NOTE_G(7),100, 15); //G 
    buzzer.playNote(NOTE_E(7),100, 15); //E 
    buzzer.playNote(NOTE_C(7),100, 15); //C
    buzzer.playNote(NOTE_D(7),100, 15); //D 
    buzzer.playNote(NOTE_B(7),100, 15); //B 
    buzzer.playNote(NOTE_F(7),100, 15); //F 
    buzzer.playNote(NOTE_C(8),100, 15); //C 
    delay(3000);
    buzzer.stopPlaying();
    delay(1000);
  }
//----- End of Editing Area ----- 
  
  loop_start_time = millis();
  lsm303.readAcceleration(loop_start_time);
  
  proxSensors.read();
  lineSensors.read(lineSensorValues);

//----- Editing Area -----
  uint8_t sum[4] = {
     proxSensors.countsFrontWithRightLeds() + proxSensors.countsFrontWithLeftLeds(),
     proxSensors.countsLeftWithLeftLeds() + proxSensors.countsFrontWithLeftLeds(),
     proxSensors.countsRightWithRightLeds() + proxSensors.countsFrontWithRightLeds(),
     proxSensors.countsFrontWithRightLeds() + proxSensors.countsRightWithRightLeds() 
  };

  //If the robot sees a line or an obstacle, make a turn 
  //Put line and infraed sensors in separate statements 
  //so we can assign different behaviors in the future  
  if(lineSensorValues[0] < QTR_THRESHOLD || lineSensorValues[NUM_SENSORS - 1] < QTR_THRESHOLD){
    turn();  
  }
  else if (sum[0] > DIST_THRESHOLD || sum[1] > DIST_THRESHOLD || sum[2] > DIST_THRESHOLD || sum[3] > DIST_THRESHOLD)
  {
    turn();
  }


  // Otherwise, go straight.
  else{
    if(keep_moving)
    {
      if(check_for_contact()) turn();
      motors.setSpeeds(FORWARD_SPEED, FORWARD_SPEED);
    }

    else if(!keep_moving){
      motors.setSpeeds(0, 0);
      delay(5000);
      keep_moving = true;
    }
  }
}

void turn(){
    motors.setSpeeds(-REVERSE_SPEED, -REVERSE_SPEED);
    delay(REVERSE_DURATION);
    motors.setSpeeds(-TURN_SPEED, TURN_SPEED);
    delay(TURN_DURATION);
    last_turn_time = millis();
}
//----- End of Editing Area -----

// check for contact, but ignore readings immediately after turning or losing contact
bool check_for_contact()
{
  static long threshold_squared = (long) XY_ACCELERATION_THRESHOLD * (long) XY_ACCELERATION_THRESHOLD;
  return (lsm303.ss_xy_avg() >  threshold_squared) && (loop_start_time - last_turn_time > MIN_DELAY_AFTER_TURN);
}

// class Accelerometer -- member function definitions

// enable accelerometer only
// to enable both accelerometer and magnetometer, call enableDefault() instead
void Accelerometer::enable(void)
{
  // Enable Accelerometer
  // 0x27 = 0b00100111
  // Normal power mode, all axes enabled
  writeAccReg(LSM303::CTRL_REG1_A, 0x27);

  if (getDeviceType() == LSM303::device_DLHC)
  writeAccReg(LSM303::CTRL_REG4_A, 0x08); // DLHC: enable high resolution mode
}

void Accelerometer::getLogHeader(void)
{
  Serial.print("millis    x      y     len     dir  | len_avg  dir_avg  |  avg_len");
  Serial.println();
}

void Accelerometer::readAcceleration(unsigned long timestamp)
{
  readAcc();
  if (a.x == last.x && a.y == last.y) return;

  last.timestamp = timestamp;
  last.x = a.x;
  last.y = a.y;

  ra_x.addValue(last.x);
  ra_y.addValue(last.y);
}

float Accelerometer::len_xy() const
{
  return sqrt(last.x*a.x + last.y*a.y);
}

float Accelerometer::dir_xy() const
{
  return atan2(last.x, last.y) * 180.0 / M_PI;
}

int Accelerometer::x_avg(void) const
{
  return ra_x.getAverage();
}

int Accelerometer::y_avg(void) const
{
  return ra_y.getAverage();
}

long Accelerometer::ss_xy_avg(void) const
{
  long x_avg_long = static_cast<long>(x_avg());
  long y_avg_long = static_cast<long>(y_avg());
  return x_avg_long*x_avg_long + y_avg_long*y_avg_long;
}

float Accelerometer::dir_xy_avg(void) const
{
  return atan2(static_cast<float>(x_avg()), static_cast<float>(y_avg())) * 180.0 / M_PI;
}



// RunningAverage class
// based on RunningAverage library for Arduino
// source:  http://playground.arduino.cc/Main/RunningAverage
// author:  Rob.Tillart@gmail.com
// Released to the public domain

template <typename T>
T RunningAverage<T>::zero = static_cast<T>(0);

template <typename T>
RunningAverage<T>::RunningAverage(int n)
{
  _size = n;
  _ar = (T*) malloc(_size * sizeof(T));
  clear();
}

template <typename T>
RunningAverage<T>::~RunningAverage()
{
  free(_ar);
}

// resets all counters
template <typename T>
void RunningAverage<T>::clear()
{
  _cnt = 0;
  _idx = 0;
  _sum = zero;
  for (int i = 0; i< _size; i++) _ar[i] = zero;  // needed to keep addValue simple
}

// adds a new value to the data-set
template <typename T>
void RunningAverage<T>::addValue(T f)
{
  _sum -= _ar[_idx];
  _ar[_idx] = f;
  _sum += _ar[_idx];
  _idx++;
  if (_idx == _size) _idx = 0;  // faster than %
  if (_cnt < _size) _cnt++;
}

// returns the average of the data-set added so far
template <typename T>
T RunningAverage<T>::getAverage() const
{
  if (_cnt == 0) return zero; // NaN ?  math.h
  return _sum / _cnt;
}

// fill the average with a value
// the param number determines how often value is added (weight)
// number should preferably be between 1 and size
template <typename T>
void RunningAverage<T>::fillValue(T value, int number)
{
  clear();
  for (int i = 0; i < number; i++)
  {
    addValue(value);
  }
}
Last edited by anniewong on Mon May 22, 2017 10:51 am, edited 1 time in total.

annieyfong
Posts: 4
Joined: Wed Apr 12, 2017 5:07 pm

Re: Proj 4 Robot Behavior

Post by annieyfong » Mon May 15, 2017 10:49 am

Team 4:-

. Forward, and backward movement
- Forward Movement
. Left and right direction
- Left when an object is detected
. Speed
- 300
. When will it stop
- When it sees an Object, and when it is time to take pictures
. Will it use Pan and Tilt? How?
- If no face is found:
tilt_pos += random.randint(-10, 10)
pan_pos += random.randint(-15, 15)

If face is found:
tilt_pos = random.randint(TILT_MIN_POS, TILT_MAX_POS)
pan_pos = random.randint(PAN_MIN_POS, PAN_MAX_POS)

. What will it do when it sees a face?
- Stop for 2 seconds to find more faces before taking a picture
. When will it trigger sound and lights
- Sound is triggered when a face is detected

stephannilarsen
Posts: 3
Joined: Mon Apr 17, 2017 11:05 am

Re: Proj 4 Robot Behavior

Post by stephannilarsen » Mon May 15, 2017 10:54 am

Group 3: BY TAYLOR, STEPHANNI AND CHANTEL
How our robot will respond:

How will it use:
Forward, and backward movement: Our robot will use the forward/Backward motion before it takes a picture, so it quickly move backward to take a photo, and then continue forward in its motion. this will be in order to represent a "step backward" like an acknowledgement of an intentional moment to be present before the robot captures its environment.
Left and right direction: Left and right motion will be used in order to take our robot in a circle, the circle will be done when a face is detected.
Speed: The speed will remain the same
When will it stop: Only for the moment it takes the photo.
Will it use Pan and Tilt? How?: It will pan to the top and then back to 90 degrees to show it has seen a face before it spins.
What will it do when it sees a face?: It will pan up to and down to 90 degrees then do a 360 degree rotation, to seem as though it is excited to see someone. it will only do one circle.
When will it trigger sound and lights: Lights and sounds will play from the time that it moves, and will stop as the robot takes a picture, and then turn back on as it continues to move.
Anything else? the robot will be traveling, looking for people to have fun with. It will evoke joy, and laugher. We would like our robot to be the life of the party, bring humor, charisma, and joy to its partners, human and robot.

Summary of what it will do: Our robot will travel( along a path? or in some structured way TBD). It will take pictures every 30 sec. to 1 minute. It will play music, and joyfully see faces, and when it does it will spin in a circle to the left and tilt its little camera up/down as a greeting. Upon competing its circle the robot will continue on its way. The point of our project will be to capture time and space, and metaphorically represent a walk through life. The fact that our robot will bring lively music, and only when the music pauses, and it is not distracted by noise and input, will it take an image. Ultimately our focus it to provide entertainment, and joy. And capture the little moments in-between. the robot will also respond to its environment. So recognizing faces will allow it to be interactive. to see people and respond to them.

(stay tuned: this proposal will adjust as the rest of our group agrees on plausibility and probability of success-Stephanni)

zoe.m.rathbun
Posts: 5
Joined: Wed Apr 12, 2017 5:14 pm

Re: Proj 4 Robot Behavior

Post by zoe.m.rathbun » Tue May 16, 2017 1:14 pm

GROUP 5: Zoë, Emeric and Margarita
ACTUAL PROJECT:
Creating our Shy Robot ended up being a bit more difficult than anticipated just due to the nature of what areas of the code could be edited...
Therefore we shifted directions and created a robotic behavior that was based on randomness.
Our robot turned randomly right or left when it sensed a line or object. This allowed our robot to navigate more freely and avoid obstacles.
When a picture was taken, our robot made either a positive or negative sounding noise at random. To our group, it seemed as if [Roger] our Robot was judging the face that it had seen. The noise it made after a photo was taken/a face was identified was relatively cute and organic and gave our robot a life-like appearance and behavior. Incorporating different cases of behavior (and randomness) made the robots seem more intelligent and intriguing to viewers, making the behavior and interactions seem less fully programmed to happen (as randomness was introduced).
These rather simple changes gave our robot a unique behavior which was interesting to watch. Incorporating more cases of different behavior could be an interesting way to continue the project

Code: Select all

/**
  The code controls robot movement. 
  It has two situations: moving and stop.
  In moving situation, the robot moves autonomously and makes turns if it 
  detects a white line on the ground or an object in front. 
  As soon as it receives a message from Python, it stops for five seconds. 
  If no new message coming in those five seconds, the robot changes back to moving situation.  

  The code was initially written for serving EOYS MAT 2017. 

  Rodger (Jieliang) Luo
  May 11th, 2017
*/

#include <avr/pgmspace.h>
#include <Wire.h>
#include <Zumo32U4.h>

#define DIST_THRESHOLD     9  // brightness level, for infraed sensors
#define QTR_THRESHOLD     500  // microseconds, for line sensors

//----- Editing Area -----
#define REVERSE_SPEED     100  // 0 is stopped, 400 is full speed
#define TURN_SPEED        200
#define FORWARD_SPEED     200
#define REVERSE_DURATION  300  // ms
#define TURN_DURATION     500  // ms
//----- End of Editing Area -----

// Accelerometer Settings
#define RA_SIZE 3  // number of readings to include in running average of accelerometer readings
#define XY_ACCELERATION_THRESHOLD 2400  // for detection of contact (~16000 = magnitude of acceleration due to gravity)

Zumo32U4LCD lcd;
Zumo32U4ButtonA buttonA;
Zumo32U4Buzzer buzzer;
Zumo32U4Motors motors;
Zumo32U4LineSensors lineSensors;
Zumo32U4ProximitySensors proxSensors;

#define NUM_SENSORS 3
unsigned int lineSensorValues[NUM_SENSORS];

unsigned long loop_start_time;
unsigned long last_turn_time;
#define MIN_DELAY_AFTER_TURN          1000  // ms = min delay before detecting contact event

boolean keep_moving = true;

// RunningAverage class
// based on RunningAverage library for Arduino
// source:  http://playground.arduino.cc/Main/RunningAverage
template <typename T>
class RunningAverage
{
  public:
    RunningAverage(void);
    RunningAverage(int);
    ~RunningAverage();
    void clear();
    void addValue(T);
    T getAverage() const;
    void fillValue(T, int);
  protected:
    int _size;
    int _cnt;
    int _idx;
    T _sum;
    T * _ar;
    static T zero;
};

// Accelerometer Class -- extends the LSM303 class to support reading and averaging the x-y acceleration
//   vectors from the onboard LSM303DLHC accelerometer/magnetometer
class Accelerometer : public LSM303
{
  typedef struct acc_data_xy
  {
    unsigned long timestamp;
    int x;
    int y;
    float dir;
  } acc_data_xy;

  public:
    Accelerometer() : ra_x(RA_SIZE), ra_y(RA_SIZE) {};
    ~Accelerometer() {};
    void enable(void);
    void getLogHeader(void);
    void readAcceleration(unsigned long timestamp);
    float len_xy() const;
    float dir_xy() const;
    int x_avg(void) const;
    int y_avg(void) const;
    long ss_xy_avg(void) const;
    float dir_xy_avg(void) const;
  private:
    acc_data_xy last;
    RunningAverage<int> ra_x;
    RunningAverage<int> ra_y;
};

Accelerometer lsm303;

void waitForButtonAndCountDown()
{
  ledYellow(1);
  lcd.clear();
  lcd.print(F("Press A"));

  buttonA.waitForButton();

  ledYellow(0);
  lcd.clear();

  // Play audible countdown.
  for (int i = 0; i < 3; i++)
  {
    delay(1000);
    buzzer.playNote(NOTE_G(3), 200, 15);
  }
  delay(1000);
  buzzer.playNote(NOTE_G(4), 500, 15);
  delay(1000);

  last_turn_time = millis(); 
}

void setup()
{
  // Uncomment if necessary to correct motor directions:
  //motors.flipLeftMotor(true);
  //motors.flipRightMotor(true);

  //Initial two groups of sensors
  lineSensors.initThreeSensors();
  proxSensors.initThreeSensors();

  // Initialize the Wire library and join the I2C bus as a master
  Wire.begin();
  Serial.begin(9600);

 // Initialize LSM303
  lsm303.init();
  lsm303.enable();

  waitForButtonAndCountDown();
}

void loop()
{
  if (buttonA.isPressed())
  {
    // If button is pressed, stop and wait for another press to go again.
    motors.setSpeeds(0, 0);
    buttonA.waitForRelease();
    waitForButtonAndCountDown();
  }

  
//----- Editing Area -----
  //Listening from Python
  if(Serial.read() == '1'){
    keep_moving = false; //Stop move if get a message from Python
  }
//----- End of Editing Area ----- 
  
  loop_start_time = millis();
  lsm303.readAcceleration(loop_start_time);
  
  proxSensors.read();
  lineSensors.read(lineSensorValues);

//----- Editing Area -----
  uint8_t sum[4] = {
     proxSensors.countsFrontWithRightLeds() + proxSensors.countsFrontWithLeftLeds(),
     proxSensors.countsLeftWithLeftLeds() + proxSensors.countsFrontWithLeftLeds(),
     proxSensors.countsRightWithRightLeds() + proxSensors.countsFrontWithRightLeds(),
     proxSensors.countsFrontWithRightLeds() + proxSensors.countsRightWithRightLeds() 
  };

  //If the robot sees a line or an obstacle, make a turn 
  //Put line and infraed sensors in separate statements 
  //so we can assign different behaviors in the future  
  if(lineSensorValues[0] < QTR_THRESHOLD || lineSensorValues[NUM_SENSORS - 1] < QTR_THRESHOLD){
    turn();  
  }
  else if (sum[0] > DIST_THRESHOLD || sum[1] > DIST_THRESHOLD || sum[2] > DIST_THRESHOLD || sum[3] > DIST_THRESHOLD)
  {
    turn();
  }


  // Otherwise, go straight.
  else{
    if(keep_moving)
    {
      if(check_for_contact()) turn();
      motors.setSpeeds(FORWARD_SPEED, FORWARD_SPEED);
    }

    else if(!keep_moving){
      motors.setSpeeds(0, 0);
      ledRed(1);
      float rand_int_buzz = random(0,2);
      if (rand_int_buzz < 1) {
        buzzer.playNote(NOTE_F(2), 200, 10);
      delay(100);
      buzzer.playNote(NOTE_E(2), 200, 15);
      delay(100);
      buzzer.playNote(NOTE_C(2), 400, 10);
      delay(200);
      buzzer.playNote(NOTE_G(2), 400, 15); 
     
      }
      else{
        buzzer.playNote(NOTE_F(5), 100, 10);
      delay(50);
      buzzer.playNote(NOTE_E(5), 100, 15);
      delay(50);
      buzzer.playNote(NOTE_C(5), 200, 10);
      delay(100);
      buzzer.playNote(NOTE_A(5), 200, 15);
      }
      
      delay(5000);
      keep_moving = true;
      ledRed(0);
      turn();
    }
  }
}

void turn(){
    motors.setSpeeds(-REVERSE_SPEED, -REVERSE_SPEED);
    delay(REVERSE_DURATION);
    float rand_int = random(0,2);
    if(rand_int < 1) {
      motors.setSpeeds(-TURN_SPEED, TURN_SPEED);
      delay(TURN_DURATION);
      last_turn_time = millis();
    }
    else {
      motors.setSpeeds(TURN_SPEED, -TURN_SPEED);
      delay(TURN_DURATION);
      last_turn_time = millis();
    }
}
//----- End of Editing Area -----

// check for contact, but ignore readings immediately after turning or losing contact
bool check_for_contact()
{
  static long threshold_squared = (long) XY_ACCELERATION_THRESHOLD * (long) XY_ACCELERATION_THRESHOLD;
  return (lsm303.ss_xy_avg() >  threshold_squared) && (loop_start_time - last_turn_time > MIN_DELAY_AFTER_TURN);
}

// class Accelerometer -- member function definitions

// enable accelerometer only
// to enable both accelerometer and magnetometer, call enableDefault() instead
void Accelerometer::enable(void)
{
  // Enable Accelerometer
  // 0x27 = 0b00100111
  // Normal power mode, all axes enabled
  writeAccReg(LSM303::CTRL_REG1_A, 0x27);

  if (getDeviceType() == LSM303::device_DLHC)
  writeAccReg(LSM303::CTRL_REG4_A, 0x08); // DLHC: enable high resolution mode
}

void Accelerometer::getLogHeader(void)
{
  Serial.print("millis    x      y     len     dir  | len_avg  dir_avg  |  avg_len");
  Serial.println();
}

void Accelerometer::readAcceleration(unsigned long timestamp)
{
  readAcc();
  if (a.x == last.x && a.y == last.y) return;

  last.timestamp = timestamp;
  last.x = a.x;
  last.y = a.y;

  ra_x.addValue(last.x);
  ra_y.addValue(last.y);
}

float Accelerometer::len_xy() const
{
  return sqrt(last.x*a.x + last.y*a.y);
}

float Accelerometer::dir_xy() const
{
  return atan2(last.x, last.y) * 180.0 / M_PI;
}

int Accelerometer::x_avg(void) const
{
  return ra_x.getAverage();
}

int Accelerometer::y_avg(void) const
{
  return ra_y.getAverage();
}

long Accelerometer::ss_xy_avg(void) const
{
  long x_avg_long = static_cast<long>(x_avg());
  long y_avg_long = static_cast<long>(y_avg());
  return x_avg_long*x_avg_long + y_avg_long*y_avg_long;
}

float Accelerometer::dir_xy_avg(void) const
{
  return atan2(static_cast<float>(x_avg()), static_cast<float>(y_avg())) * 180.0 / M_PI;
}



// RunningAverage class
// based on RunningAverage library for Arduino
// source:  http://playground.arduino.cc/Main/RunningAverage
// author:  Rob.Tillart@gmail.com
// Released to the public domain

template <typename T>
T RunningAverage<T>::zero = static_cast<T>(0);

template <typename T>
RunningAverage<T>::RunningAverage(int n)
{
  _size = n;
  _ar = (T*) malloc(_size * sizeof(T));
  clear();
}

template <typename T>
RunningAverage<T>::~RunningAverage()
{
  free(_ar);
}

// resets all counters
template <typename T>
void RunningAverage<T>::clear()
{
  _cnt = 0;
  _idx = 0;
  _sum = zero;
  for (int i = 0; i< _size; i++) _ar[i] = zero;  // needed to keep addValue simple
}

// adds a new value to the data-set
template <typename T>
void RunningAverage<T>::addValue(T f)
{
  _sum -= _ar[_idx];
  _ar[_idx] = f;
  _sum += _ar[_idx];
  _idx++;
  if (_idx == _size) _idx = 0;  // faster than %
  if (_cnt < _size) _cnt++;
}

// returns the average of the data-set added so far
template <typename T>
T RunningAverage<T>::getAverage() const
{
  if (_cnt == 0) return zero; // NaN ?  math.h
  return _sum / _cnt;
}

// fill the average with a value
// the param number determines how often value is added (weight)
// number should preferably be between 1 and size
template <typename T>
void RunningAverage<T>::fillValue(T value, int number)
{
  clear();
  for (int i = 0; i < number; i++)
  {
    addValue(value);
  }
}

Concept: Our Robot will have a bashful personality and behavior. In other words, it will be a shybot<3.

How will it use:
Forward, and backward movement
Forward movement will be triggered when the robot is searching for a face. Slight backward movement will be triggered when a line, object, or face is detected, contributing to our robot's shy behavior.
Left and right direction
When a line or object is detected we will trigger a random function to determine if it will go left or right.
Speed
Determining speed will be a bit of a concept of trial and error, however, I want our robot to be a little slower than the others as if it is meandering around the space in a bashful manner.
When will it stop
It will stop for a brief moment when it sees a face so that a stable picture can be taken. It will also stop when a line/object is detected before it moves and makes a turn.
Will it use Pan and Tilt? How?
After seeing a face, the robot will tilt downwards as if it is a bashful human averting its eyes/face when it makes eye contact with another being.
If no face is detected, perhaps a back and forth scanning behavior (with pan) can be triggered so that it can continue to scan the environment until a face is detected.
What will it do when it sees a face?
When our robot sees a face it will pause for a moment, take a picture, and then turn away. It will also make a sound that can be objectively considered as cute/shy to signify the fact that it has detected a face.
When will it trigger sound and lights
A sound resembling a sound of a shy animal will be triggered when the face is detected. Lights could also be added when a face is detected as a further signal of face detection.
Last edited by zoe.m.rathbun on Mon May 22, 2017 10:01 am, edited 2 times in total.

taylormoon2014
Posts: 8
Joined: Wed Apr 12, 2017 5:47 pm

Re: Proj 4 Robot Behavior

Post by taylormoon2014 » Wed May 17, 2017 10:59 am

GROUP 3!!! WE GOT OUR ROBOT TO SPIN AND BLINK LEDs WHEN IT FINDS A FACE!!!!

/**
The code controls robot movement.
It has two situations: moving and stop.
In moving situation, the robot moves autonomously and makes turns if it
detects a white line on the ground or an object in front.
As soon as it receives a message from Python, it stops for five seconds.
If no new message coming in those five seconds, the robot changes back to moving situation.

The code was initially written for serving EOYS MAT 2017.

Rodger (Jieliang) Luo
May 11th, 2017
*/

#include <avr/pgmspace.h>
#include <Wire.h>
#include <Zumo32U4.h>

#define DIST_THRESHOLD 9 // brightness level, for infraed sensors
#define QTR_THRESHOLD 500 // microseconds, for line sensors

//----- Editing Area -----
#define REVERSE_SPEED 100 // 0 is stopped, 400 is full speed
#define TURN_SPEED 200
#define FORWARD_SPEED 200
#define REVERSE_DURATION 300 // ms
#define TURN_DURATION 500 // ms
//----- End of Editing Area -----

// Accelerometer Settings
#define RA_SIZE 3 // number of readings to include in running average of accelerometer readings
#define XY_ACCELERATION_THRESHOLD 2400 // for detection of contact (~16000 = magnitude of acceleration due to gravity)

Zumo32U4LCD lcd;
Zumo32U4ButtonA buttonA;
Zumo32U4Buzzer buzzer;
Zumo32U4Motors motors;
Zumo32U4LineSensors lineSensors;
Zumo32U4ProximitySensors proxSensors;

#define NUM_SENSORS 3
unsigned int lineSensorValues[NUM_SENSORS];

unsigned long loop_start_time;
unsigned long last_turn_time;
#define MIN_DELAY_AFTER_TURN 1000 // ms = min delay before detecting contact event

boolean keep_moving = true;

// RunningAverage class
// based on RunningAverage library for Arduino
// source: http://playground.arduino.cc/Main/RunningAverage
template <typename T>
class RunningAverage
{
public:
RunningAverage(void);
RunningAverage(int);
~RunningAverage();
void clear();
void addValue(T);
T getAverage() const;
void fillValue(T, int);
protected:
int _size;
int _cnt;
int _idx;
T _sum;
T * _ar;
static T zero;
};

// Accelerometer Class -- extends the LSM303 class to support reading and averaging the x-y acceleration
// vectors from the onboard LSM303DLHC accelerometer/magnetometer
class Accelerometer : public LSM303
{
typedef struct acc_data_xy
{
unsigned long timestamp;
int x;
int y;
float dir;
} acc_data_xy;

public:
Accelerometer() : ra_x(RA_SIZE), ra_y(RA_SIZE) {};
~Accelerometer() {};
void enable(void);
void getLogHeader(void);
void readAcceleration(unsigned long timestamp);
float len_xy() const;
float dir_xy() const;
int x_avg(void) const;
int y_avg(void) const;
long ss_xy_avg(void) const;
float dir_xy_avg(void) const;
private:
acc_data_xy last;
RunningAverage<int> ra_x;
RunningAverage<int> ra_y;
};

Accelerometer lsm303;

void waitForButtonAndCountDown()
{
ledYellow(1);
lcd.clear();
lcd.print(F("Press A"));

buttonA.waitForButton();

ledYellow(0);
lcd.clear();

// Play audible countdown.
for (int i = 0; i < 3; i++)
{
delay(1000);
buzzer.playNote(NOTE_G(3), 200, 15);
}
delay(1000);
buzzer.playNote(NOTE_G(4), 500, 15);
delay(1000);

last_turn_time = millis();
}

void setup()
{
// Uncomment if necessary to correct motor directions:
//motors.flipLeftMotor(true);
//motors.flipRightMotor(true);

//Initial two groups of sensors
lineSensors.initThreeSensors();
proxSensors.initThreeSensors();

// Initialize the Wire library and join the I2C bus as a master
Wire.begin();
Serial.begin(9600);

// Initialize LSM303
lsm303.init();
lsm303.enable();

waitForButtonAndCountDown();
}

void loop()
{
if (buttonA.isPressed())
{
// If button is pressed, stop and wait for another press to go again.
motors.setSpeeds(0, 0);
buttonA.waitForRelease();
waitForButtonAndCountDown();
}


//----- Editing Area -----
//Listening from Python
if(Serial.read() == '1'){
keep_moving = false; //Stop move if get a message from Python
}
//----- End of Editing Area -----

loop_start_time = millis();
lsm303.readAcceleration(loop_start_time);

proxSensors.read();
lineSensors.read(lineSensorValues);

//----- Editing Area -----
uint8_t sum[4] = {
proxSensors.countsFrontWithRightLeds() + proxSensors.countsFrontWithLeftLeds(),
proxSensors.countsLeftWithLeftLeds() + proxSensors.countsFrontWithLeftLeds(),
proxSensors.countsRightWithRightLeds() + proxSensors.countsFrontWithRightLeds(),
proxSensors.countsFrontWithRightLeds() + proxSensors.countsRightWithRightLeds()
};

//If the robot sees a line or an obstacle, make a turn
//Put line and infraed sensors in separate statements
//so we can assign different behaviors in the future
if(lineSensorValues[0] < QTR_THRESHOLD || lineSensorValues[NUM_SENSORS - 1] < QTR_THRESHOLD){
turn();
}
else if (sum[0] > DIST_THRESHOLD || sum[1] > DIST_THRESHOLD || sum[2] > DIST_THRESHOLD || sum[3] > DIST_THRESHOLD)
{
turn();
}


// Otherwise, go straight.
else{
if(keep_moving)
{
if(check_for_contact()) turn();
motors.setSpeeds(FORWARD_SPEED, FORWARD_SPEED);
}

else if(!keep_moving){
motors.setSpeeds(200, -200);
ledRed(1);
ledYellow(1);
delay(5000);
keep_moving = true;
}
}
}

void turn(){
motors.setSpeeds(-REVERSE_SPEED, -REVERSE_SPEED);
delay(REVERSE_DURATION);
motors.setSpeeds(-TURN_SPEED, TURN_SPEED);
delay(TURN_DURATION);
last_turn_time = millis();
}
//----- End of Editing Area -----

// check for contact, but ignore readings immediately after turning or losing contact
bool check_for_contact()
{
static long threshold_squared = (long) XY_ACCELERATION_THRESHOLD * (long) XY_ACCELERATION_THRESHOLD;
return (lsm303.ss_xy_avg() > threshold_squared) && (loop_start_time - last_turn_time > MIN_DELAY_AFTER_TURN);
}

// class Accelerometer -- member function definitions

// enable accelerometer only
// to enable both accelerometer and magnetometer, call enableDefault() instead
void Accelerometer::enable(void)
{
// Enable Accelerometer
// 0x27 = 0b00100111
// Normal power mode, all axes enabled
writeAccReg(LSM303::CTRL_REG1_A, 0x27);

if (getDeviceType() == LSM303::device_DLHC)
writeAccReg(LSM303::CTRL_REG4_A, 0x08); // DLHC: enable high resolution mode
}

void Accelerometer::getLogHeader(void)
{
Serial.print("millis x y len dir | len_avg dir_avg | avg_len");
Serial.println();
}

void Accelerometer::readAcceleration(unsigned long timestamp)
{
readAcc();
if (a.x == last.x && a.y == last.y) return;

last.timestamp = timestamp;
last.x = a.x;
last.y = a.y;

ra_x.addValue(last.x);
ra_y.addValue(last.y);
}

float Accelerometer::len_xy() const
{
return sqrt(last.x*a.x + last.y*a.y);
}

float Accelerometer::dir_xy() const
{
return atan2(last.x, last.y) * 180.0 / M_PI;
}

int Accelerometer::x_avg(void) const
{
return ra_x.getAverage();
}

int Accelerometer::y_avg(void) const
{
return ra_y.getAverage();
}

long Accelerometer::ss_xy_avg(void) const
{
long x_avg_long = static_cast<long>(x_avg());
long y_avg_long = static_cast<long>(y_avg());
return x_avg_long*x_avg_long + y_avg_long*y_avg_long;
}

float Accelerometer::dir_xy_avg(void) const
{
return atan2(static_cast<float>(x_avg()), static_cast<float>(y_avg())) * 180.0 / M_PI;
}



// RunningAverage class
// based on RunningAverage library for Arduino
// source: http://playground.arduino.cc/Main/RunningAverage
// author: Rob.Tillart@gmail.com
// Released to the public domain

template <typename T>
T RunningAverage<T>::zero = static_cast<T>(0);

template <typename T>
RunningAverage<T>::RunningAverage(int n)
{
_size = n;
_ar = (T*) malloc(_size * sizeof(T));
clear();
}

template <typename T>
RunningAverage<T>::~RunningAverage()
{
free(_ar);
}

// resets all counters
template <typename T>
void RunningAverage<T>::clear()
{
_cnt = 0;
_idx = 0;
_sum = zero;
for (int i = 0; i< _size; i++) _ar = zero; // needed to keep addValue simple
}

// adds a new value to the data-set
template <typename T>
void RunningAverage<T>::addValue(T f)
{
_sum -= _ar[_idx];
_ar[_idx] = f;
_sum += _ar[_idx];
_idx++;
if (_idx == _size) _idx = 0; // faster than %
if (_cnt < _size) _cnt++;
}

// returns the average of the data-set added so far
template <typename T>
T RunningAverage<T>::getAverage() const
{
if (_cnt == 0) return zero; // NaN ? math.h
return _sum / _cnt;
}

// fill the average with a value
// the param number determines how often value is added (weight)
// number should preferably be between 1 and size
template <typename T>
void RunningAverage<T>::fillValue(T value, int number)
{
clear();
for (int i = 0; i < number; i++)
{
addValue(value);
}
}
--------------------------------------------------------------------------------------------------------------------------------------------

https://youtu.be/JNQgzDptDeM

At the MAT open house on Friday, our robot successfully spun in a circle upon finding a face. This was a humanizing and endearing variation to the normal face detection process. After completing its spin, it adjusts back to its face detection search. This allows for viewers that do not have access to the control computer with the terminal log/console to recognize when the robot has actually run a successful search and found a face.

Last edited by taylormoon2014 on Mon May 22, 2017 10:35 am, edited 2 times in total.

xinghan_liu
Posts: 4
Joined: Wed Apr 12, 2017 5:48 pm

Group #1

Post by xinghan_liu » Mon May 22, 2017 9:25 am

How will it use:
. Forward, and backward movement
. Left and right direction
. Speed
. When will it stop
. Will it use Pan and Tilt? How?
. What will it do when it sees a face?

It will make a sound once it sees a face, and light would be turned on as a notice.
And once it closes to the borderline, the light would be turned up as well.

During the making process, we figured out how to use arduino to create music. Instead of using some music package resources online, we have created our own music which includes beats and melody.


Here's the code:
#include <avr/pgmspace.h>
#include <Wire.h>
#include <Zumo32U4.h>

#define DIST_THRESHOLD 9 // brightness level, for infraed sensors
#define QTR_THRESHOLD 500 // microseconds, for line sensors

//----- Editing Area -----
#define REVERSE_SPEED 100 // 0 is stopped, 400 is full speed
#define TURN_SPEED 200
#define FORWARD_SPEED 200
#define REVERSE_DURATION 300 // ms
#define TURN_DURATION 500 // ms
//----- End of Editing Area -----

// Accelerometer Settings
#define RA_SIZE 3 // number of readings to include in running average of accelerometer readings
#define XY_ACCELERATION_THRESHOLD 2400 // for detection of contact (~16000 = magnitude of acceleration due to gravity)

Zumo32U4LCD lcd;
Zumo32U4ButtonA buttonA;
Zumo32U4Buzzer buzzer;
Zumo32U4Motors motors;
Zumo32U4LineSensors lineSensors;
Zumo32U4ProximitySensors proxSensors;

#define NUM_SENSORS 3
unsigned int lineSensorValues[NUM_SENSORS];

unsigned long loop_start_time;
unsigned long last_turn_time;
#define MIN_DELAY_AFTER_TURN 1000 // ms = min delay before detecting contact event

boolean keep_moving = true;

// RunningAverage class
// based on RunningAverage library for Arduino
// source: http://playground.arduino.cc/Main/RunningAverage
template <typename T>
class RunningAverage
{
public:
RunningAverage(void);
RunningAverage(int);
~RunningAverage();
void clear();
void addValue(T);
T getAverage() const;
void fillValue(T, int);
protected:
int _size;
int _cnt;
int _idx;
T _sum;
T * _ar;
static T zero;
};

// Accelerometer Class -- extends the LSM303 class to support reading and averaging the x-y acceleration
// vectors from the onboard LSM303DLHC accelerometer/magnetometer
class Accelerometer : public LSM303
{
typedef struct acc_data_xy
{
unsigned long timestamp;
int x;
int y;
float dir;
} acc_data_xy;

public:
Accelerometer() : ra_x(RA_SIZE), ra_y(RA_SIZE) {};
~Accelerometer() {};
void enable(void);
void getLogHeader(void);
void readAcceleration(unsigned long timestamp);
float len_xy() const;
float dir_xy() const;
int x_avg(void) const;
int y_avg(void) const;
long ss_xy_avg(void) const;
float dir_xy_avg(void) const;
private:
acc_data_xy last;
RunningAverage<int> ra_x;
RunningAverage<int> ra_y;
};

Accelerometer lsm303;

void waitForButtonAndCountDown()
{
ledYellow(1);
lcd.clear();
lcd.print(F("Press A"));

buttonA.waitForButton();

ledYellow(0);
lcd.clear();

// Play audible countdown.
for (int i = 0; i < 3; i++)
{
delay(1000);
buzzer.playNote(NOTE_G(3), 200, 15);
}
delay(1000);
buzzer.playNote(NOTE_G(4), 500, 15);
delay(1000);

last_turn_time = millis();
}


const char fugue[] PROGMEM =
"! O5 L16 agafaea dac+adaea fa<aa<bac#a dac#adaea f"
"O6 dcd<b-d<ad<g d<f+d<gd<ad<b- d<dd<ed<f+d<g d<f+d<gd<ad"
"L8 MS <b-d<b-d MLe-<ge-<g MSc<ac<a ML d<fd<f O5 MS b-gb-g"
"ML >c#e>c#e MS afaf ML gc#gc# MS fdfd ML e<b-e<b-"
"O6 L16ragafaea dac#adaea fa<aa<bac#a dac#adaea faeadaca"
"<b-acadg<b-g egdgcg<b-g <ag<b-gcf<af dfcf<b-f<af"
"<gf<af<b-e<ge c#e<b-e<ae<ge <fe<ge<ad<fd"
"O5 e>ee>ef>df>d b->c#b->c#a>df>d e>ee>ef>df>d"
"e>d>c#>db>d>c#b >c#agaegfe f O6 dc#dfdc#<b c#4";


void setup()
{
// Uncomment if necessary to correct motor directions:
//motors.flipLeftMotor(true);
//motors.flipRightMotor(true);

//Initial two groups of sensors
lineSensors.initThreeSensors();
proxSensors.initThreeSensors();

// Initialize the Wire library and join the I2C bus as a master
Wire.begin();
Serial.begin(9600);

// Initialize LSM303
lsm303.init();
lsm303.enable();

waitForButtonAndCountDown();
}

void loop()
{
if (buttonA.isPressed())
{
// If button is pressed, stop and wait for another press to go again.
motors.setSpeeds(0, 0);
buttonA.waitForRelease();
waitForButtonAndCountDown();
}


//----- Editing Area -----
//Listening from Python
if(Serial.read() == '1'){
keep_moving = false; //Stop move if get a message from Python

{
// Start playing a tone with frequency 440 Hz at maximum
// volume (15) for 200 milliseconds.
buzzer.playFrequency(440, 1000, 15);

// Delay to give the tone time to finish.
delay(1000);

buzzer.playFrequency(392, 400, 14);

// Delay to give the tone time to finish.
delay(300);
buzzer.playFrequency(349, 400, 14);

// Delay to give the tone time to finish.
delay(300);
buzzer.playFrequency(392, 400, 14);

// Delay to give the tone time to finish.
delay(300);
buzzer.playFrequency(523, 400, 15);

// Delay to give the tone time to finish.
delay(300);

buzzer.playFrequency(440, 1000, 15);

// Delay to give the tone time to finish.
delay(700);


buzzer.stopPlaying();


}



}
//----- End of Editing Area -----

loop_start_time = millis();
lsm303.readAcceleration(loop_start_time);

proxSensors.read();
lineSensors.read(lineSensorValues);

//----- Editing Area -----
uint8_t sum[4] = {
proxSensors.countsFrontWithRightLeds() + proxSensors.countsFrontWithLeftLeds(),
proxSensors.countsLeftWithLeftLeds() + proxSensors.countsFrontWithLeftLeds(),
proxSensors.countsRightWithRightLeds() + proxSensors.countsFrontWithRightLeds(),
proxSensors.countsFrontWithRightLeds() + proxSensors.countsRightWithRightLeds()
};

//If the robot sees a line or an obstacle, make a turn
//Put line and infraed sensors in separate statements
//so we can assign different behaviors in the future
if(lineSensorValues[0] < QTR_THRESHOLD || lineSensorValues[NUM_SENSORS - 1] < QTR_THRESHOLD){


// Turn the LEDs on.
ledRed(1);
ledYellow(1);
ledGreen(1);

// Wait for a second.
delay(100);

// Turn the LEDs off.
ledRed(0);
ledYellow(0);
ledGreen(0);

// Wait for a second.
delay(100);


turn();




}
else if (sum[0] > DIST_THRESHOLD || sum[1] > DIST_THRESHOLD || sum[2] > DIST_THRESHOLD || sum[3] > DIST_THRESHOLD)
{
turn();
}


// Otherwise, go straight.
else{
if(keep_moving)
{
if(check_for_contact()) turn();
motors.setSpeeds(FORWARD_SPEED, FORWARD_SPEED);
}

else if(!keep_moving){
motors.setSpeeds(0, 0);
delay(5000);
keep_moving = true;
}
}
}

void turn(){
motors.setSpeeds(-REVERSE_SPEED, -REVERSE_SPEED);
delay(REVERSE_DURATION);
motors.setSpeeds(-TURN_SPEED, TURN_SPEED);
delay(TURN_DURATION);
last_turn_time = millis();
}
//----- End of Editing Area -----

// check for contact, but ignore readings immediately after turning or losing contact
bool check_for_contact()
{
static long threshold_squared = (long) XY_ACCELERATION_THRESHOLD * (long) XY_ACCELERATION_THRESHOLD;
return (lsm303.ss_xy_avg() > threshold_squared) && (loop_start_time - last_turn_time > MIN_DELAY_AFTER_TURN);
}

// class Accelerometer -- member function definitions

// enable accelerometer only
// to enable both accelerometer and magnetometer, call enableDefault() instead
void Accelerometer::enable(void)
{
// Enable Accelerometer
// 0x27 = 0b00100111
// Normal power mode, all axes enabled
writeAccReg(LSM303::CTRL_REG1_A, 0x27);

if (getDeviceType() == LSM303::device_DLHC)
writeAccReg(LSM303::CTRL_REG4_A, 0x08); // DLHC: enable high resolution mode
}

void Accelerometer::getLogHeader(void)
{
Serial.print("millis x y len dir | len_avg dir_avg | avg_len");
Serial.println();
}

void Accelerometer::readAcceleration(unsigned long timestamp)
{
readAcc();
if (a.x == last.x && a.y == last.y) return;

last.timestamp = timestamp;
last.x = a.x;
last.y = a.y;

ra_x.addValue(last.x);
ra_y.addValue(last.y);
}

float Accelerometer::len_xy() const
{
return sqrt(last.x*a.x + last.y*a.y);
}

float Accelerometer::dir_xy() const
{
return atan2(last.x, last.y) * 180.0 / M_PI;
}

int Accelerometer::x_avg(void) const
{
return ra_x.getAverage();
}

int Accelerometer::y_avg(void) const
{
return ra_y.getAverage();
}

long Accelerometer::ss_xy_avg(void) const
{
long x_avg_long = static_cast<long>(x_avg());
long y_avg_long = static_cast<long>(y_avg());
return x_avg_long*x_avg_long + y_avg_long*y_avg_long;
}

float Accelerometer::dir_xy_avg(void) const
{
return atan2(static_cast<float>(x_avg()), static_cast<float>(y_avg())) * 180.0 / M_PI;
}



// RunningAverage class
// based on RunningAverage library for Arduino
// source: http://playground.arduino.cc/Main/RunningAverage
// author: Rob.Tillart@gmail.com
// Released to the public domain

template <typename T>
T RunningAverage<T>::zero = static_cast<T>(0);

template <typename T>
RunningAverage<T>::RunningAverage(int n)
{
_size = n;
_ar = (T*) malloc(_size * sizeof(T));
clear();
}

template <typename T>
RunningAverage<T>::~RunningAverage()
{
free(_ar);
}

// resets all counters
template <typename T>
void RunningAverage<T>::clear()
{
_cnt = 0;
_idx = 0;
_sum = zero;
for (int i = 0; i< _size; i++) _ar = zero; // needed to keep addValue simple
}

// adds a new value to the data-set
template <typename T>
void RunningAverage<T>::addValue(T f)
{
_sum -= _ar[_idx];
_ar[_idx] = f;
_sum += _ar[_idx];
_idx++;
if (_idx == _size) _idx = 0; // faster than %
if (_cnt < _size) _cnt++;
}

// returns the average of the data-set added so far
template <typename T>
T RunningAverage<T>::getAverage() const
{
if (_cnt == 0) return zero; // NaN ? math.h
return _sum / _cnt;
}

// fill the average with a value
// the param number determines how often value is added (weight)
// number should preferably be between 1 and size
template <typename T>
void RunningAverage<T>::fillValue(T value, int number)
{
clear();
for (int i = 0; i < number; i++)
{
addValue(value);
}
}

Post Reply