2015年6月10日 星期三

week16 TRT 自動化


#include <GL/glut.h>
#include <stdio.h>///存檔用
FILE * fout=NULL;///存檔用file output
FILE * fin=NULL;
float oldX=0, oldY=0;///angle=0,angle2=0,angle3=0,angle4=0
float angle[10]={0};
int angleID=1;
void drawBody(){
    glPushMatrix();
    glScalef(1, 0.5, 0.5);
    glutSolidCube(1);
    glPopMatrix();
}
void drawArm(){
    glPushMatrix();
    glScalef(0.6, 0.3, 0.3);
    glutSolidCube(1);
    glPopMatrix();
}
void display()
{
    { ///Lighting
    glColor3ub(166,132,100);
    GLfloat pos[]={0, 0, -1, 0};
    glLightfv(GL_LIGHT0, GL_POSITION, pos);
    glEnable(GL_LIGHT0);
    ///glEnable(GL_LIGHTING);
    glEnable(GL_DEPTH_TEST);
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    }
    glPushMatrix();
        glTranslatef(0.5, 0, 0);///最後一步, 把樓下的 "會對Arm Joint做旋轉的東西" 再移到右上 掛上去 ver 4
        glRotatef(angle[1], 0,0,1);///把下面整團, 都對中間做旋轉 ver 3
        glTranslatef(0.25, 0,0);///這是把它往右移, Arm Joint在中間 ver 2
        glColor3f(0,0,1);
        drawArm();///這是個Arm ver 1  Right Upper Arm
        glPushMatrix();
            glTranslatef(0.25, 0, 0);///最後一步, 把樓下的 "會對Arm Joint做旋轉的東西" 再移到右上 掛上去 ver 4
            glRotatef(angle[2], 0,0,1);///把下面整團, 都對中間做旋轉 ver 3
            glTranslatef(0.25, 0,0);///這是把它往右移, Arm Joint在中間 ver 2
            glColor3f(0,1,0);
            drawArm(); ///Right Lower Arm
        glPopMatrix();
    glPopMatrix();
    glPushMatrix();
        glTranslatef(-0.5, 0, 0);///最後一步, 把樓下的 "會對Arm Joint做旋轉的東西" 再移到右上 掛上去 ver 4
        glRotatef(-angle[3], 0,0,1);///把下面整團, 都對中間做旋轉 ver 3
        glTranslatef(-0.25, 0,0);///這是把它往右移, Arm Joint在中間 ver 2
        glColor3f(0.5,0.5,1);
        drawArm();///這是個Arm ver 1  Left Upper Arm
        glPushMatrix();
            glTranslatef(-0.25, 0, 0);///最後一步, 把樓下的 "會對Arm Joint做旋轉的東西" 再移到右上 掛上去 ver 4
            glRotatef(-angle[4], 0,0,1);///把下面整團, 都對中間做旋轉 ver 3
            glTranslatef(-0.25, 0,0);///這是把它往右移, Arm Joint在中間 ver 2
            glColor3f(0.5,1,0.5);
            drawArm(); ///Left Lower Arm
        glPopMatrix();
    glPopMatrix();
    drawBody();
    glFlush();
}
void motion(int x, int y)
{
    angle[angleID] -= y - oldY;
    oldY=y; oldX=x;
    if(fout==NULL)fout=fopen("a.txt","w+");///存檔用
    for(int i=0;i<10;i++)
    {
        printf(    "%f ",angle[i]);
        fprintf(fout,"%.1f ",angle[i]);
    }
    printf("\n");
     fprintf(fout,"\n");
}
void timerPlay(int t)
{
    glutTimerFunc(500,timerPlay,t+1);
    if(fin==NULL) fin=fopen("a.txt","r");
    for(int i=0;i<10;i++){
        fscanf(fin,"%f ",&angle[i]);
        printf("%f ",angle[i]);
    }
    glutPostRedisplay();
}
void mouse(int button, int state, int x, int y){
    if(state==GLUT_DOWN) { oldX=x; oldY=y; }
}
void keyboard(unsigned char key,int x,int y)
{
    if(key=='1')angleID=1;
    if(key=='2')angleID=2;
    if(key=='3')angleID=3;
    if(key=='4')angleID=4;
    if(key=='p')glutTimerFunc(10,timerPlay,0);

    glutPostRedisplay();

}
void timer(int t)
{
    glutTimerFunc(20, timer, t+1);
    //angle++;
    glutPostRedisplay();
}
int main(int argc, char**argv)
{
    glutInit(&argc, argv);
    glutCreateWindow("T-R-T test");
    glutDisplayFunc(display);
    glutTimerFunc(20, timer, 0);
    glutMouseFunc(mouse);
    glutMotionFunc(motion);
    glutKeyboardFunc(keyboard);
    glutMainLoop();
}

