The Code Behind The Wiggles

Mr. Wiggles uses three servos and two types of sensors to achieve the functionality it currently has. Two servos are used to control the movement, and one is used to hold a pen to draw. To avoid obstacles, an ultrasonic range finder is employed, and it consistently reads the distance between Mr Wiggles and the closest obstacle. Once the distance gets smaller than the preset safety value, it turns. The direction of the turning is decided by photocell readings, which allows Mr. Wiggles to always move towards the brighter side when turning.

The complete circuit is shown below, drawn using Fritzing.

bread

The complete code of Mr Wiggles 2.0 is attached.


// Code for Mr Wiggles 2.0
// Distance sensing component is modified 
// from code by Bruce Allen
#include Servo.h;

// Analog pin 1 for reading in the analog voltage 
// from the MaxSonar device.
// This variable is a constant because the pin will not change 
// throughout execution of this code.
const int anPin = 1;

// photocell pins and variables to record the readings
int photocellPinA = 2;
int photocellPinB = 3;
int photocellReadingA = 0;
int photocellReadingB = 0;

// initial status of Mr Wiggles
int turnLeft = 0;
int runWheels = 1;
int key=0;

// Leds are used to indicate the distance 
// to the nearest obstacle
int ledA = 10;
int ledB = 11;
int ledC = 12;

// two Servos for movement
Servo leftServo;
Servo rightServo;
// Servo for drawing
Servo drawServo;
// positions of left/right Servos
int posL = 0;
int posR = 0;
// stopping position of left/right Servos
int leftStop = 92;
int rightStop = 95;
// status of drawingServo
int timeUp=0;
int timeDown=0;
int penState=0;

// individual readings from ultrasonic sensors
long anVolt, cm;
//Create sum variable so it can be averaged
int sum=0;
// Quantity of values to average (sample size)
int avgrange = 20;
float inches = 0.00;

void setup() {
// This opens up a serial connection
// to shoot the results back to the PC console
Serial.begin(9600);
// drawServo setup
drawServo.attach(6);
penUp();
// left/right Servo setup
if(runWheels) {
rightServo.attach(8);
rightServo.write(rightStop);
leftServo.attach(7);
leftServo.write(leftStop);
}

// setting up pins
pinMode(anPin, INPUT);
pinMode(ledA, OUTPUT);
pinMode(ledB, OUTPUT);
pinMode(ledC, OUTPUT);

// initial LED status
digitalWrite(ledA, HIGH);
digitalWrite(ledB, HIGH);
digitalWrite(ledC, HIGH);
delay(200);

// if analog input pin 0 is unconnected, random analog
// noise will cause the call to randomSeed() to generate
// different seed numbers each time the sketch runs.
// randomSeed() will then shuffle the random function.
randomSeed(analogRead(0));
}

void loop() {

// obtaining distance reading
anVolt = analogRead(anPin)/2;
inches = max(5, (((avgrange-1)*inches) 
                    + anVolt) / avgrange);
cm = round(inches * 2.54);

digitalWrite(ledA, LOW);
digitalWrite(ledB, LOW);
digitalWrite(ledC, LOW);

// Obtaining photocell readings
photocellReadingA = analogRead(photocellPinA);
photocellReadingB = analogRead(photocellPinB);
// compare readings from two sides
// decide which way to turn
int direct= photocellReadingA-photocellReadingB;

// When cm > 200 run straight and fast
if(cm > 200) {

// normal speed
if(runWheels) {
leftWheelGo(50);
rightWheelGo(50);
}
// one LED lights up
digitalWrite(ledA, HIGH);

}
// When cm < 200, run one wheel slower, 
// proportional to cm
else if(cm >= 20) {

// start to turn
int lagSpeed = round(map(cm, 20, 200, -10, 50));

if(runWheels) {
if(turnLeft) {
leftWheelGo(lagSpeed);
rightWheelGo(50);
} else {
leftWheelGo(50);
rightWheelGo(lagSpeed);
}
// two LEDs light up
digitalWrite(ledB, HIGH);
}
}

// When cm < 20, spin around
else {
// spin!
if(runWheels) {
if(turnLeft) {
leftWheelGo(-10);
rightWheelGo(10);
} else {
leftWheelGo(10);
rightWheelGo(-10);
}
}
// at this stage, all three LEDs light up
digitalWrite(ledC, HIGH);
}

/*
if the pen is down add to timeDown
if the timeDown > cm the reset timeDown
        switch to penUp
else add to timeUp
if the timeUp >20 put the penDown
*/
if (penState){
if (++timeDown>cm){
penUp();
}
}
else{
if(++timeUp>20){
penDown();
}
}

}

// helper methods to control pen
void penUp(){
drawServo.write(93);
penState=0;
timeDown=0;
}

void penDown(){
drawServo.write(120);
penState=1;
timeUp=0;
}

// helper method to control left/right Servos
void leftWheelGo(int vel){
leftServo.write(leftStop + vel);
}

void rightWheelGo(int vel){
rightServo.write(rightStop - vel);
}
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