home
***
CD-ROM
|
disk
|
FTP
|
other
***
search
/
Between Heaven & Hell 2
/
BetweenHeavenHell.cdr
/
500
/
471
/
rccl166
< prev
next >
Wrap
Text File
|
1987-03-02
|
11KB
|
319 lines
/*
* RCCL Version 1.0 Author : Vincent Hayward
* RTC 2.0 School of Electrical Engineering
* MAC 2.0 Purdue University
* Dir : h
* File : pumac.c
* Remarks : generates the include file standata.h containing all the
* precalculated constants of the puma Stanford arm.
* Due to hardware problems, most of the constants have
* to be revised.
* Usage : cc stanc.c -lm ; a.out
*/
/*************************************************************************
notations : { E } : E expressed in degrees converted in radians
[ E ] : E of type double converted in type int
< E > : E ....... int ................. short
| E | : mod 2*pi ; E + n*2*pi , n so that 0 <= | E | < 2*pi
constants in upercase letters, variables in lower.
(d) type double, (i) type int, (s) type short
min is chosen as the furthest position clockwise
angles increase in counter-clockwise motion
rng is the angle between min and max positions
relative coordinates measure angles in respect with the min pos
let :
DMIN (d) the minimum position in deg. absolute coordinates
JMIN (d) .................... in rad. ....................
DRNG (d) the range in deg.
JRNG (d) ............ rad.
DCAL (d) the calibration position in deg. absolute coordinates
ACAL (d) ........................ in rad ....................
JCAL (d) ........................ in rad. relative coordinates
ECCL (i) the counters content at the calibration position
RATIO (d) the ratio encoder pulses / physical unitd (mm,rad)
sol (d) be the raw angle given by the solution equation
j (d) a joint value expressed in relative coordinates
e (s) the final encoder value
DMXV (d) the maximum joint velocity in deg. / sec.
JMXV (d) the ......................... rd. / sec.
this program generates the following constantes :
JMIN = { DMIN }
JRNG = { DRNG }
RATIO = RATIO
OFFSET = [ | { DCAL - DMIN } | * RATIO ] - ECCL
JCAL = | { DCAL - DMIN } |
ACAL = { DCAL }
JMXV = { DMXV }
to be used by :
calibrate :
sets j = JCAL
computes the REST transform from the joints values
tr_to_jns :
computes j = | sol - JMIN | and checks if j < JRNG
putenco :
e = < [ j * RATIO ] - OFFSET >
setpoint :
to monitor joint velocities
********************************************************************/
#include <stdio.h>
#define PIB2 1.57079632679489660
#define PI 3.14159265358979320
#define PIT2 6.28318530717958650
#define RADTODEG 57.29577951308232100
#define DEGTORAD 0.01745329251994330
#define YES 1
#define NO 0
#define D2 174.00 /* mm */
#define DMIN1 220.0
#define DRNG1 280.0
#define DMIN2 0.0
#define DRNG2 180.0
#define DMIN3 195.0
#define DRNG3 690.0
#define DMIN4 180.0
#define DRNG4 360.0
#define DMIN5 260.0
#define DRNG5 200.0
#define DMIN6 180.0
#define DRNG6 360.0
#define RATIO1 -12732.39545 /* pls/rad */
#define RATIO2 15915.49431 /* pls/rad */
#define RATIO3 35.64625147 /* pls/mm */
#define RATIO4 -4583.662360 /* pls/rad */
#define RATIO5 4583.662360 /* pls/rad */
#define RATIO6 -5570.423007 /* pls/rad */
#define ECCL1 32768
#define ECCL2 32768
#define ECCL3 26468
#define ECCL4 32768
#define ECCL5 25568
#define ECCL6 32768
#define ESQR1 551 /* how much to bring the arm in */
#define ESQR2 263 /* a square configuration from the */
#define ESQR3 581 /* zero index configuration */
#define ESQR4 228
#define ESQR5 -16
#define ESQR6 -13
#define DCAL1 0.0
#define DCAL2 90.0
#define DCAL3 385.0
#define DCAL4 0.0
#define DCAL5 90.0
#define DCAL6 0.0
#define DMXV1 600.
#define DMXV2 600.
#define DMXV3 500. /* mm/sec */
#define DMXV4 600.
#define DMXV5 600.
#define DMXV6 600.
#define MXDC1 300
#define MXDC2 300
#define MXDC3 200
#define MXDC4 200
#define MXDC5 200
#define MXDC6 200
#define MXOC1 250
#define MXOC2 250
#define MXOC3 200
#define MXOC4 200
#define MXOC5 200
#define MXOC6 200
double range(a)
double a;
{
while(a < 0.)
a += PIT2;
while(a >= PIT2)
a -= PIT2;
return(a);
}
#define ABS(a) ((a < 0) ? (-(a)) : (a))
#define ORDER(a, b) {int t; if (a > b) {t = b; b = a; a = t;}}
/*
* create #define constants for stanford arm
*/
main()
{
int o1, o2, o3, o4, o5, o6;
int mx1, mx2, mx3, mx4, mx5, mx6;
int mn1, mn2, mn3, mn4, mn5, mn6;
FILE *fph;
fph = fdopen(creat("standata.h", 0644), "w");
fprintf(fph, "#define D2 %20.12f\n", D2);
fprintf(fph, "#define D22 %20.12f\n", D2 * D2);
fprintf(fph, "#define JMIN1 %20.12f\n", DEGTORAD * DMIN1);
fprintf(fph, "#define JRNG1 %20.12f\n", DEGTORAD * DRNG1);
fprintf(fph, "#define ACAL1 %20.12f\n", DEGTORAD * DCAL1);
fprintf(fph, "#define JMIN2 %20.12f\n", DEGTORAD * DMIN2);
fprintf(fph, "#define JRNG2 %20.12f\n", DEGTORAD * DRNG2);
fprintf(fph, "#define ACAL2 %20.12f\n", DEGTORAD * DCAL2);
fprintf(fph, "#define JMIN3 %20.12f\n", DMIN3);
fprintf(fph, "#define JRNG3 %20.12f\n", DRNG3);
fprintf(fph, "#define ACAL3 %20.12f\n", DCAL3);
fprintf(fph, "#define JMIN4 %20.12f\n", DEGTORAD * DMIN4);
fprintf(fph, "#define JRNG4 %20.12f\n", DEGTORAD * DRNG4);
fprintf(fph, "#define ACAL4 %20.12f\n", DEGTORAD * DCAL4);
fprintf(fph, "#define JMIN5 %20.12f\n", DEGTORAD * DMIN5);
fprintf(fph, "#define JRNG5 %20.12f\n", DEGTORAD * DRNG5);
fprintf(fph, "#define ACAL5 %20.12f\n", DEGTORAD * DCAL5);
fprintf(fph, "#define JMIN6 %20.12f\n", DEGTORAD * DMIN6);
fprintf(fph, "#define JRNG6 %20.12f\n", DEGTORAD * DRNG6);
fprintf(fph, "#define ACAL6 %20.12f\n", DEGTORAD * DCAL6);
fprintf(fph, "#define JCAL1 %20.12f\n",
range(DEGTORAD * (DCAL1 - DMIN1)));
fprintf(fph, "#define JCAL2 %20.12f\n",
range(DEGTORAD * (DCAL2 - DMIN2)));
fprintf(fph, "#define JCAL3 %20.12f\n",
/* LIN JOINT */ DCAL3 - DMIN3);
fprintf(fph, "#define JCAL4 %20.12f\n",
range(DEGTORAD * (DCAL4 - DMIN4)));
fprintf(fph, "#define JCAL5 %20.12f\n",
range(DEGTORAD * (DCAL5 - DMIN5)));
fprintf(fph, "#define JCAL6 %20.12f\n",
range(DEGTORAD * (DCAL6 - DMIN6)));
fprintf(fph, "#define RATIO1 %20.12f\n", RATIO1);
fprintf(fph, "#define RATIO2 %20.12f\n", RATIO2);
fprintf(fph, "#define RATIO3 %20.12f\n", RATIO3);
fprintf(fph, "#define RATIO4 %20.12f\n", RATIO4);
fprintf(fph, "#define RATIO5 %20.12f\n", RATIO5);
fprintf(fph, "#define RATIO6 %20.12f\n", RATIO6);
fprintf(fph, "#define OFFSET1 (%6d)\n", o1 =
(int)(range((DCAL1 - DMIN1) * DEGTORAD) * RATIO1) - ECCL1);
fprintf(fph, "#define OFFSET2 (%6d)\n", o2 =
(int)(range((DCAL2 - DMIN2) * DEGTORAD) * RATIO2) - ECCL2);
fprintf(fph, "#define OFFSET3 (%6d)\n", o3 =
(int)((DCAL3 - DMIN3) * RATIO3) - ECCL3);
fprintf(fph, "#define OFFSET4 (%6d)\n", o4 =
(int)(range((DCAL4 - DMIN4) * DEGTORAD) * RATIO4) - ECCL4);
fprintf(fph, "#define OFFSET5 (%6d)\n", o5 =
(int)(range((DCAL5 - DMIN5) * DEGTORAD) * RATIO5) - ECCL5);
fprintf(fph, "#define OFFSET6 (%6d)\n", o6 =
(int)(range((DCAL6 - DMIN6) * DEGTORAD) * RATIO6) - ECCL6);
fprintf(fph, "#define JMXV1 %f\n", DEGTORAD * DMXV1);
fprintf(fph, "#define JMXV2 %f\n", DEGTORAD * DMXV2);
fprintf(fph, "#define JMXV3 %f\n", DMXV3);
fprintf(fph, "#define JMXV4 %f\n", DEGTORAD * DMXV4);
fprintf(fph, "#define JMXV5 %f\n", DEGTORAD * DMXV5);
fprintf(fph, "#define JMXV6 %f\n", DEGTORAD * DMXV6);
fprintf(fph, "#define EMXV1 0%o\n",
(int)(DEGTORAD * DMXV1 * ABS(RATIO1) / 1000.));
fprintf(fph, "#define EMXV2 0%o\n",
(int)(DEGTORAD * DMXV2 * ABS(RATIO2) / 1000.));
fprintf(fph, "#define EMXV3 0%o\n",
(int)( DMXV3 * ABS(RATIO3) / 1000.));
fprintf(fph, "#define EMXV4 0%o\n",
(int)(DEGTORAD * DMXV4 * ABS(RATIO4) / 1000.));
fprintf(fph, "#define EMXV5 0%o\n",
(int)(DEGTORAD * DMXV5 * ABS(RATIO5) / 1000.));
fprintf(fph, "#define EMXV6 0%o\n",
(int)(DEGTORAD * DMXV6 * ABS(RATIO6) / 1000.));
fprintf(fph, "#define ECCL1 0%o\n", ECCL1);
fprintf(fph, "#define ECCL2 0%o\n", ECCL2);
fprintf(fph, "#define ECCL3 0%o\n", ECCL3);
fprintf(fph, "#define ECCL4 0%o\n", ECCL4);
fprintf(fph, "#define ECCL5 0%o\n", ECCL5);
fprintf(fph, "#define ECCL6 0%o\n", ECCL6);
fprintf(fph, "#define XCCL1 0%o\n", ECCL1 + ESQR1);
fprintf(fph, "#define XCCL2 0%o\n", ECCL2 + ESQR2);
fprintf(fph, "#define XCCL3 0%o\n", ECCL3 + ESQR3);
fprintf(fph, "#define XCCL4 0%o\n", ECCL4 + ESQR4);
fprintf(fph, "#define XCCL5 0%o\n", ECCL5 + ESQR5);
fprintf(fph, "#define XCCL6 0%o\n", ECCL6 + ESQR6);
mn1 = -o1;
mx1 = (int)(DEGTORAD * DRNG1 * RATIO1) - o1;
ORDER(mn1, mx1);
mn2 = -o2;
mx2 = (int)(DEGTORAD * DRNG2 * RATIO2) - o2;
ORDER(mn2, mx2);
mn3 = -o3;
mx3 = (int)( DRNG3 * RATIO3) - o3;
ORDER(mn3, mx3);
mn4 = -o4;
mx4 = (int)(DEGTORAD * DRNG4 * RATIO4) - o4;
ORDER(mn4, mx4);
mn5 = -o5;
mx5 = (int)(DEGTORAD * DRNG5 * RATIO5) - o5;
ORDER(mn5, mx5);
mn6 = -o6;
mx6 = (int)(DEGTORAD * DRNG6 * RATIO6) - o6;
ORDER(mn6, mx6);
fprintf(fph, "#define ECMN1 0%o\n", mn1);
fprintf(fph, "#define ECMN2 0%o\n", mn2);
fprintf(fph, "#define ECMN3 0%o\n", mn3);
fprintf(fph, "#define ECMN4 0%o\n", mn4);
fprintf(fph, "#define ECMN5 0%o\n", mn5);
fprintf(fph, "#define ECMN6 0%o\n", mn6);
fprintf(fph, "#define ECMX1 0%o\n", mx1);
fprintf(fph, "#define ECMX2 0%o\n", mx2);
fprintf(fph, "#define ECMX3 0%o\n", mx3);
fprintf(fph, "#define ECMX4 0%o\n", mx4);
fprintf(fph, "#define ECMX5 0%o\n", mx5);
fprintf(fph, "#define ECMX6 0%o\n", mx6);
fprintf(fph, "#define MXDC1 0%o\n", MXDC1);
fprintf(fph, "#define MXDC2 0%o\n", MXDC2);
fprintf(fph, "#define MXDC3 0%o\n", MXDC3);
fprintf(fph, "#define MXDC4 0%o\n", MXDC4);
fprintf(fph, "#define MXDC5 0%o\n", MXDC5);
fprintf(fph, "#define MXDC6 0%o\n", MXDC6);
fprintf(fph, "#define MXOC1 0%o\n", MXOC1);
fprintf(fph, "#define MXOC2 0%o\n", MXOC2);
fprintf(fph, "#define MXOC3 0%o\n", MXOC3);
fprintf(fph, "#define MXOC4 0%o\n", MXOC4);
fprintf(fph, "#define MXOC5 0%o\n", MXOC5);
fprintf(fph, "#define MXOC6 0%o\n", MXOC6);
}