|
Page 1 of 1
|
[ 4 posts ] |
|
| Author |
Message |
|
brianchen11
Rookie
Joined: Tue Jul 03, 2012 5:16 am
Posts: 43
|
 scara robot issues I've made a scara robot. But now, the robot is very slow when searching for the next pixel to write. The current method is searching the pixels around the current pixel, then moving the arm to the pixel, the searching for the next pixel... This takes a lot more time than I thout it would (about 0.5 secs) to find the next pixel and start to move the arm. This results in the arm not moving slowly.If anyone has any suggestions or code, please reply.
|
| Sun Sep 23, 2012 8:14 pm |
|
|
|
JohnWatson
Site Admin
Joined: Thu May 24, 2012 12:15 pm
Posts: 722
|
Re: scara robot issues If you post the code on here (using the [code] blocks) I'd be more than happy to take a look through it for you and see if we can't determine what is causing the delays.
_________________Check out our Blog! And our Facebook page! Need help? Take a look at our updated help documentation and the ROBOTC Forums.
|
| Sun Sep 23, 2012 8:16 pm |
|
|
|
brianchen11
Rookie
Joined: Tue Jul 03, 2012 5:16 am
Posts: 43
|
Re: scara robot issues Quick reply! This is the header file for the functions (sorry, it's not very neat and well commented)  | | | | Code: const int speed = 70;
const int mm = 1;
const int arm1 = 15*63/8*mm, //Length of arm1 in cm
arm2 = 14*63/8*mm, //Length of arm2 in cm
arm1R = 56, //Gear ratios
arm2R = 15;
int asign1,asign2;
float s1,s2;
float t;
long a1,a2;
long target1,target2;
long encoderBBegin = (arm1R*90),
encoderCBegin = (arm2R*180);
int xMax = 0;
int yMax = 0;
byte coordinates[91][61]; // Up to 100x100 no more flash after that
int count=0;
float dire=0;
void moveToAngle(float angle1, float angle2){
target1 = angle1 * arm1R - encoderBBegin;
a1 = target1;
target2 = angle2 * arm2R - encoderCBegin;
a2 = target2;
// find t (time in s)
asign1=1;
asign2=1;
if(a1<0){
a1*=-1;
asign1 = -1;
}
if(a2<0){
a2*=-1;
asign2 = -1;
}
if(a1>a2){
t=a1/speed;
}
else{
t=a2/speed;
}
if(t<0){
t *= -1;
}
// find speeds
s1 = asign1*a1/t;
s2 = asign2*a2/t;
if(s1 < 1 && s1 > 0){
s1 = 1;
}
if(s2 < 1 && s2 > 0){
s2 = 1;
}
if(s1 > -1 && s1 < 0){
s1 = -1;
}
if(s2 > -1 && s2 < 0){
s2 = -1;
}
nxtDisplayTextLine(3,"%f - %d",angle1,target1);
nxtDisplayTextLine(4,"%f - %d",angle2,target2);
if(target1==0){
nMotorEncoderTarget[motorC] = target2; // set the target for Motor Encoder of Motor C
motor[motorC] = s2;
while(nMotorRunState[motorC] != runStateIdle){}
motor[motorC] = 0; // motor B is stopped at a power level of 0
}
else if(target2==0){
nMotorEncoderTarget[motorB] = target1; // set the target for Motor Encoder of Motor C
motor[motorB] = s1;
while(nMotorRunState[motorB] != runStateIdle){}
motor[motorB] = 0; // motor B is stopped at a power level of 0
}
else if(target1==0 && target2==0){
return;
}
else{
nMotorEncoderTarget[motorB] = target1; // set the target for Motor Encoder of Motor B
nMotorEncoderTarget[motorC] = target2; // set the target for Motor Encoder of Motor C
motor[motorB] = s1;
motor[motorC] = s2;
while(nMotorRunState[motorB] != runStateIdle || nMotorRunState[motorC] != runStateIdle){}
motor[motorB] = 0; // motor B is stopped at a power level of 0
motor[motorC] = 0; // motor C is stopped at a power level of 0
}
encoderBBegin = encoderBBegin + nMotorEncoder[motorB];
encoderCBegin = encoderCBegin + nMotorEncoder[motorC];
nMotorEncoder[motorB] = 0; // reset the Motor Encoder of Motor B
nMotorEncoder[motorC] = 0; // reset the Motor Encoder of Motor C
}
void ikinematics(int xx,int yy,int arm1,int arm2){
float B = sqrt(pow(xx,2)+pow(yy,2));
float t = atan2(yy,xx);
float theta = (t-acos((pow(arm1,2)+pow(B,2)-pow(arm2,2))/(2*arm1*B)))*180/PI;
float alpha = (acos((pow(arm1,2)-pow(B,2)+pow(arm2,2))/(2*arm1*arm2)))*180/PI;
//move motor B and C to position
alpha=360-alpha;
nxtDisplayTextLine(6,"Target: %d x %d",xx,yy);
moveToAngle(theta,alpha);
}
void liftUp(){
nMotorEncoderTarget[motorA]=90;
motor[motorA]=20;
while(nMotorRunState[motorA] != runStateIdle){}
motor[motorA]=0;
nMotorEncoder[motorA] = 0;
}
void liftDown(){
nMotorEncoderTarget[motorA]=-90;
motor[motorA]=-20;
while(nMotorRunState[motorA] != runStateIdle){}
motor[motorA]=0;
nMotorEncoder[motorA] = 0;
}
void readPBM(){
TFileHandle hFileHandle; // will keep track of our file
TFileIOResult nIOResult; // will store our IO results
string sFileName = "test1.pbm"; // the name of our file
int nFileSize = 15000; // will store our file size
char input;
string incomingString;
OpenRead(hFileHandle, nIOResult, sFileName, nFileSize);
do{
ReadByte(hFileHandle, nIOResult, input);
}while(input!=13);
do{
ReadByte(hFileHandle, nIOResult, input);
incomingString += input;
}while(input!=32);
xMax=atoi(incomingString);
incomingString = "";
do{
ReadByte(hFileHandle, nIOResult, input);
incomingString += input;
}while(input!=13);
yMax=atoi(incomingString);
incomingString = "";
do{
ReadByte(hFileHandle, nIOResult, input);
}while(input!=10);
for(int q = yMax; q > 0; q-=1){
for(int z = 0; z < xMax; z++){
do{
ReadByte(hFileHandle, nIOResult, input);
}while(input!=48 && input!=49);
coordinates[z][q]=input;
if(input=='0'){
count++;
nxtDisplayTextLine(1,"Count %d",count);
}
}
}
Close(hFileHandle, nIOResult);
nxtDisplayTextLine(2,"%d x %d",xMax,yMax);
}
int findNPixel(int x, int y, float direction){
coordinates[x][y]=1;
int c = 0;
int test = 0;
for(int i=y-1;i<y+1;i++){
for(int v=x-1;v<x+1;x++){
if(coordinates[v][i]==0){
switch(c){
case 1:
test = 315;
break;
case 2:
test = 0;
break;
case 3:
test = 45;
break;
case 4:
test = 270;
break;
case 6:
test = 90;
break;
case 7:
test = 225;
break;
case 8:
test = 180;
break;
case 9:
test = 135;
break;
}
if(direction-test<=22.5&&direction-test>=0 || test-direction<=22.5&&test-direction>=0){
return test;
}
}
c++;
}
}
return -1;
}
void drawLine(int xc, int yc){
ikinematics((xc - (int)(xMax / 2)),(yc + 9*63/8*mm),arm1,arm2);
dire = findNPixel(xc,yc,dire);
switch(dire){
case -1:
dire = 0;
return;
break;
case 0:
yc -= 1;
break;
case 45:
xc ++;
yc -=1;
break;
case 90:
xc ++;
break;
case 135:
xc ++;
yc ++;
break;
case 180:
yc ++;
break;
case 225:
xc -= 1;
yc ++;
break;
case 270:
xc -=1;
break;
case 315:
xc -=1;
yc -=1;
break;
default:
eraseDisplay();
PlayTone(800,100);
nxtDisplayTextLine(3,"ERROR at switch at LINE 258");
while(true){}
break;
}
drawLine(xc,yc);
} | | | | |
And this is the main program.
|
| Sun Sep 23, 2012 8:25 pm |
|
|
|
brianchen11
Rookie
Joined: Tue Jul 03, 2012 5:16 am
Posts: 43
|
Re: scara robot issues Sorry, the code posted doesn't work. I've rewritten the "finding the next pixel" section and made the robot simply write the coordinates it needs to go to in a text file, instead of performing it physically. Header file: | | | | Code: const int speed = 70;
const int mm = 1;
const int arm1 = 15*63/8*mm, //Length of arm1 in cm
arm2 = 14*63/8*mm, //Length of arm2 in cm
arm1R = 56, //Gear ratios
arm2R = 15;
int asign1,asign2;
float s1,s2;
float t;
long a1,a2;
long target1,target2;
long encoderBBegin = (arm1R*90),
encoderCBegin = (arm2R*180);
int xMax = 0;
int yMax = 0;
byte coordinates[91][61]; // Up to 100x100 no more flash after that
void moveToAngle(float angle1, float angle2){
target1 = angle1 * arm1R - encoderBBegin;
a1 = target1;
target2 = angle2 * arm2R - encoderCBegin;
a2 = target2;
// find t (time in s)
asign1=1;
asign2=1;
if(a1<0){
a1*=-1;
asign1 = -1;
}
if(a2<0){
a2*=-1;
asign2 = -1;
}
if(a1>a2){
t=a1/speed;
}
else{
t=a2/speed;
}
if(t<0){
t *= -1;
}
// find speeds
s1 = asign1*a1/t;
s2 = asign2*a2/t;
if(s1 < 1 && s1 > 0){
s1 = 1;
}
if(s2 < 1 && s2 > 0){
s2 = 1;
}
if(s1 > -1 && s1 < 0){
s1 = -1;
}
if(s2 > -1 && s2 < 0){
s2 = -1;
}
nxtDisplayTextLine(3,"%f - %d",angle1,target1);
nxtDisplayTextLine(4,"%f - %d",angle2,target2);
if(target1==0 && target2==0){
return;
}
else if(target1==0){
nMotorEncoderTarget[motorC] = target2; // set the target for Motor Encoder of Motor C
motor[motorC] = s2;
while(nMotorRunState[motorC] != runStateIdle){}
motor[motorC] = 0; // motor B is stopped at a power level of 0
}
else if(target2==0){
nMotorEncoderTarget[motorB] = target1; // set the target for Motor Encoder of Motor C
motor[motorB] = s1;
while(nMotorRunState[motorB] != runStateIdle){}
motor[motorB] = 0; // motor B is stopped at a power level of 0
}
else{
nMotorEncoderTarget[motorB] = target1; // set the target for Motor Encoder of Motor B
nMotorEncoderTarget[motorC] = target2; // set the target for Motor Encoder of Motor C
motor[motorB] = s1;
motor[motorC] = s2;
while(nMotorRunState[motorB] != runStateIdle || nMotorRunState[motorC] != runStateIdle){}
motor[motorB] = 0; // motor B is stopped at a power level of 0
motor[motorC] = 0; // motor C is stopped at a power level of 0
}
encoderBBegin = encoderBBegin + nMotorEncoder[motorB];
encoderCBegin = encoderCBegin + nMotorEncoder[motorC];
nMotorEncoder[motorB] = 0; // reset the Motor Encoder of Motor B
nMotorEncoder[motorC] = 0; // reset the Motor Encoder of Motor C
}
void ikinematics(int xx,int yy,int arm1,int arm2){
float B = sqrt(pow(xx,2)+pow(yy,2));
float t = atan2(yy,xx);
float theta = (t-acos((pow(arm1,2)+pow(B,2)-pow(arm2,2))/(2*arm1*B)))*180/PI;
float alpha = (acos((pow(arm1,2)-pow(B,2)+pow(arm2,2))/(2*arm1*arm2)))*180/PI;
//move motor B and C to position
alpha=360-alpha;
nxtDisplayTextLine(6,"Target: %d x %d",xx,yy);
moveToAngle(theta,alpha);
}
void liftUp(){
nMotorEncoderTarget[motorA]=90;
motor[motorA]=20;
while(nMotorRunState[motorA] != runStateIdle){}
motor[motorA]=0;
nMotorEncoder[motorA] = 0;
}
void liftDown(){
nMotorEncoderTarget[motorA]=-90;
motor[motorA]=-20;
while(nMotorRunState[motorA] != runStateIdle){}
motor[motorA]=0;
nMotorEncoder[motorA] = 0;
}
void readPBM(){
TFileHandle hFileHandle; // will keep track of our file
TFileIOResult nIOResult; // will store our IO results
string sFileName = "test1.pbm"; // the name of our file
int nFileSize = 15000; // will store our file size
char input;
string incomingString;
OpenRead(hFileHandle, nIOResult, sFileName, nFileSize);
do{
ReadByte(hFileHandle, nIOResult, input);
}while(input!=13);
do{
ReadByte(hFileHandle, nIOResult, input);
incomingString += input;
}while(input!=32);
xMax=atoi(incomingString);
incomingString = "";
do{
ReadByte(hFileHandle, nIOResult, input);
incomingString += input;
}while(input!=13);
yMax=atoi(incomingString);
incomingString = "";
do{
ReadByte(hFileHandle, nIOResult, input);
}while(input!=10);
for(int q = yMax; q > 0; q-=1){
for(int z = 0; z < xMax; z++){
do{
ReadByte(hFileHandle, nIOResult, input);
}while(input!=48 && input!=49);
coordinates[z][q]=input;
}
}
Close(hFileHandle, nIOResult);
nxtDisplayTextLine(2,"%d x %d",xMax,yMax);
}
void drawLine(int x, int y, int direction){
int dir = 0;
int lowestDif = 180;
int lowestx = 0;
int lowesty = 0;
int difference;
coordinates[x][y]=-1; //set this coordinate to -1 so it will never be drawn again.
ikinematics(x,y,arm1,arm2);
for(int b = y - 1; b < y + 1; b++){
for(int a = x - 1; a < x + 1; a++){
//
if(coordinates[a][b]==0){
//Find direction of this coordinate
if(a == x - 1 && b == y - 1 || a == x + 1 && b == y - 1){
dir = 45;
}
else if(a == x - 1 && b == y || a == x + 1 && b == y){
dir = 90;
}
else if(a == x - 1 && b == y + 1 || a == x + 1 && b == y + 1){
dir = 125;
}
else if(a == x && b == y - 1){
dir = 0;
}
else if(a == x && b == y + 1){
dir = 180;
}
else{
nxtDisplayTextLine(3,"SOMETHING IS WRONG ON LINE 202");
while(true){}
}
difference = dir - direction;
if(difference < 0){
difference*=-1;
}
if(difference <= lowestDif && difference >= 0){
lowestDif = difference;
lowestx = a;
lowesty = b;
}
//Check if it is lower than the lowestDir, if so, set it as the lowest
}
}
}
if(lowestDif == 180){
return;
}
else{
drawLine(lowestx, lowesty, dir);
}
}
void writeLine(int x, int y, int direction,TFileHandle hFileHandle2,TFileIOResult nIOResult2){
bool exit = false;
while(exit==false){
string xs = x;
string ys = y;
WriteText(hFileHandle2,nIOResult2,xs);
WriteByte(hFileHandle2,nIOResult2,9);
WriteText(hFileHandle2,nIOResult2,ys);
WriteByte(hFileHandle2,nIOResult2,13);
WriteByte(hFileHandle2,nIOResult2,10);
int dir = 0;
int lowestDif = 360;
int lowestx = 0;
int lowesty = 0;
int difference;
coordinates[x][y]=-1; //set this coordinate to -1 so it will never be drawn again.
for(int b = y - 1; b < y + 2; b++){
for(int a = x - 1; a < x + 2; a++){
if(coordinates[a][b]=='0'){
//Find direction of this coordinate
if(a == x - 1 && b == y - 1 || a == x + 1 && b == y - 1){
dir = 45;
}
else if(a == x - 1 && b == y || a == x + 1 && b == y){
dir = 90;
}
else if(a == x - 1 && b == y + 1 || a == x + 1 && b == y + 1){
dir = 125;
}
else if(a == x && b == y - 1){
dir = 0;
}
else if(a == x && b == y + 1){
dir = 180;
}
else{
nxtDisplayTextLine(3,"SOMETHING IS WRONG ON LINE 202");
while(true){}
}
difference = dir - direction;
if(difference < 0){
difference*=-1;
}
if(difference <= lowestDif){
lowestDif = difference;
lowestx = a;
lowesty = b;
}
//Check if it is lower than the lowestDir, if so, set it as the lowest
}
}
}
if(lowestDif == 360){
WriteText(hFileHandle2,nIOResult2,"Line Finish");
WriteByte(hFileHandle2,nIOResult2,13);
WriteByte(hFileHandle2,nIOResult2,10);
exit = true;
}
x = lowestx;
y = lowesty;
}
} | | | | |
Main program: | | | | Code: #include "SCARA4.h"
task main(){
bFloatDuringInactiveMotorPWM = false; // brake
nMotorPIDSpeedCtrl[motorA] = mtrSpeedReg; // enable PID on Motor A
nMotorPIDSpeedCtrl[motorB] = mtrSpeedReg; // enable PID on Motor A
nMotorPIDSpeedCtrl[motorC] = mtrSpeedReg; // enable PID on Motor A
nMotorEncoder[motorB] = 0; // reset the Motor Encoder of Motor B
nMotorEncoder[motorC] = 0; // reset the Motor Encoder of Motor C
readPBM(); //Read and save PBM into array
liftUp(); //lift clas up
TFileHandle hFileHandle3; // will keep track of our file
TFileIOResult nIOResult3; // will store our IO results
string sFileName = "lines.txt"; // the name of our file
int nFileSize = 15000; // will store our file size
OpenWrite(hFileHandle3,nIOResult3,sFileName,nFileSize);
for(int r = yMax; r > 0; r-=1){ //look through array to find black pixel
for(int t = 0; t < xMax; t++){
if(coordinates[t][r] == '0'){
writeLine(t,r,0,hFileHandle3,nIOResult3); //draw the line
}
}
}
Close(hFileHandle3,nIOResult3);
ikinematics(0,(int)(29*63/8*mm),arm1,arm2); //move back to original position
liftDown(); //lower arm
} | | | | |
|
| Mon Sep 24, 2012 3:55 am |
|
|
|
|
Page 1 of 1
|
[ 4 posts ] |
|
Who is online |
Users browsing this forum: No registered users and 2 guests |
|
You cannot post new topics in this forum
You cannot reply to topics in this forum
You cannot edit your posts in this forum
You cannot delete your posts in this forum
You cannot post attachments in this forum
|
|
