Pursuits of Design and Robotics

The documentation of concept development and general processes

SoapBox Code

Leave a comment

Well, this code kicked my face. But I think I got it. 

Let’s start with the obvious information: I have a distance sensor, and I have a servo. When the distance provides some desired information, Soap will return a desired output. I start with smoothing, because I’m starting to think everything should always be smoothed, at least a little, for better results. After I smooth, I track. I track by putting the smoothed distance values in an array for 10 frames [so trackingArray.length = 10] which is no time at all, but better than tracking for seconds, meaning high tolerances, and thus lower reflexes. So the dog tracks your movement for 10 frames which is probably around a second. Then the dog runs some tests on this information. If you mass addition the differences between the distances from one frame to the next, the result will yield whether or not you are approaching the dog or distancing from it. This result also tells you, on average for the past 10 frames, how fast the movement was as well. 

Additionally there are factors such as slow movement while being close to the dog versus fast movement far away from the dog, and so on. My favorite aspect of this puppy, though, is that he knows if you are hitting him, and he gets upset with you. For every frame that the dog interprets your movements as a hit, he will become jaded and less friendly to your approach. If you patiently hover your hand slowly to him, however, he will grow to like you again.

I was pretty proud of this code for a first project. Priya and I faced a lot of struggles in dealing with the COM Port communicating while the servo was plugged in, which I learned today [also known as too late] was the fault of the cable I was using to load the code to the Arduino. Additionally, we also struggled heavily with the sound output which Priya took charge of and failed after I failed first. We still don’t know why that didn’t work. It worked on all of the example files, but not on ours. However I know the rest of the code works due to the most lovely Serial.println function.

So, all you need is a Servo and a distance sensor and this disastrous code will tell you what’s up.

#include <Servo.h>
//orange to ground
//red to 24
//green outputs 24
Servo arms;
Servo head;

int distThresh = 600;
int soundThresh = 600;

int distSensor = 0;
int microphone = 1;

int armsPin = 9;
int headPin = 10;

int const smootheFC = 3;
int thisSmooth = 0;
int smoother[smootheFC];
int average;
int total;

int const trackFC = 10;
int thisFrame = 0;
int trackMovement[trackFC];

int frameCounter= 0;
int timeToMove = 1000;
int averageGen = 0;

int const stateCount = 5;
int stateIndex = 0;
int states= 0;
int saveStates[stateCount];

void setup()
{
arms.attach(armsPin);
pinMode(7, OUTPUT);
pinMode(distSensor, INPUT);

Serial.begin(57600);

for(int i = 0; i < smootheFC; i++){
//smoothes
smoother[i] = 0;
}
}

void loop() {

frameCounter ++;
int distance = analogRead(distSensor);
averageGen = 0;

if (frameCounter == timeToMove){
// Serial.println(thisSmooth);

total = total – smoother[thisSmooth];
smoother[thisSmooth] = distance;
total = total + smoother[thisSmooth];
thisSmooth ++;

if (thisSmooth >= smootheFC)
{
// Serial.println(“thisSmooth = 0”);
thisSmooth = 0;
}

Serial.print(“[“);
for(int i = 0; i < trackFC-1; i++){

Serial.print(trackMovement[i]);
Serial.print(“, “);
}

Serial.println(“]”);

average = total / smootheFC;

for(int i = trackFC -1; i > 0; i–)
{
trackMovement[i] = trackMovement[i-1];
trackMovement[0] = average;
}

for(int i = 0; i < trackFC-1; i++) averageGen += (trackMovement[i+1] – trackMovement[i]);

int score = 0;
boolean sameState = false;

for(int i = 0; i < stateCount-1; i++)
{
if (saveStates[i] == saveStates[i-1]) score++;
if (score > 2) {
sameState = true;
Serial.println(“^” );
}
}

Serial.println(averageGen );
if ((averageGen <= -400) && (trackMovement[trackFC-1] != 0))
{
states = 4;
timeToMove = 2000;
Serial.println(“DON’T HIT ME!!!” );
arms.write(0);
}
else if (average < 200 && averageGen < 50 && averageGen > -50 )
{

states = 0;
timeToMove = 1000;
Serial.println(“Guess I’m alone!” );

arms.write(0);
}
else if (average > distThresh && averageGen < 50 && averageGen > -50)
{
states = 1;
timeToMove = 3000;
Serial.println(“PET ME!” );
arms.write(120);
}
else if ((averageGen <= 0) && (averageGen > -200))
{
states = 2;
timeToMove = 1000;
Serial.println(“you are getting closerclosercloser to me!” );
}
else if ((averageGen >= 0) && (averageGen < 200))
{
states = 3;
timeToMove = 2000;
Serial.println(“you are getting farther from me!”);
}
else if (averageGen >= 400)
{
states = 5;
timeToMove = 5000;
Serial.println(“WHERE DID YOU GO!?”);
}
Serial.write(states);

saveStates[stateIndex] = states;
stateIndex++;
if (stateIndex == stateCount) stateIndex = 0;

frameCounter = 0;
}
}

 

Advertisements

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s