LEGO Mindstorms NXT Bin Emptying Robot – Mark III
Download PDF formatThis is a LEGO Mindstorms NXT "Bin Emptying Robot Mark III". It is part of a larger project I have been working on, my "Ball Sorting Factory". A forklift delivers full Bins of balls to be emptied. The Emptied bins are then taken away to the processing facility to be refilled. The Mark II version worked well, but was a little unreliable, unlike this version. Like the previous version, it uses a "Ball Repository" to store, mix and reload the Production Line with Balls when the "Ball Sorting Factory" needs them.
The major difference with this version is electronic. It uses a BETA Version of the soon to be released Mindsensors SensorMUX is a multiplexer which allows the connect of 4x NXT sensors to single NXT port. The Mindsensors SensorMUX can connect any sensor designed for Mindstorms NXT, both LEGO and/or third party sensors. The Mindsensors SensorMUX is requires an external battery source supplying a voltage range between 6 and 8.4 volts to operate. View my previous article, "Power the Mindsensors SensorMUX from a 9V Battery" for details on a simple power supply derived from a 9v (PP3 battery) for the Mindsensors SensorMUX.
I am really impressed with the Mindsensors SensorMUX, and its capabilities. It is a must have for all serious LEGO Mindstorms NXT users. It is especially useful when using multiple analogue sensors, such as combining Touch and Light Sensors. With this project, the Mindsensors SensorMUX is used to multiplex:
The only minor down side I have found is the requirement of an external power source for the Mindsensors SensorMUX. This said, the current drain is very low, so you can use a low amp/hour rated standard 9v (PP3 battery) or 7.4v Li-Po Battery.
Notes to Remember:
You must keep in mind that the Mindsensors SensorMUX is not capable of switching quickly between it’s Sensor Channels. When using it with a robot to navigate around with one sensor connected to Channel 1. Then when the robot gets to a certain point, it switch to another sensor on Channel 2 for a new specific task, before then do something else.
Also you can’t have two (or more) tasks running at once, accessing seperate Mindsensors SensorMUX Channels. It is not possible to have a separate task chatting with the Mindsensors SensorMUX to fetch Battery Information, while another task is switching Channels to chat with sensors as well. The Battery Information is access via virtual channel 0, by querying the Mindsensors SensorMUX using I2C commands.
A big thanks goes out to Xander Soldaat for producing the RobotC Programming Language Third Party Driver suite. Also for his quick response to fixing the BUGS I uncovered in his Mindsensors SensorMUX Driver. It's all part of the fun and frustration associated with BETA TESTING new LEGO Mindstorms NXT Sensors and other add-ons.
For full details on the operation of the NXT Bin Emptying Robot Mark III, refer to my NXT Bin Emptying Robot Mark II.
LEGO Mindstorms NXT Bin Emptying Robot – Mark III
RobotC Source Code:
#pragma config(Sensor, S1, MSSMUX, sensorI2CCustom)
#pragma config(Sensor, S4, NXTSERVO, sensorI2CCustom)
#pragma config(Motor, motorA, LIFTmotor, tmotorNXT, openLoop)
#pragma config(Motor, motorB, TIPmotor, tmotorNXT, openLoop, encoder)
#pragma config(Motor, motorC, DISPENSEmotor, tmotorNXT, openLoop, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
/* -----------------------------------------------------------------------------
Containor Emtying Mechanism
2013 www.rjmcnamara.com
==============================================================================*/
#include "drivers/mindsensors-sensormux.h" // 3x Touch Sensors & LASER Sensor
#include "drivers/mindensors-servo.h" // Control Ball Gate
int LASER = 0, BinCount = 0, voltage = 0, LaserLevel = 16;
/* -----------------------------------------------------------------------------
Monitor Ball Mixing Mechanism
==============================================================================*/
task Monitor()
{
eraseDisplay();
while (true)
{
nxtDisplayCenteredBigTextLine(0, "Emptying");
nxtDisplayCenteredBigTextLine(2, "Machine");
nxtDisplayTextLine(5, " Bins: %d", BinCount);
nxtDisplayTextLine(6, " S-MUX: %d mV", voltage);
nxtDisplayTextLine(7, " LASER Level: %d", LASER);
wait1Msec(250);
}
}
/* -----------------------------------------------------------------------------
Start Restocking Balls
==============================================================================*/
void RestockBalls()
{
// OPEN GATE
NXTServoSetPos(NXTSERVO, 1, 1700, 30);
wait1Msec(2000);
nxtDisplayCenteredTextLine(5, "Restock Balls");
// Start Dispensing Balls
motor[DISPENSEmotor] = 100;
wait1Msec(7000);
motor[DISPENSEmotor] = 0;
// CLOSE GATE
NXTServoSetPos(NXTSERVO, 1, 1500, 30);
wait1Msec(1000);
BinCount = 0;
}
/* -----------------------------------------------------------------------------
Reset Elevator to Down Position
==============================================================================*/
void ResetElevator()
{
motor[LIFTmotor] = 50;
wait1Msec(300);
MSSMUXsetChan(MSSMUX, 3);
SetSensorType(MSSMUX, sensorTouch);
while (!SensorValue(MSSMUX))
{
motor[LIFTmotor] = 100;
}
motor[LIFTmotor] = 0;
wait1Msec(1000);
}
/* -----------------------------------------------------------------------------
Raise Containor to Empty Position
==============================================================================*/
void RaiseContainor()
{
nMotorEncoder[TIPmotor] = 0;
while(nMotorEncoder[TIPmotor] < 100) // while the Motor Encoder of Motor B has not yet reached 360 counts:
{
motor[TIPmotor] = 100; // motor B is given a power level of 75
}
motor[TIPmotor] = 0;
// Tilt Containor Upwards
motor[TIPmotor] = 75;
wait1Msec(2000);
motor[TIPmotor] = 0;
PlaySound(soundUpwardTones);
// Lift Containor Up to Bin
motor[LIFTmotor] = -50;
wait1Msec(500);
MSSMUXsetChan(MSSMUX, 2);
SetSensorType(MSSMUX, sensorTouch);
while (!SensorValue(MSSMUX))
{
motor[LIFTmotor] = -100;
}
motor[LIFTmotor] = 0;
wait1Msec(1000);
}
/* -----------------------------------------------------------------------------
Reset Elevator Tines to Load Position
==============================================================================*/
void ResetTines()
{
MSSMUXsetChan(MSSMUX, 1);
SetSensorType(MSSMUX, sensorTouch);
while (!SensorValue(MSSMUX))
{
motor[TIPmotor] = -50;
}
wait1Msec(100);
motor[TIPmotor] = 0;
wait1Msec(1000);
PlaySound(soundDownwardTones);
}
/* -----------------------------------------------------------------------------
Raise the Elevator Tines Tip Containor
==============================================================================*/
void TipContainor()
{
// Tip Containor into Bin
while(nMotorEncoder[TIPmotor] < 480) // while the Motor Encoder of Motor B has not yet reached 360 counts:
{
motor[TIPmotor] = 100; // motor B is given a power level of 75
}
motor[TIPmotor] = 0;
wait1Msec(2000);
// Tilt Containor Downwards
while(nMotorEncoder[TIPmotor] > 100) // while the Motor Encoder of Motor B has not yet reached 360 counts:
{
motor[TIPmotor] = -75; // motor B is given a power level of 75
}
motor[TIPmotor] = 0;
wait1Msec(500);
}
/* -----------------------------------------------------------------------------
Measure Reflected LASER Level
==============================================================================*/
void MeasureLaser()
{
MSSMUXsetChan(MSSMUX, 4);
SetSensorType(MSSMUX, sensorLightActive);
LASER = SensorValue(MSSMUX);
nxtDisplayTextLine(7, " LASER Level: %d", LASER);
}
/* -----------------------------------------------------------------------------
Initilise the Bin Emptying Machine
==============================================================================*/
void Initilise()
{
// CLOSE the BALL GATE
NXTServoSetPos(NXTSERVO, 1, 1500, 30);
wait1Msec(1000);
// Check if Elevator is at TOP
MSSMUXsetChan(MSSMUX, 2);
SetSensorType(MSSMUX, sensorTouch);
if (!SensorValue(MSSMUX))
{
motor[TIPmotor] = 50;
wait1Msec(1500);
motor[TIPmotor] = 0;
}
// RESET the Elevator
if (!SensorValue(MSSMUX))
{
motor[LIFTmotor] = -50;
wait1Msec(1000);
motor[LIFTmotor] = 0;
}
MSSMUXsetChan(MSSMUX, 3);
SetSensorType(MSSMUX, sensorTouch);
while (!SensorValue(MSSMUX))
{
motor[LIFTmotor] = 50;
}
motor[LIFTmotor] = 0;
// RESET the Containor Tines
MSSMUXsetChan(MSSMUX, 1);
SetSensorType(MSSMUX, sensorTouch);
while (!SensorValue(MSSMUX))
{
motor[TIPmotor] = -50;
}
wait1Msec(100);
motor[TIPmotor] = 0;
wait1Msec(1000);
PlaySound(soundDownwardTones);
}
/* -----------------------------------------------------------------------------
Bin Emptying Machine Control Center
==============================================================================*/
task main()
{
bNoPowerDownOnACAdaptor = true; // will NOT power down when connected to AC adapter
voltage = MSSMUXreadBattery(MSSMUX);
wait1Msec(500);
StartTask(Monitor);
Initilise(); // RESET the Bin Emptying Machine
/*** Begin Containor Emptying Loop ***/
while (true)
{
MeasureLaser();
if (LASER < LaserLevel) // Detect New Containor
{
BinCount = BinCount + 1;
wait1Msec(2000);
RaiseContainor();
// Start Ball Mixing
motor[DISPENSEmotor] = -100;
TipContainor();
ResetElevator();
ResetTines();
// Stop Ball Mixing
motor[DISPENSEmotor] = 0;
// Wait Untill Containor is Removed
while (LASER < LaserLevel)
{
MeasureLaser();
wait1Msec(250);
}
// Count Number of Bins Emptied
if (BinCount == 2) RestockBalls();
}
}
}
GD Star Rating
loading...
loading...
GD Star Rating
loading...
LEGO Mindstorms NXT Bin Emptying Robot – Mark III, loading...
Link to this post!
