Bucket 'o' Bricks LEGO Brick Sorter

[inxt-generation]

EDIT: Scratch that last post. I just realized that RobotC and here use the same syntax, so I can replicate all efforts there here easily.

Hey! I'm back! With that Super-Top-Secret project I mentioned! I'll be posting more information about it over the next couple of days because I don't have time to do it all at once. Everyone fine with that Good.

I haven't taken any pics yet, so I won't be posting anything else about this until I do.

Here's the video to tide you over until then.




And again, I apologize for the video quality. Still testing out a new-ish camera, and I didn't really plan the video. I just did whatever came to mind while taping (SD-Carding?) it.

program:

Code: Select all

#pragma config(Sensor, S1, LightSens, sensorLightInactive)
#pragma config(Sensor, S2, ArmRst, sensorReflection)
#pragma config(Sensor, S3, ColorSens, sensorCOLORFULL)
#pragma config(Sensor, S4, HTIRL, sensorI2CCustom)
#pragma config(Motor, motorA, TmblrMtrSwtch, tmotorNormal, PIDControl, encoder)
#pragma config(Motor, motorB, ArmMoveMtr, tmotorNormal, PIDControl, encoder)
#pragma config(Motor, motorC, ArmTurnMtr, tmotorNormal, PIDControl, encoder)

#include "drivers/HTIRL-driver.h";

void initialize();
void InitializePF();
void ResetArmRot();
void ArmRotateToLoc(int DegreesToTurn);
void ArmLateralRst();
void ArmMoveToMarker(string DirectionToMove);
void ArmMoveToLoc(int LocToMoveTo);
void PneumSwtchMve(string ClawMovement);
void DispenseBrick4Scan();
void AlignBrick4Scan();
void TmblrMtrContr(string Direction);
void GetBlackOrGreenColor();
void RefillBrickQueue();

int BrickColor;

tPFmotor Tread1 = pfmotor_S4_C1_A;
tPFmotor Tread2AndLights = pfmotor_S4_C1_B;
tPFmotor DispenserMtr = pfmotor_S4_C2_A;
tPFmotor PneumSwtch = pfmotor_S4_C2_B;
tPFmotor BrickScannerDisp = pfmotor_S4_C3_A;
tPFmotor ColorScanAlign = pfmotor_S4_C3_B;

ePWMMotorCommand Tread1Fwd = MOTOR_FWD_PWM_4;
ePWMMotorCommand Tread2AndLightsFwd = MOTOR_FWD_PWM_7;
ePWMMotorCommand DispenserMtrDisp = MOTOR_FWD_PWM_7;
ePWMMotorCommand PneumClawUp = MOTOR_REV_PWM_7;
ePWMMotorCommand PneumClawDwn = MOTOR_FWD_PWM_7;
ePWMMotorCommand BrickScannerDispExt = MOTOR_FWD_PWM_3;
ePWMMotorCommand BrickScannerDispRtr = MOTOR_REV_PWM_3;
ePWMMotorCommand ColorScanAlignExt = MOTOR_REV_PWM_3;
ePWMMotorCommand ColorScanAlignRtr = MOTOR_FWD_PWM_3;

task main()
{
  bool DoneSorting = false;

  initialize();

  while(DoneSorting == false)
  {
    SetSensorType(ColorSens, sensorCOLORNONE);

    PFMotor(Tread2AndLights, Tread2AndLightsFwd);
    wait1Msec(400);
    if(SensorValue(LightSens) > 10)
    {
      RefillBrickQueue();
    }
    PFMotor(Tread1, Tread1Fwd);
    DispenseBrick4Scan();
    while(SensorValue(ColorSens) > 10);
    PFMotor(Tread2AndLights, MOTOR_BRAKE);
    PFMotor(Tread1, MOTOR_BRAKE);
    SetSensorType(ColorSens, sensorCOLORFULL);
    AlignBrick4Scan();
    BrickColor = SensorValue(ColorSens);
    if(BrickColor == 1 || BrickColor == 3) GetBlackOrGreenColor();

    PFMotor(Tread2AndLights, Tread2AndLightsFwd);
    wait1Msec(1000);
    PFMotor(Tread2AndLights, MOTOR_BRAKE);

    ArmMoveToLoc(BrickColor);
  }
}

void initialize()
{
  TmblrMtrContr("On");
  TmblrMtrContr("Off");
  InitializePF();
  PneumSwtchMve("Up");
  ArmLateralRst();
  ResetArmRot();
  PneumSwtchMve("Down");
  ArmMoveToMarker("Left");
}

