Project – See, See

Get Arm here

Massive Arduino Code:

Note: this is for Arduino 0011’s servo library

————————————————————————————–

#include <Servo.h>

// base sweep range
int base_limit_low = 20;
int base_limit_high = 80;
int base_center = 50;

//shoulder sweep range
int shoulder_limit_low = 60; //foward
int shoulder_limit_high = 90; //back
int shoulder_center = 50;

//elbow sweep range
int elbow_limit_low = 60; //up
int elbow_limit_high = 90; //down
int elbow_center = 75;

//wrist sweep range
int wrist_limit_low = 10; //down
int wrist_limit_high = 90;//up
int wrist_center = 60;

//turn sweep range
int turn_limit_low = 30;  //CW
int turn_limit_high = 80; //CCW
int turn_center = 50;

//grip sweep range
int grip_close = 20; //close
int grip_open = 60; //open

//servo attatchments
Servo base; //analogue 0
Servo shoulder; //analogue 1
Servo elbow; //analogue 2
Servo wrist; //analogue 3
Servo turn; //analogue 4
Servo grip; //analogue 5

//joint positions
int base_pos;
int shoulder_pos;
int elbow_pos;
int wrist_pos;
int turn_pos;
int grip_pos;

//IR stuffs
int lowEnd = 90;
int IRsensor = 5;//analogue pin 5

//video stuffs
int cam1 = 1; //digital 1
int cam2 = 3; //digital 3
int cam3 = 5;  //digital 5
int flash = 250;//blink time
boolean coin = false; //flash counter

//misc
int increment = 1; //degree increment for the servos
int sweep_direction_base = 1; //directional tokens
int sweep_direction_shoulder = 1;
int sweep_direction_elbow = 1;
int sweep_direction_wrist = 1;
int sweep_direction_turn = 1;
int sweep_direction_grip = 1;
boolean reset = false; //reset token
int pulse1 = 30;
int pulse2 = 50;
int reset_time = 5000; //initial reset time
unsigned long time;

void setup()
{
base.attach(14); //analog pin 0
shoulder.attach(15); //analog pin 1
elbow.attach(16); //analog pin 2
wrist.attach(17); //analog pin 3
turn.attach(18); //analog pin 4
//grip.attach(19); //analog pin 5

base.setMaximumPulse(2200); //set max pulse
shoulder.setMaximumPulse(2200); //set max pulse
elbow.setMaximumPulse(2200); //set max pulse
wrist.setMaximumPulse(2200); //set max pulse
turn.setMaximumPulse(2200); //set max pulse
Serial.begin(9600);

//initialize joint positions
base_pos = base_center;
shoulder_pos = shoulder_center;
elbow_pos = elbow_center;
turn_pos = turn_center;
wrist_pos = wrist_center;
//grip_pos = grip_open;

//cameras
pinMode(cam1, OUTPUT); //arm cam
pinMode(cam2, OUTPUT);
pinMode(cam3, OUTPUT);
}

void loop()
{
time = millis();
if(reset == false)
{
while(time<reset_time)
{
reset_all();
time = millis();
}
reset = true;
}

int val = readVal(IRsensor);// read in IR
//Serial.println(val);
//sweep_all();

int i = time % flash;
Serial.println(i);

if(isValid(val)) //if in proximity
{
freezeALL();

if((time % 5) == 0) //mod for the flshing so it won’t interfere with the arm
{
Serial.println(“flash mode”);
flsh_mode();
}
}
else //not in proximity
{
digitalWrite(cam1, HIGH); //default camera setup
digitalWrite(cam2, LOW);
digitalWrite(cam3, LOW);

sweep_all(); //do the sweep with the arm
}
}

///////////////RESET FUNCTIONS
void reset_all()
{
base_reset();
//delay(250);
shoulder_reset();
// delay(250);
elbow_reset();
//delay(250);
wrist_reset();
// delay(250);
turn_reset();
//delay(250);
grip_reset();

delay(pulse1);
Servo::refresh();
}

void base_reset()
{
base.write(base_center);
Servo::refresh();
}

void shoulder_reset()
{
shoulder.write(shoulder_center);
Servo::refresh();
}

void elbow_reset()
{
elbow.write(elbow_center);
Servo::refresh();
}

void wrist_reset()
{
wrist.write(wrist_center);
Servo::refresh();
}

void turn_reset()
{
turn.write(turn_center);
Servo::refresh();
}

void grip_reset()
{
grip.write(grip_open);
Servo::refresh();
}

