ROBOTC.net Blog  

ROBOTC News

Archive for January 19th, 2011

NXT Color sorter using only ONE motor

without comments

All credit goes to ivanseidel of our ROBOTC forums. Great job!

ivanseidel noticed a few projects that used 2, 3, 4, 5, or even 6 motor assemblies that sorted LEGOs into separate containers. He then wondered, “where are the one motor sorters?”. So he went to work.

Below you will find a video of ivanseidel’s amazing creation, the one motor color sorter:


YouTube Direct Link 

On the actual coding algorithm, ivanseidel says, “The code is also a new thing, that adapts to the colors very easy, like a fusy algorithm.”

Code:

Below is the code for the main program:

#pragma config(Sensor, S1,     L,                   sensorLightActive)
#pragma config(Sensor, S4,     T,                   sensorTouch)
#pragma config(Motor,  motorA,          M1,            tmotorNormal, openLoop, encoder)

//Função: separador de cores com lego mindstorms nxt
//Código feito por Ivan Seidel
//http://techlego.blogspot.com/

//Peço que deixem meus créditos apenas!

#include "MV-lib.c"

#define SpeedNormal           56
#define DifMaxLight           50
#define RotateAngleFirst      200
#define RotateAngleMultiple   180
#define RotateAngleBack       150
#define RotateAngleAfterT     0
#define MaxColors             7

int ArrayLights[10];
int ColorsFound = 0;
int LightValue;

//Le o valor do sensor deluz, e checa se esta no range de algum valor do array
//Reads light sensor and checks if that value is in the range of somevalue in the array
int IsInRange(int CheckValue){
 int i=0,found=-1;
 while(i<ColorsFound){
 if(CheckValue<=ArrayLights[i]+DifMaxLight &&  CheckValue>=ArrayLights[i]-DifMaxLight && ArrayLights[i]!=0){
 found=i;
 break;
 }
 i++;
 }
 return found;
}

void InsertValue(int InsertValue){
 ArrayLights[ColorsFound]=InsertValue;
 ColorsFound++;
}

task main(){
 int KeepRuning=true;
 while(KeepRuning){

 //Reseta todos os valores e vai para o comeco
 //Resets position and all states to 0
 while(SensorValue[T]==0){
 motor[M1]=SpeedNormal;
 }
 MV_StopMotors();
 MV_Vira(-SpeedNormal,RotateAngleAfterT,M1);
 wait10Msec(50);
 //Inicia o programaprincial de separar as cores
 //Starts the main program of sorting coloros
 int samples=10,LightValue;
 for(int y=0;y<samples;y++){
 LightValue+=SensorRaw[L];
 }
 LightValue=LightValue/samples;
 MV_Vira(SpeedNormal/1.5,RotateAngleFirst,M1);
 bool looping=true;
 while(looping){
 int foundColorPosition=IsInRange(LightValue);
 if(foundColorPosition!=-1){
 for (int i=0;i < foundColorPosition+1;i++){
 PlayTone(500,5);
 wait10Msec(10);
 }
 wait10Msec(50);
 MV_Vira(SpeedNormal,(foundColorPosition+1)*RotateAngleMultiple,M1);
 MV_Vira(-SpeedNormal*1.6,RotateAngleBack,M1);
 wait10Msec(50);
 looping=false;
 }else{
 InsertValue(LightValue);
 }
 }
 }
}

Below is the code for MV-lib.c:

//----------------------------------------------//
//----------------------------------------------//
//Library que possui as funcoes de mover motores//
//----------------------------------------------//
//----------------------------------------------//
#define MV_EMotor    motorA
#define MV_DMotor    motorC
#define MV_GarraMotor     motorB
#define MV_TimeLimit 70 //5 secs

void MV_StopMotors(){
 motor[MV_EMotor] = 0;
 motor[MV_DMotor] = 0;
 motor[MV_GarraMotor]  = 0;
}

void MV_Reto_Unlimited(int forca,int steering=0){
 motor[MV_EMotor]=forca+steering;
 motor[MV_DMotor]=forca-steering;
}
void MV_Spin_Unlimited(int forca){
 motor[MV_EMotor]=forca;
 motor[MV_DMotor]=-forca;
}

void MV_Vira(int forca, int degree,byte lado, int tempo=0){
 time100[T2]=0;
 if(degree>0){
 nMotorEncoder[lado]=0;
 while(nMotorEncoder[lado]<degree && nMotorEncoder[lado]>-degree && time100[T2]<MV_TimeLimit){
 motor[lado]=forca;
 }
 MV_StopMotors();
 }else if(tempo>0){
 time100[T1]=0;
 while(time100[T1]<tempo){
 motor[lado]=forca;
 motor[lado]=forca;
 }
 MV_StopMotors();
 }
}

void MV_Spin(int forca, int degree, int tempo=0){
 time100[T2]=0;
 nMotorEncoder[MV_EMotor]=0;
 nMotorEncoder[MV_DMotor]=0;
 if(degree>0){
 while(nMotorEncoder[MV_EMotor]<degree &&  nMotorEncoder[MV_EMotor]>-degree &&  time100[T2]<MV_TimeLimit){
 motor[MV_EMotor]=forca;
 motor[MV_DMotor]=-forca;
 }
 MV_StopMotors();
 }else if(tempo>0){
 time100[T1]=0;
 while(time100[T1]<tempo){
 motor[MV_EMotor]=forca;
 motor[MV_DMotor]=-forca;
 }
 MV_StopMotors();
 }
}

Written by Vu Nguyen

January 19th, 2011 at 8:33 am

Posted in Cool projects,NXT