The other thing affecting the consistency was the guiding rails. These were originally out of coat hanger but have now been replaced with hollow brass rods. These are a closer fit to the lego holes and hence give a good guide. It did have a bit of friction at the ends which was an issue but the timing belt solution overcame it.
Still, there wasn't enough resolution with my old 2 tab encoder wheels. So I have made 2 new encoder wheels with 8 tabs. This will provide much greater resolution for the coordinates but is yet to be attached. These are below:
The code has also been changed now neater (separated into tabs) and more efficient in its function. This is below:
Automated Gantry: This contains all the integers (data type for number storage) to tell the program which pins to reference on the eleven for each variable. It also outlines whether each variable is an INPUT or OUTPUT. It also contains general routines such as that to print out the location of the gantry, to move back to (0,0).
#include <Wire.h>
#include <Servo.h>
int Xdir=6; // drive pin to X direction relay
int Ydir=4; // drive pin to Y direction relay
int Xenable=5; // drive pin to X enable relay
int Yenable=10; // drive pin to Y enable relay
int Xpulse=3; // input from X encoder
int Ypulse=2; // input from Y encoder
int Solenoid = 9;
boolean
Xdirection=false; // true if motor
forward / false if reverse
boolean
Ydirection=false; // true if motor forward
/ false if reverse
Servo Lift;
int Xposition=0; // encoder count for X
int Yposition=0; // encoder count for Y
boolean Xtravel = false;
boolean Ytravel = false;
int OldXstate,
OldYstate; // input state of encoder
pins (HIGH,LOW)
String inputString =
"";
boolean
stringComplete=false;
String cmd = "";
int arg [4] = {0,0,0,0};
int ArgIndex;
void setup()
{
Serial.begin(9600);
Serial.setTimeout(2);
inputString.reserve(200);
Serial.println(" Automated Gantry
Warehouse");
pinMode(Xdir, OUTPUT);
pinMode(Ydir, OUTPUT);
pinMode(Xenable, OUTPUT);
pinMode(Yenable, OUTPUT);
pinMode(Solenoid,OUTPUT);
pinMode(Xpulse,INPUT);
pinMode(Ypulse,INPUT);
digitalWrite(Solenoid,LOW);
Lift.attach(7);
Lift.write(128);
OldXstate = digitalRead(Xpulse); // get state of encoder inputs
OldYstate = digitalRead(Ypulse); // ...
}
void loop(){
// serialEvent();
if (stringComplete){
HandleCommand();
inputString="";
stringComplete=false;
}
}
void Home(){
MoveTo(0,0);
}
void MoveTo(int x, int y){
// if ((x < 0) || (y < 0) || (x > 40)
|| (y > 40)){
// Serial.println("XY commands out of
bounds");
// return;
// }
Serial.println(y);
if (x !=Xposition){
if (x > Xposition) XForward();
if (x < Xposition) XReverse();
}
while (Xposition !=x){
CheckEncoders();
}
XStop();
ShowEncoders();
delay (1000);
// ShowEncoders();
// delay (2000);
if (y != Yposition){
if (y > Yposition) YForward();
if (y < Yposition) YReverse();
}
ShowEncoders();
while (y !=Yposition){
CheckEncoders();
}
YStop();
ShowEncoders();
}
void PickUp(){
MoveTo(arg[0], arg[1]);
}
Commands: This is the code for manual control of the robot. By inputting a command from the code in the serial monitor, it will reference the array of commands, and run the routine set to that command; movement of the x, movement of the y, on and off of electromagnet, driving the servo, going to a certain coordinate, resetting and reading the encoders to see the coordinate.
void HandleCommand(){
ParseInput();
if (cmd == "xf") XForward();
if (cmd == "xs") XStop();
if (cmd == "xr") XReverse();
if (cmd == "yf") YForward();
if (cmd == "ys") YStop();
if (cmd == "yr") YReverse();
if (cmd == "se") ShowEncoders();
if (cmd == "xy") PickUp();
if (cmd == "re") ResetEncoders();
if (cmd == "servo") DriveServo();
if (cmd == "solon") SolenoidOn();
if (cmd == "soloff") SolenoidOff();
if (cmd == "home") Home();
}
// from the serial input
separate all arguments
void ParseInput(){
Serial.println(inputString);
int space, slash;
for(int x = 0; x < 4; x++){ // flush the last args
arg[x] = 0;
}
ArgIndex = 0; // reset
number of args to zero
space=inputString.indexOf(' ');
slash = inputString.indexOf('\n');
inputString = inputString.substring(0,slash);
cmd = inputString.substring(0,space);
inputString =
inputString.substring(space+1,slash);
while (inputString.indexOf(' ') > 0){
space = inputString.indexOf(' ');
arg[ArgIndex] =
inputString.substring(0,space).toInt();
inputString =
inputString.substring(space+1,inputString.length());
ArgIndex++;
}
arg[ArgIndex] = inputString.toInt();
}
void serialEvent(){
// Serial.println("l");
while(Serial.available()){
// Serial.println("d");
char inChar=(char)Serial.read();
inputString += inChar;
// Serial.println(inputString);
if(inChar=='\n'){
stringComplete=true;
// Serial.println("complete");
}
}
}
Encoders: This
contains the code for reading the photo interrupters and then updating the
coordinates in accordance. This essentially runs the function of the location
system of the robot knows where it is. On top of this it contains the routines
for resetting the coordinates back to (0,0) and printing out the current
position.
The arduino reads the photo interrupter which depending on
the starting position of the encoder wheel attached to the motor (gap or
filled) will read the starting position HIGH or LOW. It will then continue to
read the photo interrupter until it detects a change. Once this change is
detected it must update the position of the axis. To determine whether to
decrement or increment the value, it will check the variable Xdirection (in
Boolean is false). It equals Xdirection (false) then the value of the X
coordinate will increase by 1. Otherwise if it does not equal Xdirection, it
will decrease by 1. It will then update the OldXstate so it knows where it is now.
This process continuously repeats at an extremely fast rate for the X axis and
Y axis to keep an update of the location of the gantry.
void CheckEncoders(){
int X,Y;
X = digitalRead(Xpulse);
Y = digitalRead(Ypulse);
// Serial.print(Xposition);
// Serial.print(" ");
// Serial.println(Yposition);
delay(200);
if (OldXstate == LOW){
if(X ==HIGH){
if (Xdirection) Xposition++;
if (!Xdirection) Xposition--;
OldXstate = X;
goto
CheckY;
}
}
if
(OldXstate == HIGH){
if (X == LOW){
if (Xdirection) Xposition++;
if (!Xdirection) Xposition--;
OldXstate = LOW;
}
}
CheckY:
if (OldYstate == LOW){
if(Y ==HIGH){
if (Ydirection) Yposition++;
if (!Ydirection) Yposition--;
OldYstate = Y;
//
Serial.println(Yposition);
return;
}
}
if (OldYstate == HIGH){
if (Y == LOW){
if (Ydirection) Yposition++;
if (!Ydirection) Yposition--;
OldYstate = LOW;
//
Serial.println(Yposition);
}
}
}
// used in serial to show
encoder counts
void ShowEncoders(){
Serial.print("X= ");
Serial.print(Xposition);
Serial.print( " Y= ");
Serial.println(Yposition);
}
void PrintPosition(){
Serial.print(Xposition);
Serial.print(" ");
Serial.println(Yposition);
}
void ResetEncoders(){
Xposition = 0;
Yposition = 0;
Serial.println("Encoders reset");
}
Motor Control:
This contains the routines for the driving of the x-axis and y-axis motors.
This includes forward movement, reverse movement, stopping of the motors. These
are referenced in an array and are initiated by the commands outlined in the
“commands” tab. When the motor is set at HIGH, it moves in a forward direction.
When LOW, it moves in reverse. When the enable is LOW, the motor is off. When
the enable is HIGH, the motor is on.
void SetXForward(){
digitalWrite(Xdir,HIGH);
Xdirection= true;
}
void SetXReverse(){
digitalWrite(Xdir,LOW);
Xdirection = false;
}
void XForward(){
Xdirection=true;
digitalWrite(Xenable,LOW);
delay(30);
digitalWrite(Xdir,HIGH);
digitalWrite(Xenable,HIGH);
}
void YForward(){
Ydirection=true;
digitalWrite(Yenable,LOW);
delay(30);
digitalWrite(Ydir,HIGH);
digitalWrite(Yenable,HIGH);
}
void XReverse(){
Xdirection=false;
digitalWrite(Xenable,LOW);
delay(30);
digitalWrite(Xdir,LOW);
digitalWrite(Xenable,HIGH);
}
void YReverse(){
Ydirection=false;
digitalWrite(Yenable,LOW);
delay(30);
digitalWrite(Ydir,LOW);
digitalWrite(Yenable,HIGH);
}
void XStop(){
digitalWrite(Xenable,LOW);
digitalWrite(Xdir,LOW);
Xtravel = false;
}
void YStop(){
digitalWrite(Yenable,LOW);
digitalWrite(Ydir,LOW);
Ytravel = false;
}
void Test(){
XForward();
delay(1250);
XStop();
delay(1000);
XReverse();
delay(1250);
XStop();
delay(1000);
YForward();
delay(500);
YStop();
delay(1000);
YReverse();
delay(500);
YStop();
delay(1000);
}
Servo: This
contains the routine for driving the servo to a specific angle. By doing so, it
will lower a string with the electromagnet attached to it. This will bring it low
enough to pick up an item. This code can function manually from the serial
monitor. When the command for the servo is called, and followed by a value, the
servo will drive to that angle.
void DriveServo(){
Lift.write(arg[0]);
}
Solenoid: This
contains the code for the on and off routines of the electromagnet. When it is
written HIGH, it is on. When it is written LOW, it is off.
void SolenoidOn(){
digitalWrite(Solenoid,HIGH);
}
void SolenoidOff(){
digitalWrite(Solenoid,LOW);
}
In this code new commands for manual control have also been added. They are now:
|
Command
|
Routine
|
Function
|
|
xf
|
XForward()
|
Move X-axis
forward
|
|
xs
|
XStop()
|
Stop X-axis
movement
|
|
xr
|
XReverse()
|
Move X-axis
backwards
|
|
yf
|
YForward()
|
Move Y-axis
forward
|
|
ys
|
YStop()
|
Stop Y-axis
movement
|
|
yr
|
YReverse()
|
Move Y-axis
backwards
|
|
se
|
ShowEncoders()
|
Print the
current coordinates (in the form “x” “y”)
|
|
re
|
ResetEncoders()
|
Reset the
coordinate value to 0,0
|
|
xy “ “ “ “
|
PickUp()
|
Move to the
written x (input value in first “ “) and y coordinate (input value in second “
“)
|
|
servo “ “
|
DriveServo()
|
Move servo to
position “ “. This lowers and raises the electromagnet (The Z-Axis)
IMPORTANT:
Only values between 60 (highest point) and 230 (lowest point) should be inputted.
|
|
solon
|
SolenoidOn()
|
Turn
electromagnet on.
|
|
soloff
|
SolenoidOff()
|
Turn
electromagnet off.
|
|
home
|
Home()
|
Move x and y
axis back to starting position.
|
All that is left now is the code for the item inventory, putting on a new electromagnet (as my other one broke), adding from limit switches and drilling some new holes to connect the robot to the base (it was slightly out of alignment before). The new electromagnet, resistors (10K) for the limit circuit circuit, and limit switches are below:
Pretty neat dude, I want more videos
ReplyDelete