Sariel.pl

My first MOC using the LEGO Mindstorms NXT. Features a simple 4-speed gearbox that shifts automatically based on changes in the speed of the motor driving the gearbox.  

Datasheet:

Completion date: 25/02/2012
Power: electric (NXT brick)
Language: ROBOTC
Bricks: 1
Motors: 3 x NXT motor
Sensors: none

Having recently finally bought the LEGO Mindstorms NXT set, I have been looking for an opportunity to practice some programming. The default NXT-G software was a nightmare to me, so I was hesitating between leJOS and ROBOTC, finally settling for the latter.

The gearbox uses my 4-speed compact gearbox design, together with two motors that change speeds and a third motor that drives the gearbox and whose speed is being monitored by the program.

The program, which can be found complete below, consists of three task: one main and two subtasks included in it. One subtask monitors the use of the NXT brick’s buttons (because the program replaces their default functionality), while the other monitors the voltage of the brick’s battery (which can change quite a lot, depending on the load on the motors).  This is my first ROBOTC program, and I suppose I will be using these two subtasks on a regular basis.

The main tasks launches the two subtasks and controls all three motors. Once the propulsion motors is started, it monitors its speed counting the amount of degrees of rotation 10 times per second. Thus the basic speed unit is degrees/0.1 second, and depending on the condition of the batteries I’m using it’s usually around 70. There are two threshold speed values declared in the program, and if the motor exceeds one of them, the gearbox shifts up or down accordingly. Then it waits for a given interval to allow the motor to work a while with the changed gear ratio, and gets back to monitoring its speed.

The three values – lower and upper speed threshold value and the shifting interval –  are declared right at the beginning of the program for easy adjustment. I found it to be convenient and I will be probably doing so in my future programs as well.

The gearbox is synchronized and non-sequential, so it can be switched from any speed to any other speed. For simplicity, or more specifically to limit the number of possible combinations, I have designed the program to switch only between “adjacent” speeds – so it can swicth from 3rd to 4th or from 3rd to 2nd, but not from 3rd to 1st. There is one exception: shutting down. The program can be instructed to shut down by pressing the brick’s default exit button, and upon doing so it shifts the gearbox down to 1st. Obviously, to do so it needs to keep tracks of the currently selected gear – this information is stored in the variable gear, and updated when shifting down or up.

Programming the gearbox was an interesting experience and it taught me a lot about ROBOTC. I was surprised to found that rotating the “control” motors by a given degree proved less accurate than rotating them for a given time interval. By adjusting precisely their running time, I was able to achieve great accuracy needed to shift the transmission driving rings in the gearbox without pushing them too strongly into other parts of the gearbox.

Program:

// config start
int mspeed_high = 78;
int mspeed_low = 75;
int shift_time = 3000;
// config end

int gear = 1;

task Buttons()
{
  while(true)
  {
    nNxtButtonTask  = -2;
    nNxtExitClicks = 2;

    if(nNxtButtonPressed == 0)
    {
      eraseDisplay();
      motor[motorA] = 0;

            switch(gear)
            {
              case 1:
                break;

              case 2:
                motor[motorB] = -100;
                wait1Msec(120);
                motor[motorB] = 0;
                break;

              case 3:
                motor[motorC] = 100;
                wait1Msec(60);
                motor[motorC] = 0;
                wait1Msec(500);
                motor[motorB] = -100;
                wait1Msec(70);
                motor[motorB] = 0;
                break;

              case 4:
                motor[motorC] = -100;
                wait1Msec(60);
                motor[motorC] = 0;
                wait1Msec(500);
                motor[motorB] = -100;
                wait1Msec(70);
                motor[motorB] = 0;
                break;
            }

      StopAllTasks();
    }
  }
  return;
}

task BatLev()
{
  while(true)
  {
    nxtDisplayCenteredTextLine(0, "Brick up @ %3.1fV", nImmediateBatteryLevel / (float) 1000);
    wait1Msec(50);
  }
  return;
}

task main ()
{
  StartTask(Buttons);
  StartTask(BatLev);

  motor[motorA] = 0;
  bool mstatus = false;
  nMotorEncoder[motorB] = 0;
  nMotorEncoder[motorC] = 0;
  nxtDisplayCenteredTextLine(2, "MOTOR A OFF");
  nxtDisplayCenteredTextLine(4, "GEAR 1");
  wait1Msec(50);

  while(true)
  {
    if(nNxtButtonPressed == 3)
    {
      if (mstatus == true)
      {
        PlaySound(soundBlip);
        motor[motorA] = 0;
        mstatus = false;
        nxtDisplayCenteredTextLine(2, "MOTOR A OFF");
        nxtDisplayClearTextLine(5);
        nxtDisplayClearTextLine(7);
        wait1Msec(500);
      }
      else
      {
        motor[motorA] = 100;
        mstatus = true;
        nxtDisplayCenteredTextLine(2, "MOTOR A ON");
        wait1Msec(500);
        int mspeed = nMotorEncoder[motorA];

        while(nNxtButtonPressed != 3)
        {
          nMotorEncoder[motorA] = 0;
          wait1Msec(100);
          mspeed = nMotorEncoder[motorA];
          nxtDisplayCenteredTextLine(5, "Speed: %d", mspeed);

          if (mspeed > mspeed_high) // gearing up
          {
            nxtDisplayCenteredTextLine(7, "Gearing up...");
            switch(gear)
            {
              case 1:
                motor[motorB] = 100;
                wait1Msec(110);
                motor[motorB] = 0;
                gear = 2;
                nxtDisplayCenteredTextLine(4, "GEAR 2");
                wait1Msec(shift_time);
                break;

              case 2:
                gear = 3;
                motor[motorB] = -100;
                wait1Msec(60);
                motor[motorB] = 0;
                wait1Msec(300);
                motor[motorC] = -100;
                wait1Msec(60);
                motor[motorC] = 0;
                nxtDisplayCenteredTextLine(4, "GEAR 3");
                wait1Msec(shift_time);
                break;

              case 3:
                motor[motorC] = 100;
                wait1Msec(110);
                motor[motorC] = 0;
                gear = 4;
                nxtDisplayCenteredTextLine(4, "GEAR 4");
                wait1Msec(shift_time);
                break;

              case 4:
                nxtDisplayClearTextLine(7);
                break;
            }
          }
          else if (mspeed < mspeed_low) // gearing down
          {
            nxtDisplayCenteredTextLine(7, "Gearing down...");
            switch(gear)
            {
              case 4:
                motor[motorC] = -100;
                wait1Msec(110);
                motor[motorC] = 0;
                gear = 3;
                nxtDisplayCenteredTextLine(4, "GEAR 3");
                wait1Msec(shift_time);
                break;

              case 3:
                motor[motorC] = 100;
                wait1Msec(60);
                motor[motorC] = 0;
                wait1Msec(300);
                motor[motorB] = 100;
                wait1Msec(60);
                motor[motorB] = 0;
                gear = 2;
                nxtDisplayCenteredTextLine(4, "GEAR 2");
                wait1Msec(shift_time);
                break;

              case 2:
                motor[motorB] = -100;
                wait1Msec(110);
                motor[motorB] = 0;
                gear = 1;
                nxtDisplayCenteredTextLine(4, "GEAR 1");
                wait1Msec(shift_time);
                break;

              case 1:
                nxtDisplayClearTextLine(7);
                break;
            }
          }
        }
      }
    }
  }
}
									

Photos:

Video: