ROBOTC.net Blog  

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();
 }
}