Mindboards

Code: Select all

#define AXIS OUT_A
#define ARM OUT_B
#define espace -45
#define gauche -90
#define droite 90
#define haut 130
#define bas 0
#define degree1turn 20171
#define heighTomber 20

int stockGauche;
int degreeVal;
int goal;
bool axisMvt;
bool armMvt;
bool Mvt;


task orientation()
{
     axisMvt = true;
     
     //compute degree to turn to get the orientation degreeVal
     int degreMoteur = degreeVal*degree1turn/360 - MotorRotationCount(AXIS);
     RotateMotor(AXIS, 100, degreMoteur);
     axisMvt = false;
}

//move arm till a certain value in degree


task heigh()
{
     armMvt = true;
     
     //compute degree to turn to get the orientation goal
     int degreMoteur = goal - MotorRotationCount(BRAS);
     int strenghMotorB = 20;
     RotateMotor(ARM, forceMoteurB, degreMoteur);
     armMvt = false;
}

task mvtLook()
{
     while(true)
     {
                Mvt = (axisMvt||armMvt);
                Wait(100);
     }
}

void goTo(int axisVal, int armVal)
{
     until(Mvt == false);
     degreeVal = axisVal;
     goal = armVal;
     StartTask(heigh);
     StartTask(orientation);
}

task main()
{
     //INITIALISATION
     ResetTachoCount(AXIS);
     ResetTachoCount(BRAS);
     
     axisMvt = false;
     armMvt = false;
     
     StartTask(mvtLook);
     
     goTo(0, haut);
     goTo(espace, bas);

}

Code: Select all

#define degA 1000
#define degB 1000

task main()
{
     ResetTachoCount(OUT_A);
     ResetTachoCount(OUT_B);
     
     int depart_A = MotorRotationCount(OUT_A);
     int depart_B = MotorRotationCount(OUT_B);
     
     int distA = 0;
     int distB = 0;
     
     OnFwd(OUT_A, 50);
     OnFwd(OUT_B, 50);
     
     bool reachA = false;
     bool reachB = false;
     
     do
     {
                  distA = MotorRotationCount(OUT_A) - depart_A;
                  distB = MotorRotationCount(OUT_B) - depart_B;
                  
                  reachA = (distA >= degA);
                  reachB = (distB >= degB);
                  
                  if (reachA)
                  {
                     Off(OUT_A);
                  }
                  
                  if (reachB)
                  {
                     Off(OUT_B);
                  }
                  
     } while (((reachA && reachB) == false));
     
     NumOut(0, LCD_LINE3, MotorRotationCount(OUT_A));
     NumOut(50, LCD_LINE3, MotorRotationCount(OUT_B));
     Wait(3000);
     
}

Code: Select all

char MotAready, MotBready, MotCready;

MotAControlTask1() {
  // do sth.
  // do sth else
  // do sth until end-condition
  MotAready=true;
}

MotAControlTask2() {
  // do sth.
  // do sth else
  // do sth until end-condition
  MotAready=true;
}
  
task main() {
  MotAready=false;
  start MotAControlTask1; // start 1st task
  while (!MotAready); // wait until 1st task has finished

  Wait(1); // give the time slice scheduler a chance to update all the motor and task states

  MotAready=false;
  start MotAControlTask2; // start 2nd task
  while (!MotAready); // wait until 2nd task has finished
  //...
  //...
}

Code: Select all

MotAControlTask2() {
  MotAready=false;
  Wait(1);
  // do sth.
  // do sth else
  // do sth until end-condition
  MotAready=true;
}