#pragma config(Motor,  motorA,          axis1,         tmotorNXT, openLoop, encoder)
#pragma config(Motor,  motorB,          axis2,         tmotorNXT, openLoop, reversed, encoder)
#pragma config(Motor,  motorC,           ,             tmotorNXT, openLoop, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//
float const GearRatio=pow((56),-1);
int const GearRatio2=(56);
float ValEncA;
float MotPowerA;
bool PosReached;

float PIDMotA(float ConsignA ,float ValCodA)
{
   float  Kp, Ki, Kd, Prop, Inte, Deri, OldErr, ErrDiff;
   float PowMotA;
   Kp=3;
   Ki=0;
   Kd=30;
  OldErr=ErrDiff;
  ErrDiff=ConsignA-ValCodA;
  Prop=ErrDiff;
  Inte=Inte+ErrDiff;
  Deri=ErrDiff-OldErr;
  PowMotA=(Kp*Prop)+(Ki*Inte)+(Kd*Deri);
  return PowMotA; //return motor power
}

void moveJ(float Aj, float Bj)
{
   while (!PosReached)
   {
      Aj*=GearRatio2;
      MotPowerA=PIDMotA(Aj,nMotorEncoder[axis1]);
      wait1Msec(10);
      motor[axis1]=MotPowerA;
      if (MotPowerA<10&MotPowerA>-10)
      {
         motor[axis1]=0; //Position reached
         PosReached=true;
      }
   }
   PosReached=false;
   return;
}

task main()
{
   moveJ(0,0);
   wait1Msec(2000);
   moveJ(45,0);
   wait1Msec(2000);
   moveJ(0,0);
   wait1Msec(2000);
}