機器人自動化
#include <GL/glut.h>
#include <stdio.h>///存檔用
FILE * fout=NULL;///存檔用file output
FILE * fin=NULL;
float oldX=0, oldY=0;///angle=0,angle2=0,angle3=0,angle4=0
float angle[10]={0};
float angleOld[10]={0};
float angleNew[10]={0};
int angleID=1;
float a=0;
void drawBody(){
    glPushMatrix();
    glScalef(0.8, 0.9, 0.3);
    glutSolidSphere(0.5,100,100);
    glPopMatrix();
}
void drawArm(){
    glPushMatrix();
    glScalef(1.2, 0.8, 0.3);
    glutSolidSphere(0.3,50,50);
    glPopMatrix();
}
void display()
{
    { ///Lighting
    glColor3ub(166,132,100);
    GLfloat pos[]={0, 0, -1, 0};
    glLightfv(GL_LIGHT0, GL_POSITION, pos);
    glEnable(GL_LIGHT0);
    ///glEnable(GL_LIGHTING);
    glEnable(GL_DEPTH_TEST);
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    }
    glPushMatrix();
        glTranslatef(0.5, 0, 0);///最後一步, 把樓下的 "會對Arm Joint做旋轉的東西" 再移到右上 掛上去 ver 4
        glRotatef(angle[1], 0,0,1);///把下面整團, 都對中間做旋轉 ver 3
        glTranslatef(0.25, 0,0);///這是把它往右移, Arm Joint在中間 ver 2
        glColor3f(0,0,1);
        drawArm();///這是個Arm ver 1  Right Upper Arm
        glPushMatrix();
            glTranslatef(0.25, 0, 0);///最後一步, 把樓下的 "會對Arm Joint做旋轉的東西" 再移到右上 掛上去 ver 4
            glRotatef(angle[2], 0,0,1);///把下面整團, 都對中間做旋轉 ver 3
            glTranslatef(0.25, 0,0);///這是把它往右移, Arm Joint在中間 ver 2
            glColor3f(0,1,0);
            drawArm(); ///Right Lower Arm
        glPopMatrix();
    glPopMatrix();
    glPushMatrix();
        glTranslatef(-0.5, 0, 0);///最後一步, 把樓下的 "會對Arm Joint做旋轉的東西" 再移到右上 掛上去 ver 4
        glRotatef(-angle[3], 0,0,1);///把下面整團, 都對中間做旋轉 ver 3
        glTranslatef(-0.25, 0,0);///這是把它往右移, Arm Joint在中間 ver 2
        glColor3f(0.5,0.5,1);
        drawArm();///這是個Arm ver 1  Left Upper Arm
        glPushMatrix();
            glTranslatef(-0.25, 0, 0);///最後一步, 把樓下的 "會對Arm Joint做旋轉的東西" 再移到右上 掛上去 ver 4
            glRotatef(-angle[4], 0,0,1);///把下面整團, 都對中間做旋轉 ver 3
            glTranslatef(-0.25, 0,0);///這是把它往右移, Arm Joint在中間 ver 2
            glColor3f(0.5,1,0.5);
            drawArm(); ///Left Lower Arm
        glPopMatrix();
    glPopMatrix();
    drawBody();
    glFlush();
}
void motion(int x, int y)
{
    angle[angleID] -= y - oldY;
    oldY=y; oldX=x;

}
void readNew()
{
   if(fin==NULL) fin=fopen("a.txt","r");
    for(int i=0;i<10;i++)
        {
        angleOld[i]=angleNew[i];
        fscanf(fin,"%f ",&angleNew[i]);
        printf("%f ",angleNew[i]);
        }
    printf("\n");
}
void timerPlay(int t)
{
    glutTimerFunc(20,timerPlay,t+1);
    if(t%20==0) readNew();
    for(int i=0;i<10;i++){
        angle[i]=(1-a)*angleOld[i]+(a)*angleNew[i];
    }
    glutPostRedisplay();
}
void mouse(int button, int state, int x, int y){
    if(state==GLUT_DOWN) { oldX=x; oldY=y; }
}
void keyboard(unsigned char key,int x,int y)
{
    if(key=='1')angleID=1;
    if(key=='2')angleID=2;
    if(key=='3')angleID=3;
    if(key=='4')angleID=4;
    if(key=='p')glutTimerFunc(10,timerPlay,0);
    if(key=='s') {

                if(fout=NULL)fout=fopen("a.txt","w+");
                for(int i=0;i<10;i++)
                    {
                    printf("%.1f \t",&angle[i]);
                    fprintf(fout,"%.1f ",angle[i]);
                    }
                fprintf(fout,"\n");
                }
    glutPostRedisplay();

}
void timer(int t)
{
    glutTimerFunc(20, timer, t+1);
    //angle++;
    glutPostRedisplay();
}
int main(int argc, char**argv)
{
    glutInit(&argc, argv);
    glutCreateWindow("T-R-T test");
    glutDisplayFunc(display);
    glutTimerFunc(20, timer, 0);
    glutMouseFunc(mouse);
    glutMotionFunc(motion);
    glutKeyboardFunc(keyboard);
    glutMainLoop();
}

張貼者： 於
標籤： ,

沒有留言:

張貼留言

訂閱： 張貼留言 (Atom)