Project 16: Arduino Smart 5 DOF Robot Arm
Main Ideas:
In this project we have built 5 DOF Robot Arm, and we have used the Ultra
Sonic HC-SR04 Sensor connected to the robot Arm to be used
for object detecting, so it will check if we but something near to robot arm (i.e.
less than 7 cm), then it will open the arm claw and it will wait for us to give
the sample, then robot claw will be closed. On the top of the robot arm we have
fixed TCS3200 color sensor to detect the color of the sample object and based
on the detected color the robot Arm will move the sample to the right sorting place,
i.e. it will move the red samples to red cup and green samples to green cup and
so on.
Also we have used IR Remote to define the mode of the robot arm, as in
the future we are planning to improve the robot arm to do different functions beside
the object detecting and color sorting, so we can press on the IR remote
buttons to choose the mode for the robot arm.
Circuit
·
Ultra Sonic HC-SR04 Sensor
·
TCS3200 color
sensor
·
5 MG995 Servo Motors
·
Arduino UNO
·
IR Remote
·
Ultra Sonic
HC-SR04 Sensor
·
9 Volt battery
Code
/*********
https://arduino4everyones.blogspot.com/
Color
ranges 2 cm till 5 cm
Main Ideas:
The TCS3200
color sensor can detect a wide range of colors based on their wavelength.
This sensor is
can be used for color recognition/detection projects such as color matching,
color sorting.
The sensor has
four different types of filter covered diodes. In the 8 x 8 array of
photodiodes,
16 photodiodes
have Red filters, 16 have Blue filters, 16 have Green filters and the rest 16
photodiodes are
clear with no
filters. Each type can be activated using the S2, S3 selection inputs.
*********/
#include <VarSpeedServo.h>
#include <Ultrasonic.h>
#include <IRremote.h>
decode_results results;
//hand down initial_postion 135
VarSpeedServo myservoarmdown1;
//hand down initial_postion 135
VarSpeedServo myservoarmdown2;
//hand up initial_postion 65
//pos=0 up top , pos=140 down end , p0s=65 stright
VarSpeedServo myservoarmup;
//base Servo initial_postion 90
VarSpeedServo myservoBase;
//claw Servo initial_postion 90
//pos=100s close , pos=45 half open , pos=10 full open
VarSpeedServo myservoclaw;
//Hand Rotate Servo initial_postion 90
//pos=90 stright , pos=0 trun right , pos=180 trun
left
VarSpeedServo myservohandrotate;
const int servoarmdown1 = 3; // the digital pin used for the servo
const int servoarmdown2 = 5; // the digital pin used for the servo
const int servoarmupPin = 6; // the digital pin used for the servo
const int servoBasePin = 11; // the digital pin used for the servo
const int servoclawPin = 10; // the digital pin used for the servo
const int servohandrotatPin = 9; // the digital pin used for the servo
// sequences are defined as an array of points in the
sequence
// each point has a position from 0 - 180, and a speed
to get to that position
servoSequencePoint slow[] = {{180, 20}, {90, 20}, {0,
50}}; // go to position 100 at speed of 20, position 20 speed 20, position 60,
speed 50
servoSequencePoint twitchy[] = {{0, 255}, {180, 40},
{90, 127}, {120, 60}};
//use this flag to contol if move down is done becasue
run the servo background will send the instraction to go up even before we go
down
int movedowndone;
//Ultrasonic ultrasonic(12, 13); // (Trig PIN,Echo
PIN)
Ultrasonic ultrasonic(12, 13, 10000UL);
// TCS230 or TCS3200 pins wiring to Arduino
#define S0 A0
// perpule
#define S1 A1
//gray
#define S2 A2
//white
#define S3 A3 //yellow
#define sensorOut A4 //green
// Capture frequencies read by the TCS230/TCS3200
Sensor
int redFrequency = 0;
int greenFrequency = 0;
int blueFrequency = 0;
//Define the colors
//red color
int redColor = 0;
//green color
int greenColor = 0;
//blue color
int blueColor = 0;
const int RECV_PIN = 7;
IRrecv irrecv(RECV_PIN);
unsigned long key_value = 0;
//clawstatus=1 close initial
//clawstatus=2 open based on ultrasonic
//clawstatus=3 cloase after ultrasonic open
int clawstatus;
int distnaceCM;
//mode=1 smart move 1 , move based on ultrasonic
int mode ;
void setup() {
Serial.begin(9600);
distnaceCM =
0;
clawstatus =
1;
mode = 1;
irrecv.enableIRIn();
// Setting the
outputs
pinMode(S0,
OUTPUT);
pinMode(S1,
OUTPUT);
pinMode(S2,
OUTPUT);
pinMode(S3,
OUTPUT);
// Setting the
sensorOut as an input
pinMode(sensorOut, INPUT);
// Setting
frequency scaling to 20%
digitalWrite(S0, HIGH);
digitalWrite(S1, LOW);
//set initial
postion for the base
basemoveaction();
}
void smart_move1()
{
// we will use
ultarsonic to check if hand near then open and wait for the object
scan ();
//clawstatus=1
close initial
//clawstatus=2
open based on ultrasonic
//clawstatus=3
cloase after ultrasonic open
//clawstatus=4
open after color detection
if (distnaceCM
<= 10 && (clawstatus == 1 || clawstatus == 4))
{
Serial.print(distnaceCM);
Serial.println(" cm" );
int z;
//less half
open
z = clawmove
(60, 2);
delay
(1000);
clawstatus =
2;
//close
z = clawmove
(110, 2);
clawstatus =
3;
delay
(1000);
clawstatus =
3;
color_detection();
}
}
void smart_move()
{
int z;
//less half
open
z = clawmove
(60, 2);
delay (1000);
myservoarmdown1.attach(servoarmdown1);
myservoarmdown2.attach(servoarmdown2);
//move down ,
run normal , using true
//with speed
of 20
myservoarmdown1.write(80, 20, true);
myservoarmdown2.write(80, 20, true);
delay (1000);
//close
z = clawmove
(110, 2);
delay (1000);
//move up , run normal , using true
//with speed
of 255
myservoarmdown2.write(145, 255, true);
myservoarmdown1.write(145, 255, true);
delay (2000);
}
int clawmove (int posm, int directiona)
{
// function
declaration
myservoclaw.attach(servoclawPin);
if (directiona
== 1)
{
//close =90
myservoclaw.write(posm, 150, true);
delay (500);
myservoclaw.detach();
}
if (directiona
== 2)
{
//halfopen
=45
myservoclaw.write(posm, 150, true);
delay (500);
myservoclaw.detach();
}
if (directiona
== 3)
{
//halfopen
=0
myservoclaw.write(posm, 150, true);
delay (500);
myservoclaw.detach();
}
return 1; //
return the value
}
int handrotatewmove (int posm, int directiona)
{
// function
declaration
myservohandrotate.attach(servohandrotatPin);
if (directiona
== 1)
{
//stright
=90
myservohandrotate.write(posm, 150, true);
delay (500);
}
if (directiona
== 2)
{
//trun left
=45
myservohandrotate.write(posm, 150, true);
delay (500);
}
if (directiona
== 3)
{
//trun right
=o
myservohandrotate.write(posm, 150, true);
delay (500);
}
return 1; //
return the value
}
int basemove (int posm, int directiona)
{
// function
declaration
myservoBase.attach(servoBasePin);
if (directiona
== 1)
{
//front =90
myservoBase.write(posm, 70, true);
delay (500);
myservoBase.detach();
}
if (directiona
== 2)
{
//trun left
=180
myservoBase.write(posm, 70, true);
delay (500);
myservoBase.detach();
}
if (directiona
== 3)
{
//trun
right =0
myservoBase.write(posm, 70, true);
delay (500);
myservoBase.detach();
}
if (directiona
== 4)
{
//trun
right =30 deg
myservoBase.write(30, 70, true);
delay (500);
myservoBase.detach();
}
if (directiona
== 5)
{
//trun
right =50 deg
myservoBase.write(60, 70, true);
delay (500);
myservoBase.detach();
}
if (directiona
== 6)
{
//trun left
=150
myservoBase.write(120, 70, true);
delay (500);
myservoBase.detach();
}
if (directiona
== 7)
{
//trun left
=120
myservoBase.write(150, 70, true);
delay (500);
myservoBase.detach();
}
return 1; //
return the value
}
void clawopenmovebaseback()
{
myservoarmdown1.attach(servoarmdown1);
myservoarmdown2.attach(servoarmdown2);
int z;
//move down ,
run normal , using true
//with speed
of 20
myservoarmdown1.write(100, 20, true);
myservoarmdown2.write(100, 20, true);
delay (1000);
//less half
open
z = clawmove
(60, 2);
delay (1000);
//close
z = clawmove
(110, 2);
clawstatus =
4;
delay (1000);
//move up , run normal , using true
//with speed
of 255
myservoarmdown1.write(135,
255, true);
myservoarmdown2.write(135, 255, true);
delay (1000);
z = basemove
(90, 2);
delay (1000);
}
void basemoveaction()
{
int z;
//move front
z = basemove
(90, 2);
delay (1000);
// //trun right
// // z = basemove (0, 3);
// // trun left
// // z = basemove (180, 1);
//
// z = basemove (0, 4);
// delay (1000);
// z = basemove (0, 5);
// delay (1000);
// z = basemove (0, 3);
// delay (1000);
// z = basemove (0, 6);
// delay (1000);
// z = basemove (0, 7);
// delay (1000);
// z = basemove (180, 1);
// delay (1000);
// z = basemove (90, 2);
}
void handrotatewmoveaction()
{
int z;
//move stright
z =
handrotatewmove (90, 2);
//trun right
//z =
handrotatewmove (0, 3);
// trun left
//z =
handrotatewmove (180, 1);
}
void clawmoveaction()
{
int z;
//open half
z = clawmove
(45, 2);
//open full
z = clawmove
(10, 3);
// close
z = clawmove
(100, 1);
}
void move_armupdown()
{
myservoarmdown1.attach(servoarmdown1);
myservoarmdown2.attach(servoarmdown2);
//move down ,
run normal , using true
//with speed
of 20
myservoarmdown1.write(100, 20, true);
myservoarmdown2.write(100, 20, true);
delay (1000);
//move up , run normal , using true
//with speed
of 255
myservoarmdown1.write(135, 255, true);
myservoarmdown2.write(135, 255, true);
delay(2000);
}
void move_armupservo()
{
myservoarmup.attach(servoarmupPin);
myservoarmup.write(65, 50, true);
delay(1000);
}
void scan ()
{
distnaceCM =
ultrasonic.read(CM);
//Serial.print(distnaceCM); // CM or INC
//Serial.println(" cm" );
delay(100);
}
void color_detection()
{
// Set RED (R)
filtered photodiodes to be read
digitalWrite(S2, LOW);
digitalWrite(S3, LOW);
// Reading the
output frequency
redFrequency =
pulseIn(sensorOut, LOW);
// Remaping
the value of the RED (R) frequency from 0 to 255
// You must
replace with your own values. Here's an example:
// this range
for 2-3 cm distance
//redColor =
map(redFrequency, 137, 144, 255, 0);
redColor =
redFrequency;
// Printing
the RED (R) value
Serial.print(" R = ");
Serial.print(redColor);
delay(100);
// Setting
GREEN (G) filtered photodiodes to be read
digitalWrite(S2, HIGH);
digitalWrite(S3, HIGH);
// Reading the
output frequency
greenFrequency
= pulseIn(sensorOut, LOW);
// Remaping
the value of the GREEN (G) frequency from 0 to 255
// You must
replace with your own values. Here's an example:
// Printing
the GREEN (G) value
// this range
for 2-3 cm distance
//greenColor =
map(greenFrequency, 206, 212, 255, 0);
greenColor =
greenFrequency;
// Printing
the GREEN (G) value
Serial.print(" G = ");
Serial.print(greenColor);
delay(100);
// Setting
BLUE (B) filtered photodiodes to be read
digitalWrite(S2, LOW);
digitalWrite(S3, HIGH);
// Reading the
output frequency
blueFrequency
= pulseIn(sensorOut, LOW);
// Remaping
the value of the BLUE (B) frequency from 0 to 255
// You must
replace with your own values. Here's an example:
// this range
for 2-5 cm distance
//blueColor =
map(blueFrequency, 96, 102, 255, 0);
blueColor =
blueFrequency;
// Printing
the BLUE (B) value
Serial.print(" B = ");
Serial.print(blueColor);
Serial.println("");
delay(200);
int z ;
// Checks the
current detected color and prints
// a message
in the serial monitor
if (redColor
< greenColor && redColor < blueColor) {
Serial.println("****** Red color detected *****");
//trun right
z = basemove
(0, 3);
delay
(1000);
}
if (greenColor
< redColor && greenColor < blueColor) {
Serial.println("****** Green color detected ******");
z = basemove
(0, 5);
delay
(1000);
}
if (blueColor
< redColor && blueColor < greenColor) {
Serial.println("****** Blue color detected ******");
// trun left
z = basemove
(180, 1);
delay
(1000);
}
//clawstatus =
4;
delay(400);
clawopenmovebaseback();
//Serial.println("");
}
void check_mode()
{
if
(irrecv.decode(&results))
{
if
(results.value == 0XFFFFFFFF)
results.value = key_value;
switch
(results.value)
{
case
0xFFA25D:
Serial.println("CH-");
mode =
1;
Serial.println(mode);
break;
case
0xFF629D:
Serial.println("CH");
mode =
2;
Serial.println(mode);
break;
case
0xFFE21D:
Serial.println("CH+");
break;
case
0xFF22DD:
Serial.println("|<<");
break;
case
0xFF02FD:
Serial.println(">>|");
break ;
case
0xFFC23D:
break ;
case
0xFFE01F:
Serial.println("-");
break ;
case
0xFFA857:
Serial.println("+");
break ;
case
0xFF906F:
Serial.println("EQ");
break ;
case
0xFF6897:
Serial.println("0");
break ;
case
0xFF9867:
Serial.println("100+");
break ;
case
0xFFB04F:
Serial.println("200+");
break ;
case
0xFF30CF:
Serial.println("1");
break ;
case
0xFF18E7:
Serial.println("2");
break ;
case
0xFF7A85:
Serial.println("3");
break ;
case
0xFF10EF:
Serial.println("4");
break ;
case
0xFF38C7:
Serial.println("5");
break ;
case
0xFF5AA5:
Serial.println("6");
break ;
case
0xFF42BD:
Serial.println("7");
break ;
case
0xFF4AB5:
Serial.println("8");
break ;
case
0xFF52AD:
Serial.println("9");
break ;
}
key_value =
results.value;
irrecv.resume();
delay(100);
}
}
void loop() {
check_mode();
delay(100);
if (mode == 1)
{
smart_move1();
}
else if (mode
== 2)
{
smart_move();
}
}
Please provide whole details
ReplyDelete