void InitializePF()
{
  PFMotor(ColorScanAlign, ColorScanAlignRtr);
  wait1Msec(500);
  PFMotor(ColorScanAlign, MOTOR_BRAKE);

  PFMotor(BrickScannerDisp, BrickScannerDispRtr);
  wait1Msec(500);
  PFMotor(BrickScannerDisp, MOTOR_BRAKE);
}

void ResetArmRot()
{
  motor[ArmTurnMtr] = -50;
  while(SensorValue(ArmRst) != 100);
  motor[ArmTurnMtr] = 20;
  while(SensorValue(ArmRst) == 100);
  motor[ArmTurnMtr] = 0;
}

void ArmRotateToLoc(int DegreesToTurn)
{
  nMotorEncoder[ArmTurnMtr] = 0;
  nMotorEncoderTarget[ArmTurnMtr] = DegreesToTurn;
  motor[ArmTurnMtr] = 50;
  while(nMotorRunState[ArmTurnMtr] != runStateIdle);
  motor[ArmTurnMtr] = 0;
}

void ArmLateralRst()
{
  motor[ArmMoveMtr] = -30;
  while(SensorValue(ArmRst) <= 49);
  motor[ArmMoveMtr] = 0;
}

void ArmMoveToMarker(string DirectionToMove)
{
  if(DirectionToMove == "Left")
  {
    motor[ArmMoveMtr] = 50;
    wait1Msec(500);
    while(SensorValue(ArmRst) < 40);
    motor[ArmMoveMtr] = 0;
  }
  else
  {
    motor[ArmMoveMtr] = -50;
    wait1Msec(500);
    while(SensorValue(ArmRst) < 40);
    motor[ArmMoveMtr] = 0;
  }
}

void ArmMoveToLoc(int LocToMoveTo)
{
  switch (LocToMoveTo)
  {
    case 1:
      PneumSwtchMve("Up");
      wait1Msec(700);
      ArmRotateToLoc(1132);
      ArmMoveToMarker("Left");
      PneumSwtchMve("Down");
      wait1Msec(700);
      ArmMoveToMarker("Right");
      ResetArmRot();
    break;
    case 2:
      PneumSwtchMve("Up");
      wait1Msec(700);
      ArmRotateToLoc(1460);
      ArmMoveToMarker("Left");
      PneumSwtchMve("Down");
      wait1Msec(700);
      ArmMoveToMarker("Right");
      ResetArmRot();
    break;
    case 3:
      PneumSwtchMve("Up");
      wait1Msec(700);
      ArmRotateToLoc(1132);
      PneumSwtchMve("Down");
      wait1Msec(700);
      ResetArmRot();
    break;
    case 4:
      PneumSwtchMve("Up");
      wait1Msec(700);
      ArmRotateToLoc(1460);
      PneumSwtchMve("Down");
      wait1Msec(700);
      ResetArmRot();
    break;
    case 5:
      PneumSwtchMve("Up");
      wait1Msec(700);
      ArmRotateToLoc(1132);
      ArmMoveToMarker("Right");
      PneumSwtchMve("Down");
      wait1Msec(700);
      ArmMoveToMarker("Left");
      ResetArmRot();
    break;
    case 6:
      PneumSwtchMve("Up");
      wait1Msec(700);
      ArmRotateToLoc(1460);
      ArmMoveToMarker("Right");
      PneumSwtchMve("Down");
      wait1Msec(700);
      ArmMoveToMarker("Left");
      ResetArmRot();
    break;
  }
}

void PneumSwtchMve(string ClawMovement)
{
  if(ClawMovement == "Up")
  {
    PFMotor(PneumSwtch, PneumClawUp);
    wait1Msec(500);
    PFMotor(PneumSwtch, MOTOR_BRAKE);
  }
  else
  {
    PFMotor(PneumSwtch, PneumClawDwn);
    wait1Msec(500);
    PFMotor(PneumSwtch, MOTOR_BRAKE);
  }
}

void DispenseBrick4Scan()
{
  PFMotor(BrickScannerDisp, BrickScannerDispExt);
  wait1Msec(250);
  PFMotor(BrickScannerDisp, BrickScannerDispRtr);
  wait1Msec(250);
  PFMotor(BrickScannerDisp, MOTOR_BRAKE);
}

void AlignBrick4Scan()
{
  PFMotor(ColorScanAlign, ColorScanAlignExt);
  wait1Msec(250);
  PFMotor(ColorScanAlign, ColorScanAlignRtr);
  wait1Msec(250);
  PFMotor(ColorScanAlign, MOTOR_BRAKE);
}