//////////// SWEEP FUNCTIONS
void sweep_all()
{
base_sweep();
delay(pulse1);
Servo::refresh();

shoulder_sweep();
delay(pulse1);
Servo::refresh();

elbow_sweep();
delay(pulse1);
Servo::refresh();

wrist_sweep();
delay(pulse1);
Servo::refresh();

turn_sweep();
delay(pulse1);
Servo::refresh();
}

void base_sweep()
{
if(base_pos <= base_limit_low)
{
sweep_direction_base = 1; //go east
//delay(pulse2);
}
if(base_pos >= base_limit_high)
{
sweep_direction_base = -1; //go west
//delay(pulse2);
}
if(sweep_direction_base == 1) //east
{
base_pos = base_pos+increment;
base.write(base_pos);
}
if(sweep_direction_base == -1) //west
{
base_pos = base_pos-increment;
base.write(base_pos);
}
}

void shoulder_sweep()
{
if(shoulder_pos <= shoulder_limit_low)
{
sweep_direction_shoulder = 1; //go up
//delay(pulse2);
}
if(shoulder_pos >= shoulder_limit_high)
{
sweep_direction_shoulder = -1; //go down
//delay(pulse2);
}
if(sweep_direction_shoulder == 1) //up
{
shoulder_pos = shoulder_pos+increment;
shoulder.write(shoulder_pos);
}
if(sweep_direction_shoulder == -1) //down
{
shoulder_pos = shoulder_pos-increment;
shoulder.write(shoulder_pos);
}
}

void elbow_sweep()
{
if(elbow_pos <= elbow_limit_low)
{
sweep_direction_elbow = 1; //go up
//delay(pulse2);
}
if(elbow_pos >= elbow_limit_high)
{
sweep_direction_elbow = -1; //go down
//delay(pulse2);
}
if(sweep_direction_elbow == 1) //up
{
elbow_pos = elbow_pos+increment;
elbow.write(elbow_pos);
}
if(sweep_direction_elbow == -1) //down
{
elbow_pos = elbow_pos-increment;
elbow.write(elbow_pos);
}
}

void wrist_sweep()
{
if(wrist_pos <= wrist_limit_low)
{
sweep_direction_wrist = 1; //go up
//delay(pulse2);
}
if(wrist_pos >= wrist_limit_high)
{
sweep_direction_wrist = -1; //go down
//delay(pulse2);
}
if(sweep_direction_wrist == 1) //up
{
wrist_pos = wrist_pos+increment;
wrist.write(wrist_pos);
}
if(sweep_direction_wrist == -1) //down
{
wrist_pos = wrist_pos-increment;
wrist.write(wrist_pos);
}
}

void turn_sweep()
{
if(turn_pos <= turn_limit_low)
{
sweep_direction_turn = 1; //go up
//delay(pulse2);
}
if(turn_pos >= turn_limit_high)
{
sweep_direction_turn = -1; //go down
//delay(pulse2);
}
if(sweep_direction_turn == 1) //up
{
turn_pos = turn_pos+increment;
turn.write(turn_pos);
}
if(sweep_direction_turn == -1) //down
{
turn_pos = turn_pos-increment;
turn.write(turn_pos);
}
}

//freeze arm at the current position
void freezeALL()
{
shoulder.write(shoulder_pos);
//Servo::refresh();
elbow.write(elbow_pos);
//Servo::refresh();
wrist.write(wrist_pos);
//Servo::refresh();
turn.write(turn_pos);
//Servo::refresh();
base.write(base_pos);
delay(pulse1);
Servo::refresh();
}

/////////////////////////////////IR stuff
int readVal(int sensorNum) //mass sampling to blur out noise
{
int val = analogRead(sensorNum);
val+= analogRead(sensorNum);
val+= analogRead(sensorNum);
val+= analogRead(sensorNum);
val+= analogRead(sensorNum);
val = val/5;
return(val);
}

boolean isValid(int input) //check to see if something is in proximity
{
if(input>lowEnd)
{
Serial.println(“true”);
return true;
}
else
{
Serial.println(“false”);
return false;
}
}

////////////////////// camera control
void flsh_mode()
{
if(coin == false) //all off
{
digitalWrite(cam1, LOW);
digitalWrite(cam2, LOW);
digitalWrite(cam3, LOW);
coin = true; //flip
Serial.println(“flash mode 1”);
}
else //all on
{
digitalWrite(cam1, HIGH);
digitalWrite(cam2, HIGH);
digitalWrite(cam3, HIGH);
coin = false; //flip
Serial.println(“flash mode 2”);
}
}

Advertisements

Leave a Reply

Please log in using one of these methods to post your comment:

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