|
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
|
|