int shift_power1 = 20;
int shift_power2 = 50;
int shift_power3 = 30;
int stall_cnt_b;
int stall_cnt_limit_1 = 20;

task motor_b_stall_monitoring() { // Check every 20 msec to see if the motor encoder has moved. If not, increment a counter
  int last_encoder_val;
  last_encoder_val = nMotorEncoder[motorB];
  while (true) {
    int encoder_delta;
    wait1Msec(20);
    encoder_delta = (nMotorEncoder[motorB] - last_encoder_val);
    if ((encoder_delta > 2) || (encoder_delta < -2)) { // a large delta means motor rotated
       stall_cnt_b = 0;
       last_encoder_val = nMotorEncoder[motorB];
    } else {
      if (stall_cnt_b < 1000)
        ++stall_cnt_b;
    }
  }
}

void shift_b() {
   nMotorEncoderTarget[motorC] = 60;//40
   motor[motorC] = shift_power1;
   nMotorEncoderTarget[motorB] = 25;
   motor[motorB] = -shift_power2;
   while(nMotorRunState[motorB] != runStateIdle) {};

   StartTask(motor_b_stall_monitoring);
   while (stall_cnt_b < stall_cnt_limit_1) {
      nMotorEncoderTarget[motorB] = 1;
      motor[motorB] = -shift_power3;
   }
   StopTask(motor_b_stall_monitoring);

   wait1Msec(1000);
   nMotorEncoderTarget[motorC] = 60;
   motor[motorC] = -shift_power1;
   while(nMotorRunState[motorC] != runStateIdle) {};
}


void shift_a(int the_angle) {
   nMotorEncoderTarget[motorA] = the_angle;
   motor[motorA] = -95;
   while(nMotorRunState[motorA] != runStateIdle) {};
}

void stand_b() {
   nMotorEncoderTarget[motorB] = nMotorEncoder[motorB];
   motor[motorB] = shift_power2;
   while(nMotorRunState[motorB] != runStateIdle) {};
}



void walk_start() {
   shift_b();
   shift_a(90); // c stride forward
   stand_b();
}


void init_motor() {
   nMotorEncoder[motorA] = 0;
   nMotorEncoder[motorB] = 0;
   nMotorEncoder[motorC] = 0;
   bFloatDuringInactiveMotorPWM = false;
   nMotorPIDSpeedCtrl = mtrSpeedReg;
}

void reset_pose()
{
   nNxtButtonTask  = -2;   // Grab control of the buttons
   int nBtn;
   bool keep_looping = true;
   while (keep_looping)   {
     while ((nBtn = nNxtButtonPressed) == -1) {}; // wait for button press
      nMotorEncoderTarget[motorA] = 1;
     switch (nBtn)     {
        case kLeftButton:
                  motor[motorA] = 100;
                  while(nMotorRunState[motorA] != runStateIdle) {};
                 break;
         case kRightButton:
                  motor[motorA] = -100;
                  while(nMotorRunState[motorA] != runStateIdle) {};
                  break;
         case kEnterButton: // reset encoder reading to 0
                  nMotorEncoder[motorA] = 0;
                  wait1Msec(1000);
                  keep_looping = false;
                  break;
      }
      nxtDisplayString(1, "%03d", nMotorEncoder[motorA]);
  }
}

task main() {
   init_motor();
   reset_pose();
   walk_start();
}