home
***
CD-ROM
|
disk
|
FTP
|
other
***
search
/
Between Heaven & Hell 2
/
BetweenHeavenHell.cdr
/
500
/
471
/
rccl168
< prev
next >
Wrap
Text File
|
1987-03-02
|
13KB
|
362 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 pumadata.h containing all the
* precalculated constants of the puma 600.
* Usage : cc pumac.c -lm ; a.out
*/
/*************************************************************************
notations : { E } : E expressed in degrees converted to radians
[ E ] : E of type double converted to 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 }
EMXV = [ JMXV * RATIO / 1000] (max enc inc. in 1 ms)
ECMN = - OFFSET (min enc value)
ECMX = { DRNG } * RATIO - OFFSET (max enc value)
MXDC = MXDC (max desired current in dac readings)
MXOC = MXOC (max observed current in adc readings)
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 >
e5 = < [ j5 * RATIO5 + j4 * RATI54 ] - OFFSET5 >
e6 = < [ j6 * RATIO6 + j5 * RATI65 + j4 * RATI64 ] - OFFSET6 >
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
/* dimension in millimeters */
#define A2 431.80
#define A3 20.320
#define D3 128.778
#define D4 433.070
#define DMIN1 200.0
#define DRNG1 320.0
#define DMIN2 137.0
#define DRNG2 266.0
#define DMIN3 308.0
#define DRNG3 284.0
#define DMIN4 250.0
#define DRNG4 290.0
#define DMIN5 260.0
#define DRNG5 200.0
#define DMIN6 180.0 /* was 210.0 in full count */
#define DRNG6 360.0 /* 240.0 */
#define RATIO1 -9964.8501 /* pls/rad */
#define RATIO2 13727.4322 /* pls/rad */
#define RATIO3 -8547.6231 /* pls/rad */
#define RATIO4 12101.5626 /* pls/rad */
#define RATIO5 11446.9010 /* pls/rad */
#define RATIO6 06102.4752 /* pls/rad ,in full count was 12204.9560 */
#define RATI54 (-168.2565) /* pls/rad */
#define RATI64 (78.9028) /* pls/rad ,in full count was (157.8056) */
#define RATI65 (1033.4033) /* pls/red, in full count was (2066.8067)*/
#define ECCL1 32000 /*32768*/
#define ECCL2 32000 /*32768*/
#define ECCL3 19000 /*19341*/
#define ECCL4 26000 /*32768*/
#define ECCL5 40000 /*50749*/
#define ECCL6 40000 /*32768*/
#define ESQR1 283 /* how much to bring the arm in */
#define ESQR2 600 /* 704 a square configuration from the */
#define ESQR3 449 /* zero index configuration */
#define ESQR4 480
#define ESQR5 146 /* 147 */
#define ESQR6 185
#define DCAL1 0.0
#define DCAL2 270.0
#define DCAL3 180.0
#define DCAL4 0.0
#define DCAL5 90.0
#define DCAL6 0.0
#define DMXV1 660.
#define DMXV2 660.
#define DMXV3 660.
#define DMXV4 660.
#define DMXV5 660.
#define DMXV6 660.
#define MXDC1 1000 /* 500 */
#define MXDC2 1000 /* 400 */
#define MXDC3 1000 /* 300 */
#define MXDC4 500 /* 200 */
#define MXDC5 500 /* 200 */
#define MXDC6 500 /* 200 */
#define MXOC1 500 /* 350 */
#define MXOC2 500 /* 350 */
#define MXOC3 500 /* 350 */
#define MXOC4 300
#define MXOC5 300
#define MXOC6 300
#define CV 7.05 /* oz in -> N mm */
#define C21 ( 5500.00 ) /* this is in oz in */
#define C31 (-30.0 )
#define C32 ( 1100.0 )
#define C50 (-70.0 )
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 puma 600 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("pumadata.h", 0644),"w");
fprintf(fph, "#define A2 %20.12f\n", A2);
fprintf(fph, "#define A3 %20.12f\n", A3);
fprintf(fph, "#define D3 %20.12f\n", D3);
fprintf(fph, "#define D4 %20.12f\n", D4);
fprintf(fph, "#define D32 %20.12f\n", D3 * D3);
fprintf(fph, "#define E432 %20.12f\n", D4 * D4 + A3*A3 + A2*A2);
fprintf(fph, "#define AA3D4 %20.12f\n", atan2(A3, -D4));
fprintf(fph, "#define E4AA4AD %20.12f\n", 4. * A2 * A2 * A3 * A3 +
4. * A2 * A2 * D4 * D4);
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", DEGTORAD * DMIN3);
fprintf(fph, "#define JRNG3 %20.12f\n", DEGTORAD * DRNG3);
fprintf(fph, "#define ACAL3 %20.12f\n", DEGTORAD * 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",
range(DEGTORAD * (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 RATI54 %20.12f\n", RATI54);
fprintf(fph, "#define RATI65 %20.12f\n", RATI65);
fprintf(fph, "#define RATI64 %20.12f\n", RATI64);
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)(range((DCAL3 - DMIN3) * DEGTORAD) * 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
+ range((DCAL4 - DMIN4) * DEGTORAD) * RATI54) - ECCL5);
fprintf(fph, "#define OFFSET6 (%6d)\n", o6 =
(int)( range((DCAL6 - DMIN6) * DEGTORAD) * RATIO6
+ range((DCAL5 - DMIN5) * DEGTORAD) * RATI65
+ range((DCAL4 - DMIN4) * DEGTORAD) * RATI64) - ECCL6);
fprintf(fph, "#define JMXV1 %f\n", DEGTORAD * DMXV1);
fprintf(fph, "#define JMXV2 %f\n", DEGTORAD * DMXV2);
fprintf(fph, "#define JMXV3 %f\n", DEGTORAD * 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)(DEGTORAD * 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)(DEGTORAD * DRNG3 * RATIO3) - o3;
ORDER(mn3, mx3);
mn4 = -o4;
mx4 = (int)(DEGTORAD * DRNG4 * RATIO4) - o4;
ORDER(mn4, mx4);
mn5 = -o5;
mx5 = (int)(DEGTORAD * DRNG5 * RATIO5
+ DEGTORAD * DRNG4 * RATI54) - o5;
ORDER(mn5, mx5);
mn6 = -o6;
mx6 = (int)(DEGTORAD * DRNG6 * RATIO6 + DEGTORAD * DRNG5 * RATI65
+ DEGTORAD * DRNG4 * RATI64) - 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);
fprintf(fph, "#define CP21\t%f\n", CV * -(C21));
fprintf(fph, "#define CP31\t%f\n", CV * -(C31));
fprintf(fph, "#define CP32\t%f\n", CV * -(C32));
fprintf(fph, "#define CP50\t%f\n", CV * -(C50));
}