View unanswered posts | View active topics It is currently Tue Jul 22, 2014 6:27 am






Reply to topic  [ 4 posts ] 
scara robot issues 
Author Message
Rookie

Joined: Tue Jul 03, 2012 5:16 am
Posts: 43
Post 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
Profile
Site Admin
Site Admin

Joined: Thu May 24, 2012 12:15 pm
Posts: 555
Post 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 Wiki and our Forums.

I just met you,
And this is crazy,
But here's my code now,
So fix it, maybe?
~ Carly Rae Jepsen parody


Sun Sep 23, 2012 8:16 pm
Profile
Rookie

Joined: Tue Jul 03, 2012 5:16 am
Posts: 43
Post 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.
Code:
#include "SCARA3.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


   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'){                  
            drawLine(t,r);               //draw the line
         }
      }
   }

   ikinematics(0,(int)(29*63/8*mm),arm1,arm2); //move back to original position
   liftDown(); //lower arm
}


Sun Sep 23, 2012 8:25 pm
Profile
Rookie

Joined: Tue Jul 03, 2012 5:16 am
Posts: 43
Post 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
Profile
Display posts from previous:  Sort by  
Reply to topic   [ 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

Search for:
Jump to:  



Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Group.
Designed by ST Software for PTF.