APPENDICE A - unipv · 100 APPENDICE A PROGRAMMI UTILIZZATI Questa appendice ha lo scopo di illustrare i programmi utilizzati per il controllo del robot nelle diverse fasi di sperimentazione

100

APPENDICE A

PROGRAMMI UTILIZZATI

Questa appendice ha lo scopo di illustrare i programmi utilizzati per il controllo del robot nelle

diverse fasi di sperimentazione.

A.1 COMPENSAZIONE DELLA COPPIA GRAVITAZIONALE

M4CGRAV.C/********************************************************************

M4CGRAV.C COMPENSAZIONE DELLA GRAVITA' ASSE 3 ********************************************************************//*** include files ***/

#include <stdio.h>#include <conio.h>#include <string.h>#include <stdlib.h>

#include "user1ms.h"/*** macros & defines ***/

#define TIME_OUT 5000

#define DELAY 4000/*** global variables ***/

long huge *Buffers[NUM_AXES]; /* buffers where the motor shaft */ /* set-points are stored */

long DimBuffers[NUM_AXES]; /* sizes (i.e. # of elements) of */ /* the buffers where the motor */ /* shaft set-points are stored */

long InitPos[NUM_AXES]; /* motor shaft initial positions */

int NumRecMotPos; /* # of motor shaft positions */ /* to be recorded */

int NumRecCurrSP; /* # of motor current set-points */ /* to be recorded */

short Currents1[NUM_AXES];

long VelRif[NUM_AXES];

long OldVelRif[NUM_AXES];

long VelMot[NUM_AXES];

long gravdat[NUM_AXES];

101

long Velrif[NUM_AXES];

long Coppiag;

/*** function prototypes ***/

void main(void);void UserInput(void);void FreeBuffers(void);void ComputeSetPoints(long *);void InitPID1(long []);int PID1(long [],long []);

void main(void)˝{˝ long MotorPositions[NUM_AXES]; /* motor shaft positions */˝

/* [bit resolver] */˝

˝ long PositionSetPoints[NUM_AXES]; /* motor shaft set-points */

/* [bit resolver] */

int DelayCounter = DELAY; /* counter */

int PIDError = 0; /* if different from 0 the */ /* following error for one */ /* joint is too big */

int i; /* counter */

/* get the user input data */ UserInput();

/* display a message for the user */ printf("\n\nPress any key to start the test ..."); getch(); printf("\n\nTest started (press any key to terminate) ...");

/* initialize the PC-C3G communication and the data recording */ InitRecordMotorPositions(NumRecMotPos); InitRecordCurrents(NumRecCurrSP); B3Init();

/* read the motor shaft initial positions */ while(IntActive == 0)

; if(IntActive) { ReadMotorPositions(InitPos); IntActive = 0; }

/* initilize the PID regulators */ InitPID1(InitPos); InitPID(InitPos); /* send the DRIVE ON request to the C3G */ if(DriveOn(TIME_OUT) == 0) {

/* wait some time before starting the motion */ IntActive = 0; do {

102

if(IntActive) { ReadMotorPositions(MotorPositions); PIDError = PID(InitPos,MotorPositions); WriteCurrents(Currents); DelayCounter--; SetWatchdog(); IntActive = 0; }

} while(DelayCounter);

/* motion !! */ IntActive = 0; do {

if(IntActive) { ReadMotorPositions(MotorPositions); ComputeSetPoints(PositionSetPoints); PIDError = PID1(PositionSetPoints,MotorPositions);

WriteCurrents(Currents1); SetWatchdog();

SaveMotorPositions(gravdat); /* SaveCurrents(Currents1); */

IntActive = 0; }

} while((!kbhit()) && (!PIDError)); }

/* send the DRIVE OFF request to the C3G */ DriveOff(TIME_OUT);

/* terminate */ B3Terminate(); FreeBuffers();

/* write recorded data on disk */ RecordMotorPositions("compgr3.dat");

/* in an error occurred then display a message */ if(PIDError) printf("\n\nMax following error exceeded [joint %d]\n\n",-PIDError);}

