Monday, 23 September 2013

Updating the Code and a progress video

A number of major changes have been made in the code. These have been necessary in order to fix the issue of interference. Shielded cable did nothing, neither did wire management so with the code updates, routines have been altered to fix interference.

The first fix was quite simple. Since the relays were the source of interference, when one axis moved, its caused false signals to be fed to the other photo interrupter. As a result the movement of one axis, would slightly change the coordinate of the other axis. Thus the update routine was given the command to not update whilst the other axis was running.

The second fix was more complex. I myself did not write this part but got help from others at the workshop. After probing the photo interrupter signal on an oscilloscope, we were able to see the signal wave and see how often it peaked abruptly. This was approximately 30 milliseconds, hence a debounce routine was run every 30 milliseconds in order to essentially reverse whatever abrupt false signal was read.

The code below reflects these changes which occurred in the first tab of my major code. There were other very tiny changes for example in the values of the servo for the up and down routines but I won't include these.

//Automated Gantry Warehousing Robot
#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;
int Xhome = 11;
int Yhome = 8;
volatile int Xstate=LOW;
volatile int Ystate=LOW;
volatile int xx = 0;
boolean Xdirection=false; // true if motor forward / false if reverse
boolean Ydirection=false; // true if motor forward / false if reverse
boolean Xmoving=false;
boolean Ymoving=false;
Servo Lift;
volatile int Xposition=0; // encoder count for X
volatile 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;
int BayX[4] = {6,17,28,39};
int BayY[3] = {6,25,39};
unsigned long millilog[200];
unsigned long EncoderCheck;
int mindex = 0;
void setup()
{
Serial.begin(9600);
Serial.setTimeout(2);
inputString.reserve(200);
Serial.println(" Automated Gantry Warehouse");
attachInterrupt(1,UpdateX,FALLING);
attachInterrupt(0,UpdateY,FALLING);
pinMode(Xdir, OUTPUT);
pinMode(Ydir, OUTPUT);
pinMode(Xenable, OUTPUT);
pinMode(Yenable, OUTPUT);
pinMode(Solenoid,OUTPUT);
// pinMode(Xpulse,INPUT);
// pinMode(Ypulse,INPUT);
pinMode(Xhome,INPUT);
pinMode(Yhome,INPUT);
digitalWrite(Solenoid,LOW);
Lift.attach(7);
Lift.write(128);
OldXstate = digitalRead(Xpulse); // get state of encoder inputs
OldYstate = digitalRead(Ypulse); // ...
ServoUp();
GoHome();
}
void loop(){
if (stringComplete){
HandleCommand();
inputString="";
stringComplete=false;
}
}
void Home(){
MoveTo(BayX[0],BayY[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(x);
//Serial.println(y);
//return;
if (x !=Xposition){
if (x > Xposition) XForward();
if (x < Xposition) XReverse();
}
if (Xdirection){
while(Xposition <x){}
}
if (!Xdirection){
while(Xposition > x){}
}
XStop();
ShowEncoders();
delay (1000);
if (y != Yposition){
if (y > Yposition) YForward();
if (y < Yposition) YReverse();
}
ShowEncoders();
if (Ydirection){
while(Yposition < y ){}
}
if (!Ydirection){
while(Yposition > y){}
}
YStop();
ShowEncoders();
}
void PickUp(){
if ((arg[0] > 3) || (arg[1] > 3)){
Serial.println("Invalid Bay Number");
return;
}
delay(100);
MoveTo(BayX[arg[0]], BayY[arg[1]]);
}
void GetFridge(){
if ((arg[0] > 3) || (arg[1] > 3)){
Serial.println("Invalid Bay Number");
return;
}
MoveTo(BayX[arg[0]],BayY[arg[1]]);
delay(400);
ServoDown();
delay(500);
SolenoidOn();
delay(400);
ServoUp();
delay(300);
MoveTo(BayX[0],BayY[0]);
delay(2000);
ServoDown();
delay(100);
SolenoidOff();
delay(500);
ServoUp();
}
void StoreFridge(){
if ((arg[0] > 3) || (arg[1] > 3)){
Serial.println("Invalid Bay Number");
return;
}
MoveTo(BayX[0],BayY[0]);
delay(300);
ServoDown();
delay(400);
SolenoidOn();
delay(300);
ServoUp();
delay(200);
MoveTo(BayX[arg[0]],BayY[arg[1]]);
delay(2000);
ServoDown();
delay(500);
SolenoidOff();
delay(300);
ServoUp();
}
void GoHome(){
if (digitalRead(Xhome) == HIGH) XReverse();
while(digitalRead(Xhome) == HIGH){}
XStop();
delay(500);
if (digitalRead(Yhome) == HIGH) YReverse();
while(digitalRead(Yhome) == HIGH){}
YStop();
ResetEncoders();
MoveTo(BayX[0],BayY[0]);
}
unsigned long last_X_update_time= 0;
unsigned long last_Y_update_time= 0;
int debounce_time = 30;
void UpdateX(){
if (!Xmoving) return;
if (abs(millis()-last_X_update_time)>debounce_time){
//Not too quick, count.
if (Xdirection) Xposition++;
if (!Xdirection) Xposition--;
last_X_update_time = millis();
}
// mindex++;
// millilog[mindex] = millis();
}
void UpdateY(){
if (!Ymoving) return;
if (abs(millis()-last_Y_update_time)>debounce_time){
if (Ydirection) Yposition++;
if (!Ydirection) Yposition--;
last_Y_update_time = millis();
}
}
Finally the long awaited video can be seen below. From it the difference from V1 to V2 is clear, particularly that this one actually works. Still there is much to fix and add. I'm thinking about a PCB to counter messy circuit boards and interference, buttons to make for a less repetitive and more efficient design, and a new mechanism for the Z axis that drops straight down rather than side on.





No comments:

Post a Comment