/* Example #1: Draw Lines along major axis +Y ^ | +-->+X +---------------+ +Z | FRONT | *0 0* | | | L R | | | *1 000 1* | | | | | | *2 2* | | +---------------+ offsets: L1 -18.5, 30, 3 L2 -18.5, 0, 3 L3 -18.5, -30, 3 R1 18.5, 30, 3 R2 18.5, 0, 3 R3 18.5, -30, 3 */ #include #include #include #include #include #define SCL(A) ((A)/50.0) #include #include #include #include "3d.h" #include "ikLib.h" #define R0 0 #define R1 1 #define R2 2 #define L0 3 #define L1 4 #define L2 5 typedef struct legs_s { point3d_t offset; // offsets for legs re body point3d_t seglen; // leg segment lengths point3d_t legAngles; // leg mount angles point3d_t rest; // rest positions point3d_t target; // endpoint target position point3d_t servoAngles;// ik results for servo angles } legs_t; legs_t legs[6]; void InKintation( legs_t * this) ; void Box(float sizex, float sizey, float sizez, GLenum type ) ; void leg(float servo1, float servo2, float servo3); float servoAngle[18]; void initLegs() { Set3d(&legs[R0].offset, pt3( 18.5, 30, 3)); Set3d(&legs[R1].offset, pt3( 18.5, 0, 3)); Set3d(&legs[R2].offset, pt3( 18.5, -30, 3)); Set3d(&legs[L0].offset, pt3(-18.5, 30, 3)); Set3d(&legs[L1].offset, pt3(-18.5, 0, 3)); Set3d(&legs[L2].offset, pt3(-18.5, -30, 3)); // X and Y arent implemented yet, its 12:30am and I dont want to hurt my brain Set3d(&legs[R0].legAngles, pt3( 0, 0, 0)); Set3d(&legs[R1].legAngles, pt3( 0, 0, 0 )); Set3d(&legs[R2].legAngles, pt3( 0, 0, 0 )); Set3d(&legs[L0].legAngles, pt3( 0, 0, 180 )); Set3d(&legs[L1].legAngles, pt3( 0, 0, 180 )); Set3d(&legs[L2].legAngles, pt3( 0, 0, 180 )); Set3d(&legs[R0].rest, pt3( 18.5+10, 30, 3-13)); Set3d(&legs[R1].rest, pt3( 18.5+10, 0, 3-13)); Set3d(&legs[R2].rest, pt3( 18.5+10, -30, 3-13)); Set3d(&legs[L0].rest, pt3(-18.5-10, 30, 3-13)); Set3d(&legs[L1].rest, pt3(-18.5-10, 0, 3-13)); Set3d(&legs[L2].rest, pt3(-18.5-10, -30, 3-13)); Set3d(&legs[R0].seglen, pt3( -8.5, 17, 31 )); Set3d(&legs[R1].seglen, pt3( -8.5, 17, 31 )); Set3d(&legs[R2].seglen, pt3( -8.5, 17, 31 )); Set3d(&legs[L0].seglen, pt3( 8.5, 17, 31 )); Set3d(&legs[L1].seglen, pt3( 8.5, 17, 31 )); Set3d(&legs[L2].seglen, pt3( 8.5, 17, 31 )); } void InKintation( legs_t * this) { double C, c, b, t; double len0, len1, len2; double px, py, pz; double t0, t1, t2; len0 = this->seglen.x; len1 = this->seglen.y; len2 = this->seglen.z; px = this->target.x - this->offset.x; py = this->target.y - this->offset.y; pz = this->target.z - this->offset.z; // printf(" len: %f, %f, %f \n", len0, len1, len2); // printf(" p-: %f, %f, %f \n", px, py, pz); //-- C = dist3d(0.0, 0.0, 0.0, px-len0, py, pz); c = elbow(C, len2, len1); b = elbow(len2, C, len1); t = dir(dist3d(0.0,0.0,0.0, px, py, 0.0), pz); // printf(" C=%f c=%f b=%f t=%f\n", C, c, b, t); t0 = dir( px, py ) - this->legAngles.z; t1 = t + b; t2 = 180.0 + c; // printf(" t0 %f, t1 %f, t2 %f\n", t0, t1, t2); //-- this->servoAngles.x = t0; this->servoAngles.y = t1; this->servoAngles.z = t2; } void *legUpdate() { int i; int T; initLegs() ; for(i=0; i < 6; i++) { Set3d( &(legs[i].target), legs[i].rest ); } while (1) { for(i=0; i < 6; i++) { // printf("Leg %d:\n", i); InKintation( &(legs[i])); servoAngle[(i*3)+0] = legs[i].servoAngles.x; servoAngle[(i*3)+1] = legs[i].servoAngles.y; servoAngle[(i*3)+2] = legs[i].servoAngles.z; legs[i].target.x = legs[i].rest.x *1.5; legs[i].target.y = legs[i].rest.y + 6*cos(toRads(-2*T)); legs[i].target.z = legs[i].rest.z + 15*sin(toRads(2*T)); } T++; if (T == 360) T = 0; usleep(20000); } return NULL; } void Display(void) { // legUpdate(); glClear(GL_COLOR_BUFFER_BIT); // clear buffer glLoadIdentity(); glTranslated( 0, 0, -1.5); // position robot. glRotated(22, 1, 0, 0); // position robot. glPushMatrix(); glColor3f(1,1,1); Box(SCL(41.25), SCL(12.5), SCL(66.5), GL_LINE_LOOP ); // create body glTranslated(SCL(18.75), SCL(3), SCL(33.25)); // position to first leg leg(servoAngle[0], servoAngle[1], servoAngle[2]) ; glPopMatrix(); glPushMatrix(); glTranslated(SCL(18.75), SCL(3), 0); // position to second leg leg(servoAngle[3], servoAngle[4], servoAngle[5]) ; glPopMatrix(); glPushMatrix(); glTranslated(SCL(18.75), SCL(3), SCL(-33.25)); // position to second leg leg(servoAngle[6], servoAngle[7], servoAngle[8]) ; glPopMatrix(); glPushMatrix(); glRotated(180, 0, 1, 0); glTranslated(SCL(18.75), SCL(3), SCL(-33.25)); // position to 4 leg(servoAngle[9], servoAngle[10], servoAngle[11]) ; glPopMatrix(); glPushMatrix(); glRotated(180, 0, 1, 0); glTranslated(SCL(18.75), SCL(3), 0); // position to 4 leg(servoAngle[12], servoAngle[13], servoAngle[14]) ; glPopMatrix(); glRotated(180, 0, 1, 0); glTranslated(SCL(18.75), SCL(3), SCL(33.25)); // position to 4 leg(servoAngle[15], servoAngle[16], servoAngle[17]) ; glFlush(); glutSwapBuffers(); } void Timer(int extra) { glutPostRedisplay(); glutTimerFunc(15,Timer,0); } void Box(float sizex, float sizey, float sizez, GLenum type ) { //normals static GLfloat n[6][3] = { {-1.0, 0.0, 0.0}, /* to our left */ {0.0, 1.0, 0.0}, /* up */ {1.0, 0.0, 0.0}, /* to our right */ {0.0, -1.0, 0.0}, /* down */ {0.0, 0.0, 1.0}, /* towards us */ {0.0, 0.0, -1.0} /* away from us */ }; //corner numbers for 6 faces of a cube static GLint faces[6][4] = { {0, 1, 2, 3}, {3, 2, 6, 7}, {7, 6, 5, 4}, {4, 5, 1, 0}, {5, 6, 2, 1}, {7, 4, 0, 3} }; // 8 points on the cube GLfloat v[8][3]; GLint i; // [][0] is x co-ord [][1] is y co-ord [][2] is z co-ord // set up point x co-ords v[0][0] = v[1][0] = v[2][0] = v[3][0] = -sizex/2.0 ; v[4][0] = v[5][0] = v[6][0] = v[7][0] = sizex/2.0 ; // set up point y co-ords v[0][1] = v[1][1] = v[4][1] = v[5][1] = -sizey/2.0 ; v[2][1] = v[3][1] = v[6][1] = v[7][1] = sizey/2.0 ; // set up point z co-ords v[0][2] = v[3][2] = v[4][2] = v[7][2] = -sizez/2.0 ; v[1][2] = v[2][2] = v[5][2] = v[6][2] = sizez/2.0 ; for (i = 5; i >= 0; i--) { //glBegin(GL_QUADS); // glBegin(type); glNormal3fv(&n[i][0]); glVertex3fv(&v[faces[i][0]][0]); glVertex3fv(&v[faces[i][1]][0]); glVertex3fv(&v[faces[i][2]][0]); glVertex3fv(&v[faces[i][3]][0]); glEnd(); } } void leg(float servo1, float servo2, float servo3) { glColor3f(1,0,0); // set colour glRotated(servo1,0,1,0); // set angle glTranslated(SCL((8.5/2.0)-1.5), 0, 0); // distance to left pivot from middle Box(SCL(8.5), SCL(6.5), SCL(7), GL_LINE_LOOP ); // create it glTranslated(SCL((8.5/2.0)-1.0), 0, 0); // distance to right pivot from middle glColor3f(1,1,0); // set colour glRotated(servo2,0,0,1); // set angle glTranslated(SCL((17.0/2.0)-1.0), 0, 0); // distance to left pivot from middle Box(SCL(17.0), SCL(6.5), SCL(5.0), GL_LINE_LOOP ); // create it glTranslated(SCL((17.0/2.0)-2.5), 0, 0); // distance to right pivot from middle glColor3f(0,0,1); glRotated(servo3,0,0,1); glTranslated(SCL((31.0/2.0)-2), 0, 0); Box(SCL(31.0), SCL(6.5), SCL(7.5), GL_LINE_LOOP ); } int main(int argc, char **argv) { int i; glutInit(&argc, argv); pthread_t thread1; glutInitDisplayMode(GLUT_DOUBLE|GLUT_RGB ); glutInitWindowSize(640,480); glutCreateWindow("This is the window title"); glutDisplayFunc(Display); glMatrixMode(GL_PROJECTION); glLoadIdentity(); //glFrustum(-1,1, -1, 1, 1, 3); gluPerspective(480, 0.7, 0.1, 6); glMatrixMode(GL_MODELVIEW); // set camera position glLoadIdentity(); //glRotated(45,1,0,0); glutTimerFunc(0,Timer,0); for (i = 0; i < 18; i++) { servoAngle[i] = 0; } if( (pthread_create( &thread1, NULL, &legUpdate, NULL)) ){ printf("Failed to start motion control thread \n"); } glutMainLoop(); return 0; }