/******************************************************************

This function gets the user input data.

******************************************************************/void UserInput(void){ char s[80]; int JointNum; long NumData,k; float dummy; FILE *fp; int n,i;

for(i=0;i<NUM_AXES;i++) { Buffers[i] = NULL; DimBuffers[i] = 0L; }

printf("\n\nPlease specify the motor shaft set-points\n");

do { printf("\nJoint [1-6, 0 to finish]: "); gets(s);

103

JointNum = atoi(s); if((JointNum >= 1) && (JointNum <= 6) &&

(Buffers[JointNum-1] == NULL)) {

printf("\nFile name: "); gets(s); fp = fopen(s,"rt"); if(fp == NULL) { fprintf(stderr,"\n\nCannot open %s\n\n",s); exit(1); } NumData = 0; do { n = fscanf(fp,"%e",&dummy); if(n == 1)

NumData++; } while(n != EOF); fclose(fp); DimBuffers[JointNum-1] = NumData; Buffers[JointNum-1] = (long huge *)AllocBuffer(NumData,sizeof(long)); if(Buffers[JointNum-1] == NULL) { fprintf(stderr,"\n\nNot enough memory\n\n"); exit(1); } fp = fopen(s,"rt"); if(fp == NULL) { fprintf(stderr,"\n\nCannot open %s\n\n",s); exit(1); } k = 0; do { n = fscanf(fp,"%e",&dummy); if((n == 1) && (k < NumData)) {

Buffers[JointNum-1][k] = (long)dummy; k++;

} } while((n != EOF) && (k < NumData)); fclose(fp);

} } while(JointNum != 0);

printf("\n\nData Recording\n\n"); do { printf("Motor shaft positions - number of samples: "); gets(s); NumRecMotPos = atoi(s); } while(NumRecMotPos <= 0); do { printf("Motor current set-points - number of samples: "); gets(s); NumRecCurrSP = atoi(s); } while(NumRecCurrSP <= 0);}

/******************************************************************

This function computes the motor shaft set-points.

******************************************************************/void ComputeSetPoints(long *PositionSetPoints){ int i; static long k = 0;

for(i=0;i<NUM_AXES;i++) { if((Buffers[i]!=NULL)&&(i<5)) {

104

if(k < DimBuffers[i]) PositionSetPoints[i] = InitPos[i] + Buffers[i][k]; else PositionSetPoints[i] = InitPos[i] + Buffers[i][DimBuffers[i]-1];

} else

PositionSetPoints[i] = InitPos[i];

if((Buffers[i]!=NULL)&&(i==5)) { if(k < DimBuffers[5]){

Coppiag = Buffers[5][k]; PositionSetPoints[5] = InitPos[5]; }

else { Coppiag = Buffers[5][DimBuffers[5]-1];

PositionSetPoints[5] = InitPos[5]; } } }

k++;}

/******************************************************************

This function frees the buffesr used to store the motor shaft set-points.

******************************************************************/void FreeBuffers(void){ int i;

for(i=0;i<NUM_AXES;i++) if(Buffers[i] != NULL)

FreeBuffer((PUNT)Buffers[i]);}

/*************************************************************************

PID1.C - Versione per SMART-3 S2 modificata per misura della gravità

*************************************************************************/

/*** include files ***/

#include <stdio.h>#include <math.h>#include <conio.h>

#include "user1ms.h"

/*** defines ***/

#define __PID1__

#define MAX_ERRPOS 131072L /* massimo errore di posizione */ /* in tacche resolver (corrisponde */ /* a due giri motore) */

#define H 0.001F /* passo di campionamento */

#define cost -0.0000016F

/*** global variables ***/

105

double KP1[NUM_AXES]; /* guadagni dei regolatori di posizione */

double KV1[NUM_AXES]; /* guadagni degli anticipi di velocita' */

double KPV1[NUM_AXES]; /* guadagni dei regolatori di velocita' */

double KI1[NUM_AXES]; /* azioni integrali dei regolatori */ /* di velocita' */

double OldInt1[NUM_AXES]; /* stati degli integratori */ /* (nei regolatori di velocita') */

double OldPosSP1[NUM_AXES]; /* set-points di posizione al passo */ /* precedente */

double OldMotPos1[NUM_AXES]; /* posizioni degli assi dei motori al */ /* passo precedente */

short MaxCurrents1[NUM_AXES]; /* valori max dei set-points di corrente */ /* in unita' DAC */

long lErrPos1[NUM_AXES]; /* errori di posizione [unita' resolver] */

/*** function prototyping ***/

void InitPID1(long []);int PID1(long [],long[]);

