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);
}
}