void braccio(int nSteps, int mBaseDest, int m1Dest, int m2Dest, int m3Dest, int mRotPinzaDest, int mPinzaDest){
if ( (m1Att!=m1Dest) || \
(m2Att!=m2Dest) || \
(m3Att!=m3Dest) || \
(mRotPinzaAtt!=mRotPinzaDest) || \
(mPinzaAtt!=mPinzaDest) || \
(mBaseAtt!=mBaseDest) ){
float mBaseStep = ( (float)mBaseAtt-(float)mBaseDest) / nSteps;
float m1Step = ( (float)m1Att-(float)m1Dest) / nSteps;
float m2Step = ( (float)m2Att-(float)m2Dest) / nSteps;
float m3Step = ( (float)m3Att-(float)m3Dest) / nSteps;
float mRotPinzaStep = ( (float)mRotPinzaAtt-(float)mRotPinzaDest) / nSteps;
float mPinzaStep = ( (float)mPinzaAtt-(float)mPinzaDest) / nSteps;
int i;
for(i = 0; i<nSteps; i++){
setPwmValue(CH_PINZA, mPinzaAtt);
setPwmValue(CH_ROT_PINZA, mRotPinzaAtt);
setPwmValue(CH_ROT_BASE, mBaseAtt);
setPwmValue(CH_M1, m1Att);
setPwmValue(CH_M2, m2Att);
setPwmValue(CH_M3, m3Att);
mBaseAtt -= mBaseStep;
m1Att -= m1Step;
m2Att -= m2Step;
m3Att -= m3Step;
mRotPinzaAtt -= mRotPinzaStep;
mPinzaAtt -= mPinzaStep;
usleep(10);
}
}
//fix possible error in division of float
mBaseAtt = mBaseDest;
m1Att = m1Dest;
m2Att = m2Dest;
m3Att = m3Dest;
mRotPinzaAtt = mRotPinzaDest;
mPinzaAtt = mPinzaDest;
setPwmValue(CH_ROT_BASE, mBaseAtt);
setPwmValue(CH_M1, m1Att);
setPwmValue(CH_M2, m2Att);
setPwmValue(CH_M3, m3Att);
setPwmValue(CH_ROT_PINZA, mRotPinzaAtt);
setPwmValue(CH_PINZA, mPinzaAtt);
}
void main(void) {
int t=15;
/* include your code here */
f1.functionId=ULTRASONIC_FRONT;
f1.status=READY;
f1.root=UNIQUE_FUNCTION;
f1.timerCount=50;
DisableInterrupts;
InitReg();
InitClock();
InitComunication();
InitPorts();
InitInputCompare();
InitKbi();
beginComunication();
initExecutingVector();
InitBuffer(&bufferIn);
InitBuffer(&bufferOut);
InitPwm();
InitADC();
EnableInterrupts;
InitRtc();
/*
despues de habilitar interrupciones y
antes de empezar a mover el robot debe calibrar sensores de meta
no arrancar el motor sin antes ejecutar initGoalSensor()!
*/
initGoalSensor();
setPwmValue(t);
if(t>35){
kbiSampleFreq=1000;
}else{
kbiSampleFreq=1000+((35-t)*200);
}
for(;;) {
if(goalStatus == 0){
PTDD_PTDD1;
setGoalMode(STOP_ON_GOAL);
SENTIDO_M1_1=1;
SENTIDO_M1_2=0;
SENTIDO_M2_1=1;
SENTIDO_M2_2=0;
SENSOR_DE_META_ON;
PTDD_PTDD0=1;
/*
functionHandler();
dispatcher(&executingVector,&bufferOut);
frameGenerator();*/
/* f1.functionId=ULTRASONIC_FRONT;
f1.status=READY;
getUltrasonic(&f1);
while(f1.status!=AVAILABLE);
t=3000;
while(t!=0){
t--;
} */
/*
f1.functionId=ULTRASONIC_FRONT;
f1.status=READY;
f1.root=UNIQUE_FUNCTION;
f1.timerCount=50;
getUltrasonic(&f1);
while(f1.status!=AVAILABLE);
t=3000;
while(t!=0){
t--;
} */
/*
f1.functionId=ULTRASONIC_RIGHT;
f1.status=READY;
getUltrasonic(&f1);
while(f1.status!=AVAILABLE);
t=30000;
while(t!=0){
t--;
} */
} /* loop forever */
}
/* please make sure that you never leave main */
}
