NXC: motor rotate to a specific ("absolute") encoder target

[schodet]

will this redefined function then automatically overwrite the genuine version of the API?

No, you may call the new version PosRegEnableAgain or PosRegRestart.

and I'm not quite sure how to write a task which lets a motor approach a target and then automatically stops when finally has reached a certain preciseness (e.g., 5 degrees) - and then signalizes this achievement by a flag.

You can make the difference between the actual position and the desired position, when the absolute value of the difference is less than 5, raise your flag.

[HaWe]

Do you mean sth like this:
(it's really hard to understand and very complicated to handle...!)

Code: Select all

inline void PosRegRestart(byte output, byte p = PID_3, byte i = PID_1, byte d = PID_1)
{
    SetOutput(output,
               OutputModeField, OUT_MODE_MOTORON+OUT_MODE_BRAKE+OUT_MODE_REGULATED,
               RegModeField, OUT_REGMODE_POS,
               RunStateField, OUT_RUNSTATE_RUNNING,
               PowerField, 0,
               TurnRatioField, 0,
               RegPValueField, p, RegIValueField, i, RegDValueField, d,
               UpdateFlagsField, UF_UPDATE_MODE+UF_UPDATE_SPEED+UF_UPDATE_PID_VALUES+UF_UPDATE_RESET_COUNT);
    Wait(MS_2);
}
long targetA;
char targetA_flag;
task ReachEncoderTargetA() {
  PosRegRestart(byte Out_A, byte p = PID_3, byte i = PID_1, byte d = PID_1); // <= where do I have to pass the target?
  while(abs(MotorCounter(OUT_A)-targetA)>=5);
  targetA_flag=1;
}

task main() {
  targetA=7654;
  targetA_flag=0;
  start ReachEncoderTargetA;
  while(! targetA_flag);
  ...
}

[schodet]

Do you mean sth like this:
(it's really hard to understand and very complicated to handle...!)

I have made some changes, not tested:

Code: Select all

inline void PosRegRestart(byte output, byte p = PID_3, byte i = PID_1, byte d = PID_1)
{
    SetOutput(output,
               OutputModeField, OUT_MODE_MOTORON+OUT_MODE_BRAKE+OUT_MODE_REGULATED,
               RegModeField, OUT_REGMODE_POS,
               RunStateField, OUT_RUNSTATE_RUNNING,
               PowerField, 0,
               TurnRatioField, 0,
               RegPValueField, p, RegIValueField, i, RegDValueField, d,
               UpdateFlagsField, UF_UPDATE_MODE+UF_UPDATE_SPEED+UF_UPDATE_PID_VALUES);
    Wait(MS_2);
}
long targetA;
char targetA_flag;
task ReachEncoderTargetA() {
  PosRegRestart(OUT_A);
  PosRegSetAngle(OUT_A, targetA);
  while(abs(MotorTachoCount(OUT_A)-targetA)>=5)
    Wait(MS_1); // No need to take all CPU power.
  targetA_flag=1;
}

task main() {
  targetA=7654;
  targetA_flag=0;
  start ReachEncoderTargetA;
  while(! targetA_flag);
  ...
}

[HaWe]

no, it does not work at all :(

it stops at 3387 although the target was 4700 and it doesn't seem to coast.

(I adjusted the nomenclature a little, now this is your function:)

Code: Select all

#define A1  OUT_B // upper arm
long A1target; 
char A1rdy;

task RotateToTargetA1() {
  PosRegRestart(A1);
  PosRegSetAngle(A1, A1target);
  while(abs(MotorTachoCount(A1)-A1target)>=5)
    Wait(1); // No need to take all CPU power.
  A1rdy=1;
}

my own function goes like this (positioning error meanwhile 10-20 degrees):

Code: Select all

#define A1  OUT_B // upper arm
#define MEnc(m) MotorRotationCount(m)

long A1target;
char A1rdy;

task  RotateToTargetA1() {
  long dabs, oldabs;
  char port=A1, pwr=-100;

  A1rdy=0; Wait(1);
  while (abs(MEnc(port)-A1target)>5) {
    dabs=abs(MEnc(port)-A1target);
    if (dabs>100) pwr=-100;
    else if (dabs>80) pwr=-80;
    else if (dabs>50) pwr=-70;
    else if (dabs<=50) pwr=-60;
    if ((oldabs<=dabs)&&(dabs<80)) pwr=-100;
    if (dabs<=5) { Off(port); Wait(10);}
    else {
      if (MEnc(port)<A1target) { OnFwd(port,-pwr); Wait(5);} // maybe shorter delay like 2 or 3
      if (MEnc(port)>A1target) { OnFwd(port,pwr); Wait(5);}
    }
    oldabs=dabs;
    Wait(1);
  }
  Off(port); // redundant, just for debugging
  Wait(30);
  A1rdy= true;
  Coast(port);
}

[schodet]

Could you post a complete minimal program with a description of what you expect so that I can test it?

[HaWe]

you're welcome!
robot construction: each Motor is attached to a screw drive which controls 1 joint of an robot arm (actually made 2 linear actuators out of them plus the turn table).
if the arm has reached his minimum extension then the arm presses itself to a touch sensor.
At the start each motor is driven into the specified direction until his specific touch sensor is pressed,
then drives to the other direction until the touch sensor is released again,
and just in this moment the related motor encoder is initialized to zero.

For follow-up use this zero postion is the relative zero point for the related joint.
All 66 targets (squares + extra positions) are defined by 3D coordinates of each joint which are listed by 66x3 encoder values

(JFYI: there will have to be some more squares to be defined to store the target for out-taken pieces; and pure trigonometric target calculation caused too large errors because of bending of joints and statics by gravitation and slippage in propulsion, but once there will be implemented sort of 9-point-calibration).

the minimal code has not been tested, I just deleted about 2000 lines of my chess robot and Bluetooth network code which seemed to be non-essential for robot-arm-testing.

/******************************************************************************************************/
==>> this code version contains my own OLD motor control tasks, not yours. <<==
(currently I'm using RotateMotorPID(const port,...) approaching relative targets instead)
/******************************************************************************************************/

now here we go:

Code: Select all

//                               micro-NeXC
//                             Robot Arm Test

/******************************************************************************/
string version ="4.18.RoBTM";


/******************************************************************************/
// (c) H. Wunder 2011
/******************************************************************************/






/******************************************************************************/
// Basics
/******************************************************************************/

char key;
char CursPos=120;

#define printf1( _x, _y, _format1, _value1) { \
  string sval1 = FormatNum(_format1, _value1); \
  TextOut(_x, _y, sval1); \
}


#define min(a,b) (a<b?a:b)

#define max(a,b) (a>b?a:b)


inline long round(float f)
{ return (f<0?f-0.5:f+0.5); }


inline char inArea(long x, long v, long thr) {
  return ((x>=v-thr)&&(x<=v+thr));
}





/******************************************************************************/
inline bool keypressed(){
/******************************************************************************/
   char test;
   test=( ButtonPressed(BTN1, false) || ButtonPressed(BTN2, false)
       || ButtonPressed(BTN3, false) || ButtonPressed(BTN4, false));
   return test;
}



/******************************************************************************/
inline char readkey() {
/******************************************************************************/
  char result = -1;

    if (ButtonPressed(BTNCENTER, false))
      result = BTNCENTER;
    if (ButtonPressed(BTNEXIT, false))
      result = BTNEXIT;
    else if (ButtonPressed(BTNLEFT, false))
      result = BTNLEFT;
    else if (ButtonPressed(BTNRIGHT, false))
      result = BTNRIGHT;

    return result;
}



/******************************************************************************/
// Sound: PlayTones
/******************************************************************************/

Tone sndBeep[]      = {TONE_G5,200};
Tone sndBeepBeep[]  = {TONE_C5,200 , 0,100, TONE_C5,200};
Tone sndBoop[]      = {TONE_C4,200};
Tone sndChordUp[]   = {TONE_C4,50, TONE_E4,50, TONE_G4,50,
                        TONE_C5,50, TONE_E5,50, TONE_G5,50, TONE_C6,200};
Tone sndChordDn[]   = {TONE_C6,50, TONE_G5,50, TONE_E5,50,
                        TONE_C5,50, TONE_G4,50, TONE_E4,50,  TONE_C4,200};
Tone sndChord[]     = {TONE_C4,50, TONE_E4,50, TONE_G4,50, TONE_C5,50};
Tone sndBlip[]      = {TONE_C7,10 };
Tone sndBlipBlip[]  = {TONE_C7,10, 0,20, TONE_C7,10 };
Tone sndBuzz[]      = {TONE_C4/2, 200 };
Tone sndError[]     = {TONE_C4,50, 0,50, TONE_C4,50, 0,50, TONE_C4,50};
Tone sndTaDaa[]     = {TONE_G4,100, 0,100, TONE_C5,200};
Tone sndTaDoo[]     = {TONE_G4,100, 0,100, TONE_C4,200};
Tone sndFuneral[]   = {TONE_C4,450, 0,50, TONE_C4,450, 0,50, TONE_C4,450, 0,50,
                        TONE_CS4,400, TONE_C4,150, 0,50, TONE_C4,400, TONE_AS3,150,
                        0,50,TONE_AS3,400, TONE_A3,150, 0,50, TONE_AS3,400 };


/*forward*/ void BTSendBoard(char K, char L, char turn,
                             char EP, char RK, char CK, char CM,
                             char board[], byte ID);

/******************************************************************************/
// Init +  setup
/******************************************************************************/


/******************************************************************************/
void Init(){
/******************************************************************************/
  SetLongAbort(true);
  ResetSleepTimer();
  SetSleepTimeout(0);
}

/******************************************************************************/
void InitIOports() {
/******************************************************************************/
  SetSensor(0, SENSOR_TOUCH);
  SetSensor(1, SENSOR_TOUCH);
  SetSensor(2, SENSOR_TOUCH);
  SetSensor(3, SENSOR_TOUCH);
  SetMotorRegulationTime (10);
}




//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++


#define TT 0
#define A1 1
#define A2 2
#define maxTT +5300
#define minTT -5300
#define maxA1 +5200
#define minA2 -6700

#define MEnc(m) MotorRotationCount(m)
#define SRaw(s) SensorRaw(s)
#define SBool(s) SensorValue(s)


//------------------------------------------------------------------------------
//------------------------------------------------------------------------------


long TTtarget;
char TTrdy;

task  RotateToTargetTT() {
  char port=TT, pwr=-80;
  long dabs, oldabs;
  TTrdy=0; Wait(1);

  while (!(inArea(MEnc(port),TTtarget,5))) {
    dabs=abs(MEnc(port)-TTtarget);
    if (dabs>50) pwr=-80;
    else if (dabs>20) pwr=-70;
    else if (dabs<=20) pwr=-60;
    if ((oldabs<=dabs)&&(dabs<50)) pwr=-80;
    if ((inArea(MEnc(port),TTtarget,5))) { Off(port); Wait(10);}
    else {
      if (MEnc(port)<TTtarget) { OnFwd(port,-pwr); Wait(10);}
      if (MEnc(port)>TTtarget) { OnFwd(port,pwr); Wait(10);}
    }

    oldabs=dabs;
    Wait(1);
  }
  Off(port);
  Wait(50);
  TTrdy= true;
}


long A1target;
char A1rdy;

task  RotateToTargetA1() {
  long dabs, oldabs;
  char port=A1, pwr=-100;

  A1rdy=0; Wait(1);
  while (!(inArea(MEnc(port),A1target,5))) {
    dabs=abs(MEnc(port)-A1target);
    if (dabs>100) pwr=-100;
    else if (dabs>80) pwr=-80;
    else if (dabs>50) pwr=-70;
    else if (dabs<=50) pwr=-60;
    if ((oldabs<=dabs)&&(dabs<80)) pwr=-100;
    if ((inArea(MEnc(port),A1target,5))) { Off(port); Wait(10);}
    else {
      if (MEnc(port)<A1target) { OnFwd(port,-pwr); Wait(10);}
      if (MEnc(port)>A1target) { OnFwd(port,pwr); Wait(10);}
    }
    oldabs=dabs;
    Wait(1);
  }
  Off(port);
  Wait(50);
  A1rdy= true;
}


long A2target;
char A2rdy;

task  RotateToTargetA2() {
  long dabs, oldabs;
  char port=A2, pwr=-100;

  A2rdy=0; Wait(1);
  while (!(inArea(MEnc(port),A2target,5))) {
    dabs=abs(MEnc(port)-A2target);
    if (dabs>100) pwr=-100;
    else if (dabs>80) pwr=-80;
    else if (dabs>50) pwr=-70;
    else if (dabs<=50) pwr=-60;
    if ((oldabs<=dabs)&&(dabs<80)) pwr=-100;
    if ((inArea(MEnc(port),A2target,5))) { Off(port); Wait(10);}
    else {
     if (MEnc(port)<A2target) { OnFwd(port,-pwr); Wait(10);}
     if (MEnc(port)>A2target) { OnFwd(port,pwr); Wait(10);}
    }
    oldabs=dabs;
    Wait(1);
  }
  Off(port);
  Wait(50);
  A2rdy= true;
}


//------------------------------------------------------------------------------
//------------------------------------------------------------------------------


task ResetTT() {
  char state, speed=70;
  TTrdy=0;
  OnFwd(TT,-speed);
  Wait(3000);
  while(!(state==2)) {
    if (state==0) {
      OnFwd(TT, speed);
      if (SBool(TT)) {
        Off(TT);
        Wait(20);
        state=1;
      }
    }
    if (state==1) {
      OnFwd(TT,-speed);
      Wait(50);
      if (!SBool(TT)) {
        Off(TT);
        Wait(20);
        state=2;
      }
    }
    Wait(1);
  }
  Wait(200);
  if (state==2) ResetRotationCount(TT);
  Wait(200); Coast(TT);
  TTrdy=1;
}


task ResetA1() {
  char state, speed=-100;
  A1rdy=0;
  while(!(state==2)) {
    if (state==0) {
      OnFwd(A1, speed);
      if (SBool(A1)) {
        Off(A1);
        Wait(20);
        state=1;
      }
    }
    if (state==1) {
      OnFwd(A1,70);
      Wait(50);
      if (!SBool(A1)) {
        Off(A1);
        Wait(20);
        state=2;
      }
    }
    Wait(1);
  }
  Wait(200);
  if (state==2) ResetRotationCount(A1);
  Wait(200); Coast(A1);
  A1rdy=1;
}



task ResetA2() {
  char state, speed=100;
  A2rdy=0;
  while(!(state==2)) {
    if ((state==0)&&(MEnc(A1)<2500)) {
      if (MEnc(A1)<2500) {OnFwd(A2, speed);}
      if (SBool(A2)) {
        Off(A2);
        Wait(20);
        state=1;
      }
    }
    if (state==1) {
      OnFwd(A2,-70);
      Wait(50);
      if (!SBool(A2)) {
        Off(A2);
        Wait(20);
        state=2;
      }
    }
    Wait(1);
  }
  Wait(200);
  if (state==2) ResetRotationCount(A2);
  Wait(200); Coast(A2);
  A2rdy=1;
}





long tTT[129], tA1[129], tA2[129];

void InitLookupTable() {
  int p;
  int f=0;
//      sqr            TT               sh.ri             el.le
    p= 119;  tTT[p]=  840+f;     tA1[p]=5020;     tA2[p]=-2210;  //
    p= 118;  tTT[p]=  610+f;     tA1[p]=5010;     tA2[p]=-2210;
    p= 117;  tTT[p]=  320+f;     tA1[p]=4610;     tA2[p]=-2710;
    p= 116;  tTT[p]=   90+f;     tA1[p]=4630;     tA2[p]=-2650;
    p= 115;  tTT[p]= -190+f;     tA1[p]=4630;     tA2[p]=-2840;
    p= 114;  tTT[p]= -380+f;     tA1[p]=4750;     tA2[p]=-2570;
    p= 113;  tTT[p]= -640+f;     tA1[p]=4900;     tA2[p]=-2290;
    p= 112;  tTT[p]= -900+f;     tA1[p]=5380;     tA2[p]=-1260;  //

    p= 103;  tTT[p]=  965+f;     tA1[p]=4210;     tA2[p]=-3260;  //
    p= 102;  tTT[p]=  670+f;     tA1[p]=3900;     tA2[p]=-3500;
    p= 101;  tTT[p]=  380+f;     tA1[p]=4000;     tA2[p]=-3600;
    p= 100;  tTT[p]=  120+f;     tA1[p]=3900;     tA2[p]=-3640;
    p=  99;  tTT[p]= -180+f;     tA1[p]=3900;     tA2[p]=-3670;
    p=  98;  tTT[p]= -420+f;     tA1[p]=4040;     tA2[p]=-3500;
    p=  97;  tTT[p]= -700+f;     tA1[p]=4250;     tA2[p]=-3300;
    p=  96;  tTT[p]= -940+f;     tA1[p]=4380;     tA2[p]=-3150;  //

    p=  87;  tTT[p]= 1050+f;     tA1[p]=3420;     tA2[p]=-4000;
    p=  86;  tTT[p]=  770+f;     tA1[p]=3260;     tA2[p]=-4150;
    p=  85;  tTT[p]=  470+f;     tA1[p]=3160;     tA2[p]=-4240;
    p=  84;  tTT[p]=  160+f;     tA1[p]=3120;     tA2[p]=-4280;
    p=  83;  tTT[p]= -190+f;     tA1[p]=3160;     tA2[p]=-4250;
    p=  82;  tTT[p]= -480+f;     tA1[p]=3280;     tA2[p]=-4140;
    p=  81;  tTT[p]= -750+f;     tA1[p]=3480;     tA2[p]=-4030;
    p=  80;  tTT[p]=-1080+f;     tA1[p]=3680;     tA2[p]=-3830;

    p=  71;  tTT[p]= 1100+f;     tA1[p]=2830;     tA2[p]=-4580;
    p=  70;  tTT[p]=  770+f;     tA1[p]=2700;     tA2[p]=-4650;
    p=  69;  tTT[p]=  430+f;     tA1[p]=2600;     tA2[p]=-4760;
    p=  68;  tTT[p]=  100+f;     tA1[p]=2600;     tA2[p]=-4770;
    p=  67;  tTT[p]= -240+f;     tA1[p]=2600;     tA2[p]=-4770;
    p=  66;  tTT[p]= -490+f;     tA1[p]=2700;     tA2[p]=-4680;
    p=  65;  tTT[p]= -890+f;     tA1[p]=2870;     tA2[p]=-4540;
    p=  64;  tTT[p]=-1210+f;     tA1[p]=3020;     tA2[p]=-4400;

    p=  55;  tTT[p]= 1185+f;     tA1[p]=2220;     tA2[p]=-4970;
    p=  54;  tTT[p]=  860+f;     tA1[p]=2095;     tA2[p]=-5040;
    p=  53;  tTT[p]=  460+f;     tA1[p]=2020;     tA2[p]=-5100;
    p=  52;  tTT[p]=  100+f;     tA1[p]=1950;     tA2[p]=-5120;
    p=  51;  tTT[p]= -310+f;     tA1[p]=2000;     tA2[p]=-5120;
    p=  50;  tTT[p]= -680+f;     tA1[p]=2200;     tA2[p]=-5060;
    p=  49;  tTT[p]=-1020+f;     tA1[p]=2300;     tA2[p]=-5000;
    p=  48;  tTT[p]=-1320+f;     tA1[p]=2490;     tA2[p]=-4830;

    p=  39;  tTT[p]= 1300+f;     tA1[p]=1800;     tA2[p]=-5320;
    p=  38;  tTT[p]=  940+f;     tA1[p]=1640;     tA2[p]=-5400;
    p=  37;  tTT[p]=  550+f;     tA1[p]=1550;     tA2[p]=-5500;
    p=  36;  tTT[p]=  140+f;     tA1[p]=1510;     tA2[p]=-5540;
    p=  35;  tTT[p]= -270+f;     tA1[p]=1510;     tA2[p]=-5500;
    p=  34;  tTT[p]= -710+f;     tA1[p]=1660;     tA2[p]=-5410;
    p=  33;  tTT[p]=-1120+f;     tA1[p]=1740;     tA2[p]=-5300;
    p=  32;  tTT[p]=-1520+f;     tA1[p]=2150;     tA2[p]=-5170;

    p=  23;  tTT[p]= 1460+f;     tA1[p]=1430;     tA2[p]=-5610;
    p=  22;  tTT[p]= 1000+f;     tA1[p]=1200;     tA2[p]=-5730;
    p=  21;  tTT[p]=  590+f;     tA1[p]=1060;     tA2[p]=-5750;
    p=  20;  tTT[p]=  100+f;     tA1[p]=1000;     tA2[p]=-5790;
    p=  19;  tTT[p]= -330+f;     tA1[p]= 940;     tA2[p]=-5710;
    p=  18;  tTT[p]= -830+f;     tA1[p]=1240;     tA2[p]=-5700;
    p=  17;  tTT[p]=-1280+f;     tA1[p]=1300;     tA2[p]=-5600;
    p=  16;  tTT[p]=-1700+f;     tA1[p]=1610;     tA2[p]=-5450;

    p=   7;  tTT[p]= 1800+f;     tA1[p]= 930;     tA2[p]=-5840;
    p=   6;  tTT[p]= 1200+f;     tA1[p]= 520;     tA2[p]=-5750;
    p=   5;  tTT[p]=  700+f;     tA1[p]= 500;     tA2[p]=-5660;
    p=   4;  tTT[p]=  180+f;     tA1[p]= 370;     tA2[p]=-5580;
    p=   3;  tTT[p]= -360+f;     tA1[p]= 580;     tA2[p]=-5650;
    p=   2;  tTT[p]= -900+f;     tA1[p]= 870;     tA2[p]=-5750;
    p=   1;  tTT[p]=-1450+f;     tA1[p]= 880;     tA2[p]=-5550;
    p=   0;  tTT[p]=-1800+f;     tA1[p]=1240;     tA2[p]=-5620;

    p= 128;  tTT[p]=    0;       tA1[p]=   0;     tA2[p]=-6450; // align board
    p= 127;  tTT[p]=-1900+f;     tA1[p]=2660;     tA2[p]=-4470; // drop area

}

//------------------------------------------------------------------------------
void MoveClawTo(int p) {
//------------------------------------------------------------------------------
  printf1(0,56, "target sqr=%4d",p);

  if (MEnc(A1)>=(tA1[p]-1000) && (MEnc(A2)>=tA2[p])-1000)  {

    TTtarget=tTT[p];    start RotateToTargetTT;
    if(MEnc(A1)>3000) { A1target=max(tA1[p],3000); start RotateToTargetA1; }
    while (!A1rdy);
    A1target=tA1[p];    start RotateToTargetA1;
    A2target=tA2[p];    start RotateToTargetA2;
  }

  else  {
    TTtarget=tTT[p];    start RotateToTargetTT;

    A2target=min(MEnc(A2)+1000,0);    start RotateToTargetA2;
    Wait(100);
    while (!A2rdy);

    A1target=tA1[p];      start RotateToTargetA1;
    A2target=tA2[p];      start RotateToTargetA2;
    Wait(100);
  }

  while (!TTrdy);
  while (!A1rdy);
  while (!A2rdy);

  Off(OUT_ABC);
  Wait(200);

}


//------------------------------------------------------------------------------
void TestAllSquares() {
//------------------------------------------------------------------------------
  int sqr;

  for (sqr= 119; sqr>=0; --sqr ) {
    if (sqr & 0x88) continue;

    MoveClawTo(sqr);  PlayTones(sndChordUp);
    Coast(OUT_ABC); Wait(200);

    while(!keypressed()); // wait until Btn pressed
  }
}

//------------------------------------------------------------------------------
void RobotTransportFromTo(int START, int TARGET, char board[]) {
//------------------------------------------------------------------------------



  MoveClawTo(START);  PlayTones(sndBuzz);  Coast(OUT_ABC);

  PlayTones(sndBeep);  Coast(OUT_ABC);  Wait(200);
  
  MoveClawTo(TARGET);  PlayTones(sndBuzz);  Coast(OUT_ABC);

}



//------------------------------------------------------------------------------
void RobotArmsZeroInit() {
//------------------------------------------------------------------------------
   start ResetTT;
   start ResetA1;
   Wait(4000); start ResetA2;

   while(!(TTrdy&&A1rdy&&A2rdy));
}


//------------------------------------------------------------------------------
task ShowValues() {
//------------------------------------------------------------------------------
  while(1) {

    printf1(0,48, "S1%d", SBool(TT));  printf1(24,48, "TT%5d", MEnc(TT));  printf1(66,48, "run%2d", MotorRunState(TT));
    printf1(0,40, "S2%d", SBool(A1));  printf1(24,40, "A1%5d", MEnc(A1));  printf1(66,40, "run%2d", MotorRunState(A1));
    printf1(0,32, "S3%d", SBool(A2));  printf1(24,32, "A2%5d", MEnc(A2));  printf1(66,32, "run%2d", MotorRunState(A2));
    


    Wait(10);
  }
}

task EmergencyStop() {
  while(true) {
    if (Sensor(S4)) StopAllTasks();
    Wait(20);
  }
}


/******************************************************************************/
// main()
/******************************************************************************/


/******************************************************************************/
task main(){
/******************************************************************************/
 
  char board[129];                    // Piece K_start-> L_dest



// init all defauts
  Init();
  InitIOports();
  InitLookupTable();

  start EmergencyStop;

  ClearScreen();
  start ShowValues;

  RobotArmsZeroInit(); PlayTones(sndChordUp);

  MoveClawTo(128); PlayTones(sndChordUp);  Coast(OUT_ABC); Wait(200); while(!keypressed()); // to allign the board to the center of robot

  TestAllSquares();

  while(true){ }

}

edit: 2 typos corrected!

[schodet]

This seems to be a nice project !

It would be much easier if you could send a minimal program, together with a description of a simple behavior you would like to implement.

[HaWe]

more minimal is not possible, because of the pre-calibration (intit zero points at / before program start).
Reduced even more probably won't let the motor control run into the Nirwana as it does right now.

So it is already is as minimal as can be ;)

You may try it first with my own "MoveToTarget"-tasks,
then exchange the code by your absolute regulation code.

Simply take 3 motors and 4 touch sensors, S1-S3 is for the motors, S4 is for emergency stop.

Start the program, let the motors run maybe 4 seconds, then press S1-S3 and release it again half a second later, do this one after the other.
The related motors will stop then and the program sets their specific counters to 0.
So each motor (the program) knows it's calibrateted zero point.

Then let the program approach the motor targets.
The list of the target coordinates you may find in the source code, 1st target is No. 128, then 119 (then 118, 117,...)

Maybe put some torque to the motors.
Observe on the display what's happening and if the targets will be approached correctly - nothing more happens with this minimalistic code.

With my own code the targets will be approached actually more or less correctly, all motors will run simultaneously as they should, but the positioning error is often > 20 degrees - that's too much.
The error should be less than 5 degrees with your abs_pos_reg code.

[schodet]

I made a small program to demonstrate what I think you need according to what I understood from your requirements.

It makes all motors to a specific position, then disable regulation for an arbitrary movement using OnFwdEx, then restart regulation and ask for another position. Here it is:

Code: Select all

inline void
PosRegRestart(byte output, byte p = PID_3, byte i = PID_1, byte d = PID_1)
{
    SetOutput(output,
	      OutputModeField, OUT_MODE_MOTORON+OUT_MODE_BRAKE+OUT_MODE_REGULATED,
	      RegModeField, OUT_REGMODE_POS,
	      RunStateField, OUT_RUNSTATE_RUNNING,
	      PowerField, 0,
	      TurnRatioField, 0,
	      RegPValueField, p, RegIValueField, i, RegDValueField, d,
	      UpdateFlagsField, UF_UPDATE_MODE+UF_UPDATE_SPEED+UF_UPDATE_PID_VALUES);
    Wait(MS_1);
}

/* This function will makes all three motors move to the specified position
 * and returns once this is done. */
void
GoToPosition3D (int aa, int ab, int ac)
{
    /* Give the new consign. */
    PosRegSetAngle (OUT_A, aa);
    PosRegSetAngle (OUT_B, ab);
    PosRegSetAngle (OUT_C, ac);
    /* Wait until this is done. */
    while (abs (MotorTachoCount (OUT_A) - aa) > 5
	   || abs (MotorTachoCount (OUT_B) - ab) > 5
	   || abs (MotorTachoCount (OUT_C) - ac) > 5)
	Wait (MS_1);
}

task main()
{
    /* Initialise position control. */
    SetMotorPwnFreq (10);
    PosRegEnable (OUT_A);
    PosRegSetMax (OUT_A, 100, 0);
    PosRegEnable (OUT_B);
    PosRegSetMax (OUT_B, 100, 0);
    PosRegEnable (OUT_C);
    PosRegSetMax (OUT_C, 100, 0);
    /* OK, now go to a specific position. */
    GoToPosition3D (720, 360, 180);
    /* Done, tell it to the user. */
    PlayFileEx ("! Click.rso", 1, false);
    Wait (SEC_1);
    /* Now, some unregulated movement (do not reset tachocount!). */
    OnFwdEx (OUT_ABC, 30, RESET_NONE);
    Wait (SEC_1);
    /* Restart position control, motors will go back to their previous
     * position. */
    PosRegRestart(OUT_A);
    PosRegRestart(OUT_B);
    PosRegRestart(OUT_C);
    GoToPosition3D (720, 360, 180);
    PlayFileEx ("! Click.rso", 1, false);
    Wait (SEC_1);
    PlayFileEx ("! Click.rso", 1, false);
    /* OK, now go to a new specific position. */
    GoToPosition3D (900, 540, 360);
    /* Done, tell it to the user. */
    PlayFileEx ("! Click.rso", 1, false);
    Wait (SEC_1);
}

[HaWe]

thank you for your efforts!
I need to run all single motors in single tasks, but simultaneously to motors which already are running or will be started later after a short while.
Sometimes it must be possible to stop running motors intermediately triggered by other motors which have reached critical elevations (e.g., if the shoulder is still down, the elbow must wait until the shoulder will have reached a higher position and then start after a while or exit the task prematurely to prevent the joints from being hyperextended in some shoulder/elbow-extension-combinations).

So only single tasks like I showed in my exmples will do it correctly to prevent the parts to become mutually entangled.

A set of (sort of) the following functions, e.g.

Code: Select all

SetMotorRotationTarget(OUT_A, targetA);
RotateToTarget(OUT_A, absMaxPwr);
while (MotorRunstate(OUT_A)!=IDLE);

would be the best choice to have. (This is similar to a RobotC implementation!)

This function must not use MotorTachoCount internally because
1) intermediate manualy corrections will have to be done (in case of MotorTachoCount there will be error memory effects) and
2) the current encoder value is stored in MotorRotationCount (and will be intermediately accessed and checked by other independend tasks of my program)

(edited)
And, to be honest: all those manifold function calls are extremely complicated to handle.