/******************************************************************

Routine per l'inizializzazione dei parametri dei PID e dei valori massimi dei set-points di corrente.

******************************************************************/void InitPID1(long MotPos1[]){ int i;

/* inizializzazione dei guadagni degli anticipi di velocita' */ KV1[0] = 1.0F; KV1[1] = 0.65F; KV1[2] = 1.0F; KV1[3] = 0.6F; KV1[4] = 0.0F; KV1[5] = 0.5F;

/* inizializzazione dei parametri dei regolatori */ KP1[0] = 20.0F*H; KI1[0] = 0.008623F; KPV1[0] = 0.005F/H;

KP1[1] = 12.0F*H; KI1[1] = 0.01652F; KPV1[1] = 0.0026125F/H;

KP1[2] = 10.0F*H; KI1[2] = 0.02468F; KPV1[2] = 0.0025936F/H;

KP1[3] = 15.0F*H; KI1[3] = 0.04959F; KPV1[3] = 0.0025817F/H;

KP1[4] = 40.0F*H; KI1[4] = 0.02698F; KPV1[4] = 0.0028353F/H;

106

KP1[5] = 20.0F*H; KI1[5] = 0.01011F; KPV1[5] = 0.0003924F/H;

/* inizializzazione dello stato degli integratori */ for(i=0;i<NUM_AXES;i++) OldInt1[i] = 0.0F;

/* inizializzazione delle variabili che */ /* contengono i set-points al passo precedente */ for(i=0;i<NUM_AXES;i++) OldPosSP1[i] = (double)MotPos1[i];

/* inizializzazione delle variabili che contengono le */ /* posizioni degli assi dei motori al passo precedente */ for(i=0;i<NUM_AXES;i++) OldMotPos1[i] = (double)MotPos1[i];

/* inizializzazione dei valori max dei set-points di corrente */ MaxCurrents1[0] = 1569; MaxCurrents1[1] = 1228; MaxCurrents1[2] = 1228; MaxCurrents1[3] = 938; MaxCurrents1[4] = 938; MaxCurrents1[5] = 682;}

/********************************************************************

La subroutine calcola l'azione di regolazione dei regolatori d'asse del robot SMART-3 S2.

********************************************************************/int PID1(long lPosSP1[],long lMotPos1[]){ double PosSP1[NUM_AXES]; /* set-points di posizione [virgola mobile] */ double MotPos1[NUM_AXES]; /* posizioni assi motori [virgola mobile] */ double ErrPos1[NUM_AXES]; /* errori di posizione [virgola mobile] */ double MotVel1[NUM_AXES]; /* velocita' assi motori [virgola mobile] */ double VelSP1[NUM_AXES]; /* set-points di velocita' [virgola mobile] */ double VelFF1[NUM_AXES]; /* anticipi di velocita' [virgola mobile] */ double ErrVel1[NUM_AXES]; /* errori di velocita' [virgola mobile] */ double IntOut1[NUM_AXES]; /* uscite degli integratori [virgola mobile] */ double CurrSP1[NUM_AXES]; /* set-points di corrente [virgola mobile] */ double Compg; int i;

for(i=0;i<NUM_AXES;i++) {

/* calcolo degli errori di posizione */ lErrPos1[i] = lPosSP1[i] - lMotPos1[i];

/* se almeno uno degli errori di posizione eccede il massimo *//* valore ammissibile si esce segnalando l'errore */

if(labs(lErrPos1[i]) > MAX_ERRPOS) return(-(i+1));

/* conversione degli errori di posizione, dei set-points di *//* posizione e delle posizioni degli assi dei motori da unita' *//* resolver a virgola mobile */

ErrPos1[i] = (double)lErrPos1[i]; PosSP1[i] = (double)lPosSP1[i]; MotPos1[i] = (double)lMotPos1[i]; Compg = (double)Coppiag;

/* calcolo dei set-points di velocita' */ VelSP1[i] = KP1[i]*ErrPos1[i];

107

/* calcolo degli anticipi di velocita' */ VelFF1[i] = KV1[i]*(PosSP1[i] - OldPosSP1[i]); Velrif[i] = PosSP1[i] - OldPosSP1[i]; Velrif[i] = Velrif[i]/H;

/* calcolo delle velocita' degli assi dei motori */ MotVel1[i] = MotPos1[i] - OldMotPos1[i];

VelMot[i] = MotVel1[i]/H;

/* calcolo degli errori di velocita' */ ErrVel1[i] = VelSP1[i] + VelFF1[i] - MotVel1[i];

/* calcolo delle uscite degli integratori *//* (nei regolatori di velocita') */

IntOut1[i] = OldInt1[i] + KI1[i]*ErrVel1[i];

if(i!=2) CurrSP1[i] = KPV1[i]*ErrVel1[i] + IntOut1[i];

if(i==2) { CurrSP1[i] =KPV1[i]*ErrVel1[i] + IntOut1[i] + Compg; gravdat[0] =(short)(KPV1[i]*ErrVel1[i] + IntOut1[i]); gravdat[1] =VelMot[2]; gravdat[2] =MotPos1[2]; }

/* conversione dei set-points di corrente da double a short */ Currents1[i] = (short)CurrSP1[i];

/* saturazione delle uscite degli integratori *//* e implementazione dell'anti wind-up */

if(Currents1[i] > MaxCurrents1[i]) Currents1[i] = MaxCurrents1[i]; else if(Currents1[i] < -MaxCurrents1[i]) Currents1[i] = -MaxCurrents1[i];

else OldInt1[i] = IntOut1[i];

/* aggiornamento di OldPosSP e OldMotPos */ OldPosSP1[i] = PosSP1[i]; OldMotPos1[i] = MotPos1[i]; } return(0);}

A.2 COMPENSAZIONE DEI FENOMENI DI ATTRITO

M4ATTRI.C/********************************************************************

108

M4ATTRI.C COMPENSAZIONE DELL'INERZIA ********************************************************************//*** include files ***/

#include <stdio.h>#include <conio.h>#include <string.h>#include <stdlib.h>#include "user1ms.h"

/*** macros & defines ***/


#define DELAY 4000







short Currents1[NUM_AXES]; /* set-points di corrente */

long VelRif[NUM_AXES];

long OldVelRif[NUM_AXES];

long VelMot[NUM_AXES];

long Inerzia[NUM_AXES];

long Ifbvm[NUM_AXES];

long Velrif[NUM_AXES];

double B[NUM_AXES];


void main(void);void UserInput(void);void FreeBuffers(void);void ComputeSetPoints(long *);void InitPID1(long []);int PID1(long [],long []);

void main(void){ long MotorPositions[NUM_AXES]; /* motor shaft positions */


long PositionSetPoints[NUM_AXES]; /* motor shaft set-points */ /* [bit resolver] */

109




for(i=0;i<NUM_AXES;i++) { Inerzia[i] = 0; VelMot[i] = 0; Velrif[i] = 0; Ifbvm[i] = 0; B[i] = 0.0F; }

B[0] = 0.00012F; /* get the user input data */ UserInput();





/* initilize the PID regulators */ InitPID1(InitPos); InitPID(InitPos);

/* send the DRIVE ON request to the C3G */ if(DriveOn(TIME_OUT) == 0) {





if(IntActive) { ReadMotorPositions(MotorPositions); ComputeSetPoints(PositionSetPoints); PIDError = PID1(PositionSetPoints,MotorPositions);

110

WriteCurrents(Currents1); SetWatchdog();

SaveMotorPositions(Ifbvm); SaveCurrents(Currents1);

IntActive = 0; }




/* write recorded data on disk */ /* scrivo in comp.dat corrente di feedback, velocit… e posizione motore */ RecordMotorPositions("pprova2.dat"); RecordCurrents("cprova2.dat");


/******************************************************************





do { printf("\nJoint [1-6, 0 to finish]: "); gets(s); JointNum = atoi(s); if((JointNum >= 1) && (JointNum <= 6) &&



NumData++; } while(n != EOF);

111

fclose(fp); DimBuffers[JointNum-1] = NumData; Buffers[JointNum-1] = (long huge *)AllocBuffer(NumData,sizeof(long)); if(Buffers[JointNum-1] == NULL) { fprintf(stderr,"\n\nNot enough memory\n\n"); exit(1); } fp = fopen(s,"rt"); if(fp == NULL) { fprintf(stderr,"\n\nCannot open %s\n\n",s); exit(1); } k = 0; do { n = fscanf(fp,"%e",&dummy); if((n == 1) && (k < NumData)) {


} } while((n != EOF) && (k < NumData)); fclose(fp);

} } while(JointNum != 0);


/******************************************************************



for(i=0;i<NUM_AXES;i++) { if(Buffers[i] != NULL) {


} else

PositionSetPoints[i] = InitPos[i]; }

k++;}

/******************************************************************


******************************************************************/

112

void FreeBuffers(void){ int i;

for(i=0;i<NUM_AXES;i++) if(Buffers[i] != NULL)

FreeBuffer((PUNT)Buffers[i]);}

/*************************************************************************

PID1.C - Versione per SMART-3 S2 modificata per misura attrito

*************************************************************************/


#include <stdio.h>#include <math.h>#include <conio.h>

#include "user1ms.h"

/*** defines ***/

#define __PID1__




double KP1[NUM_AXES]; /* guadagni dei regolatori di posizione */

double KV1[NUM_AXES]; /* guadagni degli anticipi di velocita' */

double KPV1[NUM_AXES]; /* guadagni dei regolatori di velocita' */

double KI1[NUM_AXES]; /* azioni integrali dei regolatori */ /* di velocita' */

double OldInt1[NUM_AXES]; /* stati degli integratori */ /* (nei regolatori di velocita') */

double OldPosSP1[NUM_AXES]; /* set-points di posizione al passo */ /* precedente */

double OldMotPos1[NUM_AXES]; /* posizioni degli assi dei motori al */ /* passo precedente */

short MaxCurrents1[NUM_AXES]; /* valori max dei set-points di corrente */ /* in unita' DAC */

long lErrPos1[NUM_AXES]; /* errori di posizione [unita' resolver] */


void InitPID1(long []);int PID1(long [],long[]);

/******************************************************************

Routine per l'inizializzazione dei parametri dei PID e dei

113

valori massimi dei set-points di corrente.

******************************************************************/void InitPID1(long MotPos1[]){ int i;

/* inizializzazione dei guadagni degli anticipi di velocita' */ KV1[0] = 1.0F; KV1[1] = 0.65F; KV1[2] = 0.7F; KV1[3] = 0.6F; KV1[4] = 0.0F; KV1[5] = 0.5F;

/* inizializzazione dei parametri dei regolatori */ KP1[0] = 20.0F*H; KI1[0] = 0.008623F; KPV1[0] = 0.005F/H;

KP1[1] = 12.0F*H; KI1[1] = 0.01652F; KPV1[1] = 0.0026125F/H;

KP1[2] = 10.0F*H; KI1[2] = 0.02468F; KPV1[2] = 0.0025936F/H;

KP1[3] = 15.0F*H; KI1[3] = 0.04959F; KPV1[3] = 0.0025817F/H;

KP1[4] = 40.0F*H; KI1[4] = 0.02698F; KPV1[4] = 0.0028353F/H;

KP1[5] = 20.0F*H; KI1[5] = 0.01011F; KPV1[5] = 0.0003924F/H;

/* inizializzazione dello stato degli integratori */ for(i=0;i<NUM_AXES;i++) OldInt1[i] = 0.0F;

/* inizializzazione delle variabili che */ /* contengono i set-points al passo precedente */ for(i=0;i<NUM_AXES;i++) OldPosSP1[i] = (double)MotPos1[i];

/* inizializzazione delle variabili che contengono le */ /* posizioni degli assi dei motori al passo precedente */ for(i=0;i<NUM_AXES;i++) OldMotPos1[i] = (double)MotPos1[i];

/* inizializzazione dei valori max dei set-points di corrente */ MaxCurrents1[0] = 1569; MaxCurrents1[1] = 1228; MaxCurrents1[2] = 1228; MaxCurrents1[3] = 938; MaxCurrents1[4] = 938; MaxCurrents1[5] = 682;}

/********************************************************************


114

********************************************************************/int PID1(long lPosSP1[],long lMotPos1[]){ double PosSP1[NUM_AXES]; /* set-points di posizione [virgola mobile] */ double MotPos1[NUM_AXES]; /* posizioni assi motori [virgola mobile] */ double ErrPos1[NUM_AXES]; /* errori di posizione [virgola mobile] */ double MotVel1[NUM_AXES]; /* velocita' assi motori [virgola mobile] */ double VelSP1[NUM_AXES]; /* set-points di velocita' [virgola mobile] */ double VelFF1[NUM_AXES]; /* anticipi di velocita' [virgola mobile] */ double ErrVel1[NUM_AXES]; /* errori di velocita' [virgola mobile] */ double IntOut1[NUM_AXES]; /* uscite degli integratori [virgola mobile] */ double CurrSP1[NUM_AXES]; /* set-points di corrente [virgola mobile] */ long Torque[NUM_AXES]; static long tc=0; /* passo di campionamento */ long Tc = 75; /* componente di attrito coulombiano */ long Tcomp[NUM_AXES]; int i;


if(i!=0) Tcomp[i]=0;

/* calcolo degli errori di posizione */ lErrPos1[i] = lPosSP1[i] - lMotPos1[i];

/* se almeno uno degli errori di posizione eccede il massimo *//* valore ammissibile si esce segnalando l'errore */

if(labs(lErrPos1[i]) > MAX_ERRPOS) return(-(i+1));

/* conversione degli errori di posizione, dei set-points di *//* posizione e delle posizioni degli assi dei motori da unita' *//* resolver a virgola mobile */

ErrPos1[i] = (double)lErrPos1[i]; PosSP1[i] = (double)lPosSP1[i]; MotPos1[i] = (double)lMotPos1[i];

/* calcolo dei set-points di velocita' */ VelSP1[i] = KP1[i]*ErrPos1[i];

/* calcolo degli anticipi di velocita' */ VelFF1[i] = KV1[i]*(PosSP1[i] - OldPosSP1[i]); Velrif[i] = PosSP1[i] - OldPosSP1[i]; Velrif[i] = Velrif[i]/H;

/* calcolo delle velocita' degli assi dei motori */ MotVel1[i] = MotPos1[i] - OldMotPos1[i];

VelMot[i] = MotVel1[i]/H;

/* compensazione inerzia */ if(i==0) { if(tc<1000) Inerzia[0] = 52; else { if((tc>1000)&&(tc<=3000)) Inerzia[0] = -52; else { if((tc>3000)&&(tc<=4000)) Inerzia[0] = 52; else if (tc>4000) { Inerzia[0] = 0;

115

} } } }

/* calcolo degli errori di velocita' */ ErrVel1[i] = VelSP1[i] + VelFF1[i] - MotVel1[i];

/* calcolo delle uscite degli integratori *//* (nei regolatori di velocita') */

IntOut1[i] = OldInt1[i] + KI1[i]*ErrVel1[i];

/* calcolo delle uscite dei regolatori di velocita' */ /* stima della coppia motrice */ Torque[i] = IntOut1[i] + KPV1[i]*ErrVel1[i]; /* + Inerzia[i]; */

/* calcolo della compensazione dell'attrito viscoso */ /* e coulombiano */ /* if ((i==0)&&(tc<4000)) { if(Velrif[0]>=0) Tcomp[0] = Tc; else { if(Velrif[0]<0) Tcomp[0] = -Tc; } } else Tcomp[0] = 0; */

Tcomp[0] = Tc;

/* somma azione feedback e feedforward di compensazione */ CurrSP1[i] = Torque[i] + Tcomp[i] + B[i]*Velrif[i];

if (i==0) { Ifbvm[0] =(short)(KPV1[0]*ErrVel1[0] + IntOut1[0]); Ifbvm[1] =VelMot[0]; Ifbvm[2] =MotPos1[0]; }

/* conversione dei set-points di corrente da double a short */ Currents1[i] = (short)CurrSP1[i];

/* saturazione delle uscite degli integratori *//* e implementazione dell'anti wind-up */

if(Currents1[i] > MaxCurrents1[i]) Currents1[i] = MaxCurrents1[i]; else if(Currents1[i] < -MaxCurrents1[i]) Currents1[i] = -MaxCurrents1[i];

else OldInt1[i] = IntOut1[i];

/* aggiornamento di OldPosSP e OldMotPos */ OldPosSP1[i] = PosSP1[i]; OldMotPos1[i] = MotPos1[i]; } tc++; return(0);}

116

117

A.3 PROGRAMMA DI CONTROLLO TECNOSPAZIO

M4MAIN.C/********************************************************************

M4MAIN.C

********************************************************************/


#include <stdio.h>#include <conio.h>#include <string.h>#include <stdlib.h>#include "user1ms.h"

/*** macros & defines ***/


#define DELAY 4000








void main(void);void UserInput(void);void FreeBuffers(void);void ComputeSetPoints(long *);

void main(void){ long MotorPositions[NUM_AXES]; /* motor shaft positions */


long PositionSetPoints[NUM_AXES]; /* motor shaft set-points */ /* [bit resolver] */




118

/* get the user input data */ UserInput();





/* initilize the PID regulators */ InitPID(InitPos);

/* send the DRIVE ON request to the C3G */ if(DriveOn(TIME_OUT) == 0) {





if(IntActive) { ReadMotorPositions(MotorPositions); ComputeSetPoints(PositionSetPoints); PIDError = PID(PositionSetPoints,MotorPositions); WriteCurrents(Currents); SetWatchdog(); SaveMotorPositions(MotorPositions); SaveCurrents(Currents); IntActive = 0; }




/* write recorded data on disk */ RecordMotorPositions("motpos.dat"); RecordCurrents("current.dat");

119


/******************************************************************





do { printf("\nJoint [1-6, 0 to finish]: "); gets(s); JointNum = atoi(s); if((JointNum >= 1) && (JointNum <= 6) &&



NumData++; } while(n != EOF); fclose(fp); DimBuffers[JointNum-1] = NumData; Buffers[JointNum-1] = (long huge *)AllocBuffer(NumData,sizeof(long)); if(Buffers[JointNum-1] == NULL) { fprintf(stderr,"\n\nNot enough memory\n\n"); exit(1); } fp = fopen(s,"rt"); if(fp == NULL) { fprintf(stderr,"\n\nCannot open %s\n\n",s); exit(1); } k = 0; do { n = fscanf(fp,"%e",&dummy); if((n == 1) && (k < NumData)) {


} } while((n != EOF) && (k < NumData));

120

fclose(fp); } } while(JointNum != 0);


/******************************************************************



for(i=0;i<NUM_AXES;i++) { if(Buffers[i] != NULL) {


} else

PositionSetPoints[i] = InitPos[i]; }k++;}

/******************************************************************


******************************************************************/void FreeBuffers(void){ int i;for(i=0;i<NUM_AXES;i++) if(Buffers[i] != NULL)

FreeBuffer((PUNT)Buffers[i]);}/*************************************************************************

PID.C - Versione per SMART-3 S2

*************************************************************************/


#include <stdio.h>#include <math.h>#include <conio.h>#include "user1ms.h"

/*** defines ***/

121

#define __PID__




double KP[NUM_AXES]; /* guadagni dei regolatori di posizione */

double KV[NUM_AXES]; /* guadagni degli anticipi di velocita' */

double KPV[NUM_AXES]; /* guadagni dei regolatori di velocita' */

double KI[NUM_AXES]; /* azioni integrali dei regolatori */ /* di velocita' */

double OldInt[NUM_AXES]; /* stati degli integratori */ /* (nei regolatori di velocita') */

double OldPosSP[NUM_AXES]; /* set-points di posizione al passo */ /* precedente */

double OldMotPos[NUM_AXES]; /* posizioni degli assi dei motori al */ /* passo precedente */

short Currents[NUM_AXES]; /* set-points di corrente */

short MaxCurrents[NUM_AXES]; /* valori max dei set-points di corrente */ /* in unita' DAC */

long lErrPos[NUM_AXES]; /* errori di posizione [unita' resolver] */


void InitPID(long []);int PID(long [],long[]);

/******************************************************************

Routine per l'inizializzazione dei parametri dei PID e dei valori massimi dei set-points di corrente.

******************************************************************/void InitPID(long MotPos[]){ int i;

/* inizializzazione dei guadagni degli anticipi di velocita' */ KV[0] = 0.5F; KV[1] = 0.65F; KV[2] = 0.7F; KV[3] = 0.6F; KV[4] = 0.0F; KV[5] = 0.5F;

/* inizializzazione dei parametri dei regolatori */ KP[0] = 15.0F*H; KI[0] = 0.008623F; KPV[0] = 0.0027362F/H;

KP[1] = 12.0F*H; KI[1] = 0.01652F; KPV[1] = 0.0026125F/H;

122

KP[2] = 10.0F*H; KI[2] = 0.02468F; KPV[2] = 0.0025936F/H;

KP[3] = 15.0F*H; KI[3] = 0.04959F; KPV[3] = 0.0025817F/H;

KP[4] = 40.0F*H; KI[4] = 0.02698F; KPV[4] = 0.0028353F/H;

KP[5] = 20.0F*H; KI[5] = 0.01011F; KPV[5] = 0.0003924F/H;

/* inizializzazione dello stato degli integratori */ for(i=0;i<NUM_AXES;i++) OldInt[i] = 0.0F;

/* inizializzazione delle variabili che */ /* contengono i set-points al passo precedente */ for(i=0;i<NUM_AXES;i++) OldPosSP[i] = (double)MotPos[i];

/* inizializzazione delle variabili che contengono le */ /* posizioni degli assi dei motori al passo precedente */ for(i=0;i<NUM_AXES;i++) OldMotPos[i] = (double)MotPos[i];

/* inizializzazione dei valori max dei set-points di corrente */ MaxCurrents[0] = 1569; MaxCurrents[1] = 1228; MaxCurrents[2] = 1228; MaxCurrents[3] = 938; MaxCurrents[4] = 938; MaxCurrents[5] = 682;}

/********************************************************************


********************************************************************/int PID(long lPosSP[],long lMotPos[]){ double PosSP[NUM_AXES]; /* set-points di posizione [virgola mobile] */ double MotPos[NUM_AXES]; /* posizioni assi motori [virgola mobile] */ double ErrPos[NUM_AXES]; /* errori di posizione [virgola mobile] */ double MotVel[NUM_AXES]; /* velocita' assi motori [virgola mobile] */ double VelSP[NUM_AXES]; /* set-points di velocita' [virgola mobile] */ double VelFF[NUM_AXES]; /* anticipi di velocita' [virgola mobile] */ double ErrVel[NUM_AXES]; /* errori di velocita' [virgola mobile] */ double IntOut[NUM_AXES]; /* uscite degli integratori [virgola mobile] */ double CurrSP[NUM_AXES]; /* set-points di corrente [virgola mobile] */ int i;


/* calcolo degli errori di posizione */lErrPos[i] = lPosSP[i] - lMotPos[i];

/* se almeno uno degli errori di posizione eccede il massimo *//* valore ammissibile si esce segnalando l'errore */if(labs(lErrPos[i]) > MAX_ERRPOS)

123

return(-(i+1));

/* conversione degli errori di posizione, dei set-points di *//* posizione e delle posizioni degli assi dei motori da unita' *//* resolver a virgola mobile */ErrPos[i] = (double)lErrPos[i];PosSP[i] = (double)lPosSP[i];MotPos[i] = (double)lMotPos[i];

/* calcolo dei set-points di velocita' */VelSP[i] = KP[i]*ErrPos[i];

/* calcolo degli anticipi di velocita' */VelFF[i] = KV[i]*(PosSP[i] - OldPosSP[i]);

/* calcolo delle velocita' degli assi dei motori */MotVel[i] = MotPos[i] - OldMotPos[i];

/* calcolo degli errori di velocita' */ErrVel[i] = VelSP[i] + VelFF[i] - MotVel[i];

/* calcolo delle uscite degli integratori *//* (nei regolatori di velocita') */IntOut[i] = OldInt[i] + KI[i]*ErrVel[i];

/* calcolo delle uscite dei regolatori di velocita' */CurrSP[i] = IntOut[i] + KPV[i]*ErrVel[i];

/* conversione dei set-points di corrente da double a short */Currents[i] = (short)CurrSP[i];

/* saturazione delle uscite degli integratori *//* e implementazione dell'anti wind-up */if(Currents[i] > MaxCurrents[i]) Currents[i] = MaxCurrents[i];else if(Currents[i] < -MaxCurrents[i]) Currents[i] = -MaxCurrents[i];else OldInt[i] = IntOut[i];

/* aggiornamento di OldPosSP e OldMotPos */OldPosSP[i] = PosSP[i];OldMotPos[i] = MotPos[i];

}

return(0);}

124

BIBLIOGRAFIA

[1] M.W. Spong: Modeling and Control of Elastic Joint Robot. ASME Journal of DynamicSystems, Measurement, and Control, vol. 109, Dicembre 1987, pag. 310-319

[2] R. Kelly: PD Control with Desired Gravity Compensation of Robotic Manipulators: AReview. The International Journal of Robotics Research, vol. 16, Ottobre 1997, pag. 660-672

[3] P. Tomei: Adaptative PD Controller for Robot Manipulators. IEEE Transactions on Roboticsand Automation, vol. 7, Agosto 1991, pag. 565-570

[4] P. Parolini: Modellizzazione e identificazione di un robot industriale. Tesi di laurea.Università di Pavia, Facoltà di Ingegneria, Anno Accademico 1995-96.

[5] E. Bassi, F. Benzi, P. Parolini: Modelling and Identification of an Industrial Robot. Atti del27th International Symposium on Industrial Robots, Milano, Ottobre 1996.

[5bis] J. Slotine, Li Weiping: Applied Nonlinear Control. Prentice-Hall 1991

[6] G. Ferretti, G. Magnani, P. Rocco, J. Krauss: Identificazione di Risonanze Meccaniche neiGiunti di un Robot Industriale. Automazione e Strumentazione, Aprile 1995, pag. 139-145

[7] B. Armstrong_Hélouvry, P. Dupont, C. Canudas De Witt: A survey of Model, Analysis Toolsand Compensation Methods for the Control of Machines with Friction. Automatica, vol. 30,Luglio 1994, pag. 1083-1138

[8] C. T Johnson.,. R. D Lorenz.: Experimental Identification of Friction and Its Compensation inPrecise, Position Controlled Mechanisms. IEEE Transactions On Industry Applications, vol.28, no. 6, Novembre/Dicembre 1992, pag. 1392- 1398

[9] G.Ferretti, G.Magnani, P.Rocco: Motor and Load Velocity Estimation for Digital ServoDrives: an Application to Robots with Elastic Joints. Atti del IECON'94 Bologna, September5-9, 1994, pp. 1748-1753.

[10] R. D. Klafter, T. A. Chmielewski M.Negin: ROBOTIC ENGINEERING, an integratedapproach. Prentice Hall. New Jersey, 1989.

[11] J.Y.S. Luh: Conventional Controller Design for Industrial Robot: A Tutorial IEEETransactions on Systems, Man, and Cybernetics, Vol. 13, N°3, May/June 1983.

Documents

APPENDICE A - unipv · 100 APPENDICE A PROGRAMMI UTILIZZATI Questa appendice ha lo scopo di illustrare i programmi utilizzati per il controllo del robot nelle diverse fasi di sperimentazione