void TmblrMtrContr(string Direction)
{
  if(Direction == "On")
  {
    motor[TmblrMtrSwtch] = -50;
    wait1Msec(500);
    motor[TmblrMtrSwtch] = 0;
  }
  else
  {
    motor[TmblrMtrSwtch] = 50;
    wait1Msec(500);
    motor[TmblrMtrSwtch] = 0;
  }
}

void GetBlackOrGreenColor()
{
  short RGB[4];

  getColorSensorData(ColorSens, colorAtoD,   &RGB);

  if((RGB[0]>=200&&RGB[0]<=300)&&(RGB[1]>=200&&RGB[1]<=300)&&(RGB[2]>=100&&RGB[2]<=199))
  {
    BrickColor = 1;
  }
  else if((RGB[0]>=400&&RGB[0]<=500)&&(RGB[1]>=400&&RGB[1]<=500)&&(RGB[2]>=200&&RGB[2]<=300))
  {
    BrickColor = 3;
  }
}

void RefillBrickQueue()
{
  bool DoneDispensing = false;

  PFMotor(Tread1, Tread1Fwd);
  TmblrMtrContr("On");
  PFMotor(DispenserMtr, DispenserMtrDisp);

  while(DoneDispensing == false)
  {
    if(SensorValue(LightSens) < 10)
    {
      wait1Msec(750);
      if(SensorValue(LightSens) < 10) DoneDispensing = true;
    }
  }
  TmblrMtrContr("Off");
  PFMotor(DispenserMtr, MOTOR_BRAKE);
}

More info coming as soon as I have some pics!

Comments, suggestions, LOLs?

[inxt-generation]

EDIT: I changed the pics to a more forum-friendly size. If you click on one, (open it in a new tab) it links to the full-size version.

First of all, here's the link to the gallery: The assembly, and the dissassembly. Some of the pictures are out of order. I don't know why.

Overview
______________________________________________________________________________________________________________________

The entire thing: (just ignore all the backround stuff.)



Here's where the brick gets scanned, picked, and placed:



This part handles the brick aligning and dispensing:



The Pneumatic Station:



The arm, and dispenserys:




Next up: The Pneumatic station in detail.

[inxt-generation]

The Pneumatic Station:
______________________________________________________________________________________________________________________

Pre P.S. I realize that most of the close-up pictures are blurry. But, when I took them, on the camera display they didn't look blurry. Please forgive me.

The Pneumatic Station:




Power is supplied to the motor via this


which takes a Barrel-Jack power supply, in this case 12V 2A well regulated portable hard drive power supply. (Warning: overvolt motors at your own risk! :breakcomputer: ) puts it through a on-off switch, and then has a traditional LEGO electric plate from a broken RCX sensor port.


A converter cable is then connected to the power, and so to the PF-polarity switch pressure controller. Oh, also in this pic: the air release valve. Whoooooshhhh!



The switch is "on" when the cylinder is retracted. As the air pressure builds, the pneumatic cylinder will eventually overcome the force of the rubber bands at ~30 PSI, and thus push forward and turn the switch off, thus deactivating the motor. At ~20 PSI, the rubber bands pull the cylinder back down, and the process is repeated.


A converter cable connects the output of the PF-Switch to a RC-Motor running two small pumps:



Even though the pumps are small, since the motor is going at over 1000RPMs, it gets a lot of air out in a hurry. The performance is somewhat impeded with the switch, though. For some reason, it tends to flicker on and off rapidly, kind of like PWM. I don't know why. So, even now, it's not going full speed.


And, for your convenience, a conveniently placed air pressure meter placed at a convenient place so you can conveniently see the air pressure at your convenience!!!




Air tanks. Can't have pneumatics without air tanks!




The motorized pneumatic switch. It's really simple. Run the motor in one direction for half a second, run it in the other direction for half a second. With clutch gears, it's easy!!




And now, the moment you've all been waiting for..... The Claw!!!! :biggrin: :biggrin: :biggrin:



I think that the claw is the most unique thing about this. As you can see, it hinges on shock absorbers, which allows the cylinder to continue to move up after the claw is closed, thusly (as shown in the pic below) compressing the shocks, and raising the claw. I think that I'm the first person to do this. If any of you have seen it before, please tell me.




And (of course) a narrated video showing it all in action.






Next up: The Arm.

[inxt-generation]

Aghhh, so sorry that I haven't posted any more about this. I've been really caught up in preparing for BrickWorld, so there probably won't be any updates till it's over. Sorry.

[inxt-generation]

w00t This got featured on the NXT Step!!!! http://thenxtstep.blogspot.com/2012/08/ ... orter.html

[kieran-pierce]

Wow, just found this looks amazing, vlove the use of a mix, not just NXT, well done