Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 4 additions & 0 deletions src/emc/kinematics/5axiskins.c
Original file line number Diff line number Diff line change
Expand Up @@ -101,6 +101,8 @@ static int fiveaxis_KinematicsForward(const double *joints,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags)
{
(void)fflags;
(void)iflags;
PmCartesian r = s2r(*(haldata->pivot_length) + joints[JW],
joints[JC],
180.0 - joints[JB]);
Expand All @@ -126,6 +128,8 @@ static int fiveaxis_KinematicsInverse(const EmcPose * pos,
const KINEMATICS_INVERSE_FLAGS * iflags,
KINEMATICS_FORWARD_FLAGS * fflags)
{
(void)iflags;
(void)fflags;
PmCartesian r = s2r(*(haldata->pivot_length) + pos->w,
pos->c,
180.0 - pos->b);
Expand Down
4 changes: 4 additions & 0 deletions src/emc/kinematics/corexykins.c
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,8 @@ int kinematicsForward(const double *joints
,const KINEMATICS_FORWARD_FLAGS *fflags
,KINEMATICS_INVERSE_FLAGS *iflags
) {
(void)fflags;
(void)iflags;
pos->tran.x = 0.5 * (joints[0] + joints[1]);
pos->tran.y = 0.5 * (joints[0] - joints[1]);
pos->tran.z = joints[2];
Expand All @@ -40,6 +42,8 @@ int kinematicsInverse(const EmcPose *pos
,const KINEMATICS_INVERSE_FLAGS *iflags
,KINEMATICS_FORWARD_FLAGS *fflags
) {
(void)iflags;
(void)fflags;
joints[0] = pos->tran.x + pos->tran.y;
joints[1] = pos->tran.x - pos->tran.y;
joints[2] = pos->tran.z;
Expand Down
1 change: 1 addition & 0 deletions src/emc/kinematics/cubic.c
Original file line number Diff line number Diff line change
Expand Up @@ -88,6 +88,7 @@ static double interpolateAccel(CUBIC_COEFF coeff, double t)
*/
static double interpolateJerk(CUBIC_COEFF coeff, double t)
{
(void)t;
return 6.0 * coeff.a;
}

Expand Down
7 changes: 6 additions & 1 deletion src/emc/kinematics/genhexkins.c
Original file line number Diff line number Diff line change
Expand Up @@ -329,6 +329,8 @@ static int genhexKinematicsForward(const double * joints,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags)
{
(void)fflags;
(void)iflags;
PmCartesian aw;
PmCartesian InvKinStrutVect,InvKinStrutVectUnit;
PmCartesian q_trans, RMatrix_a, RMatrix_a_cross_Strut;
Expand All @@ -345,7 +347,7 @@ static int genhexKinematicsForward(const double * joints,

int iterate = 1;
int i;
int iteration = 0;
unsigned iteration = 0;

genhex_read_hal_pins();

Expand Down Expand Up @@ -491,6 +493,8 @@ static int genhexKinematicsInverse(const EmcPose * pos,
const KINEMATICS_INVERSE_FLAGS * iflags,
KINEMATICS_FORWARD_FLAGS * fflags)
{
(void)iflags;
(void)fflags;

PmCartesian aw, temp;
PmCartesian InvKinStrutVect, InvKinStrutVectUnit;
Expand Down Expand Up @@ -541,6 +545,7 @@ int genhexKinematicsSetup(const int comp_id,
const char* coordinates,
kparms* kp)
{
(void)coordinates;
int i,res=0;

haldata = hal_malloc(sizeof(struct haldata));
Expand Down
2 changes: 1 addition & 1 deletion src/emc/kinematics/kinematics.h
Original file line number Diff line number Diff line change
Expand Up @@ -165,7 +165,7 @@ extern int kinematicsSwitch(int switchkins_type);

#define KINS_NOT_SWITCHABLE \
extern int kinematicsSwitchable() {return 0;} \
extern int kinematicsSwitch(int switchkins_type) {return 0;} \
extern int kinematicsSwitch(int switchkins_type) { (void)switchkins_type; return 0;} \
EXPORT_SYMBOL(kinematicsSwitchable); \
EXPORT_SYMBOL(kinematicsSwitch);

Expand Down
5 changes: 5 additions & 0 deletions src/emc/kinematics/kins_util.c
Original file line number Diff line number Diff line change
Expand Up @@ -283,6 +283,7 @@ int identityKinematicsSetup(const int comp_id,
const char* coordinates,
kparms* kp)
{
(void)comp_id;
int axis_idx_for_jno[EMCMOT_MAX_JOINTS];
int jno;
int show=0;
Expand Down Expand Up @@ -330,6 +331,8 @@ int identityKinematicsForward(const double *joints,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags)
{
(void)fflags;
(void)iflags;
if (!identity_kinematics_initialized) {
rtapi_print_msg(RTAPI_MSG_ERR,
"identityKinematicsForward: not initialized\n");
Expand All @@ -346,6 +349,8 @@ int identityKinematicsInverse(const EmcPose * pos,
const KINEMATICS_INVERSE_FLAGS * iflags,
KINEMATICS_FORWARD_FLAGS * fflags)
{
(void)iflags;
(void)fflags;
if (!identity_kinematics_initialized) {
rtapi_print_msg(RTAPI_MSG_ERR,
"identityKinematicsInverse: not initialized\n");
Expand Down
4 changes: 4 additions & 0 deletions src/emc/kinematics/lineardeltakins.c
Original file line number Diff line number Diff line change
Expand Up @@ -32,13 +32,17 @@ int kinematicsForward(const double * joints,
EmcPose * pos,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags) {
(void)fflags;
(void)iflags;
set_geometry(*haldata->r, *haldata->l);
return kinematics_forward(joints, pos);
}

int kinematicsInverse(const EmcPose *pos, double *joints,
const KINEMATICS_INVERSE_FLAGS *iflags,
KINEMATICS_FORWARD_FLAGS *fflags) {
(void)iflags;
(void)fflags;
set_geometry(*haldata->r, *haldata->l);
return kinematics_inverse(pos, joints);
}
Expand Down
4 changes: 4 additions & 0 deletions src/emc/kinematics/maxkins.c
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,8 @@ int kinematicsForward(const double *joints,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags)
{
(void)fflags;
(void)iflags;
// B correction
double zb = (*(haldata->pivot_length) + joints[8]) * cos(d2r(joints[4]));
double xb = (*(haldata->pivot_length) + joints[8]) * sin(d2r(joints[4]));
Expand Down Expand Up @@ -72,6 +74,8 @@ int kinematicsInverse(const EmcPose * pos,
const KINEMATICS_INVERSE_FLAGS * iflags,
KINEMATICS_FORWARD_FLAGS * fflags)
{
(void)iflags;
(void)fflags;
// B correction
double zb = (*(haldata->pivot_length) + pos->w) * cos(d2r(pos->b));
double xb = (*(haldata->pivot_length) + pos->w) * sin(d2r(pos->b));
Expand Down
6 changes: 5 additions & 1 deletion src/emc/kinematics/pentakins.c
Original file line number Diff line number Diff line change
Expand Up @@ -242,6 +242,8 @@ int kinematicsForward(const double * joints,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags)
{
(void)fflags;
(void)iflags;

// PmCartesian aw;
// PmCartesian InvKinStrutVect,InvKinStrutVectUnit;
Expand All @@ -260,7 +262,7 @@ int kinematicsForward(const double * joints,

int iterate = 1;
int i, j;
int iteration = 0;
unsigned iteration = 0;

pentakins_read_hal_pins();

Expand Down Expand Up @@ -372,6 +374,8 @@ int kinematicsInverse(const EmcPose * pos,
const KINEMATICS_INVERSE_FLAGS * iflags,
KINEMATICS_FORWARD_FLAGS * fflags)
{
(void)iflags;
(void)fflags;

double coord[NUM_STRUTS];

Expand Down
2 changes: 2 additions & 0 deletions src/emc/kinematics/pumakins.c
Original file line number Diff line number Diff line change
Expand Up @@ -40,6 +40,7 @@ static int pumaKinematicsForward(const double * joint,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags)
{
(void)fflags;
double s1, s2, s3, s4, s5, s6;
double c1, c2, c3, c4, c5, c6;
double s23;
Expand Down Expand Up @@ -328,6 +329,7 @@ int pumaKinematicsSetup(const int comp_id,
const char* coordinates,
kparms* kp)
{
(void)coordinates;
int res=0;

haldata = hal_malloc(sizeof(*haldata));
Expand Down
4 changes: 4 additions & 0 deletions src/emc/kinematics/rosekins.c
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,8 @@ int kinematicsForward(const double *joints,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags)
{
(void)fflags;
(void)iflags;
double radius,z,theta;

radius = joints[0];
Expand All @@ -68,6 +70,8 @@ int kinematicsInverse(const EmcPose * pos,
const KINEMATICS_INVERSE_FLAGS * iflags,
KINEMATICS_FORWARD_FLAGS * fflags)
{
(void)iflags;
(void)fflags;
// There is a potential problem when accumulating bigtheta -- loss of
// precision based on size of mantissa -- but in practice, it is probably ok

Expand Down
4 changes: 4 additions & 0 deletions src/emc/kinematics/rotarydeltakins.c
Original file line number Diff line number Diff line change
Expand Up @@ -36,13 +36,17 @@ int kinematicsForward(const double * joints,
EmcPose * pos,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags) {
(void)fflags;
(void)iflags;
set_geometry(*haldata->pfr, *haldata->tl, *haldata->sl, *haldata->fr);
return kinematics_forward(joints, pos);
}

int kinematicsInverse(const EmcPose *pos, double *joints,
const KINEMATICS_INVERSE_FLAGS *iflags,
KINEMATICS_FORWARD_FLAGS *fflags) {
(void)iflags;
(void)fflags;
set_geometry(*haldata->pfr, *haldata->tl, *haldata->sl, *haldata->fr);
return kinematics_inverse(pos, joints);
}
Expand Down
4 changes: 4 additions & 0 deletions src/emc/kinematics/rotatekins.c
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,8 @@ int kinematicsForward(const double *joints,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags)
{
(void)fflags;
(void)iflags;
double c_rad = -joints[5]*M_PI/180;
pos->tran.x = joints[0] * cos(c_rad) - joints[1] * sin(c_rad);
pos->tran.y = joints[0] * sin(c_rad) + joints[1] * cos(c_rad);
Expand All @@ -39,6 +41,8 @@ int kinematicsInverse(const EmcPose * pos,
const KINEMATICS_INVERSE_FLAGS * iflags,
KINEMATICS_FORWARD_FLAGS * fflags)
{
(void)iflags;
(void)fflags;
double c_rad = pos->c*M_PI/180;
joints[0] = pos->tran.x * cos(c_rad) - pos->tran.y * sin(c_rad);
joints[1] = pos->tran.x * sin(c_rad) + pos->tran.y * cos(c_rad);
Expand Down
2 changes: 2 additions & 0 deletions src/emc/kinematics/scarakins.c
Original file line number Diff line number Diff line change
Expand Up @@ -77,6 +77,7 @@ int scaraKinematicsForward(const double * joint,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags)
{
(void)fflags;
double a0, a1, a3;
double x, y, z, c;

Expand Down Expand Up @@ -181,6 +182,7 @@ static int scaraKinematicsSetup(const int comp_id,
const char* coordinates,
kparms* kp)
{
(void)coordinates;
int res=0;

haldata = hal_malloc(sizeof(*haldata));
Expand Down
4 changes: 4 additions & 0 deletions src/emc/kinematics/scorbot-kins.c
Original file line number Diff line number Diff line change
Expand Up @@ -79,6 +79,8 @@ int kinematicsForward(
const KINEMATICS_FORWARD_FLAGS *fflags,
KINEMATICS_INVERSE_FLAGS *iflags
) {
(void)fflags;
(void)iflags;
EmcPose j1_vector; // the vector from j0 ("base") to joint 1 ("shoulder", end of link 0)
EmcPose j2_vector; // the vector from j1 ("shoulder") to joint 2 ("elbow", end of link 1)
EmcPose j3_vector; // the vector from j2 ("elbow") to joint 3 ("wrist", end of link 2)
Expand Down Expand Up @@ -136,6 +138,8 @@ int kinematicsInverse(
const KINEMATICS_INVERSE_FLAGS *iflags,
KINEMATICS_FORWARD_FLAGS *fflags
) {
(void)iflags;
(void)fflags;
// EmcPose j1_cart;
double distance_to_cp, distance_to_center;
double r_j1, z_j1; // (r_j1, z_j1) is the location of J1 in the RZ plane
Expand Down
2 changes: 2 additions & 0 deletions src/emc/kinematics/tripodkins.c
Original file line number Diff line number Diff line change
Expand Up @@ -128,6 +128,7 @@ int kinematicsForward(const double * joints,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags)
{
(void)iflags;
#define AD (joints[0])
#define BD (joints[1])
#define CD (joints[2])
Expand Down Expand Up @@ -184,6 +185,7 @@ int kinematicsInverse(const EmcPose * pos,
const KINEMATICS_INVERSE_FLAGS * iflags,
KINEMATICS_FORWARD_FLAGS * fflags)
{
(void)iflags;
#define AD (joints[0])
#define BD (joints[1])
#define CD (joints[2])
Expand Down
8 changes: 8 additions & 0 deletions src/emc/kinematics/trtfuncs.c
Original file line number Diff line number Diff line change
Expand Up @@ -153,6 +153,8 @@ int xyzacKinematicsForward(const double *joints,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags)
{
(void)fflags;
(void)iflags;
double x_rot_point = *(haldata->x_rot_point);
double y_rot_point = *(haldata->y_rot_point);
double z_rot_point = *(haldata->z_rot_point);
Expand Down Expand Up @@ -199,6 +201,8 @@ int xyzacKinematicsInverse(const EmcPose * pos,
const KINEMATICS_INVERSE_FLAGS * iflags,
KINEMATICS_FORWARD_FLAGS * fflags)
{
(void)iflags;
(void)fflags;
double x_rot_point = *(haldata->x_rot_point);
double y_rot_point = *(haldata->y_rot_point);
double z_rot_point = *(haldata->z_rot_point);
Expand Down Expand Up @@ -257,6 +261,8 @@ int xyzbcKinematicsForward(const double *joints,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags)
{
(void)fflags;
(void)iflags;
// Note: 'principal' joints are used
double x_rot_point = *(haldata->x_rot_point);
double y_rot_point = *(haldata->y_rot_point);
Expand Down Expand Up @@ -303,6 +309,8 @@ int xyzbcKinematicsInverse(const EmcPose * pos,
const KINEMATICS_INVERSE_FLAGS * iflags,
KINEMATICS_FORWARD_FLAGS * fflags)
{
(void)iflags;
(void)fflags;
double x_rot_point = *(haldata->x_rot_point);
double y_rot_point = *(haldata->y_rot_point);
double z_rot_point = *(haldata->z_rot_point);
Expand Down
1 change: 1 addition & 0 deletions src/emc/motion/command.c
Original file line number Diff line number Diff line change
Expand Up @@ -386,6 +386,7 @@ STATIC int is_feed_type(int motion_type)
*/
void emcmotCommandHandler_locked(void *arg, long servo_period)
{
(void)arg;
int joint_num, spindle_num;
int n,s0,s1;
emcmot_joint_t *joint;
Expand Down
3 changes: 2 additions & 1 deletion src/emc/motion/control.c
Original file line number Diff line number Diff line change
Expand Up @@ -206,6 +206,7 @@ static void handle_kinematicsSwitch(void);
*/
void emcmotController(void *arg, long period)
{
(void)arg;
static int do_once = 1;
if (do_once) {
pcmd_p[0] = &(emcmotStatus->carte_pos_cmd.tran.x);
Expand Down Expand Up @@ -236,7 +237,7 @@ void emcmotController(void *arg, long period)
/* calculate servo period as a double - period is in integer nsec */
servo_period = period * 0.000000001;

if(period != last_period) {
if(period != (long)last_period) {
emcmotSetCycleTime(period);
last_period = period;
}
Expand Down
Loading