// Brick injection motor is on OUT_C // Arm rotation motor is on OUT_A // Brick ejection motor is on OUT_B #define Rotate OUT_A #define Eject OUT_B #define Inject OUT_C // Color Sensor is on SENSOR_2 // Arm rotation step touch sensor is on SENSOR_1 // Arm neutral position touch sensor is on SENSOR_3 #define RotSensor SENSOR_1 #define HomeSensor SENSOR_3 #define ColorSensor SENSOR_2 // We sort 6 colors + black (no brick) to stop sorter when supply is empty #define NBColors 7 // Defines the value above which you are on sensor key position #define DummyThreshold 1005 // These arrays hold colors of the reference bricks int r[NBColors],g[NBColors],b[NBColors]; int Red, Green, Blue; int OutBin; task main () { int i; SetSensor (RotSensor, SENSOR_TOUCH); SetSensor (HomeSensor, SENSOR_TOUCH); SetSensor (ColorSensor, SENSOR_LIGHT); SetSensorMode (ColorSensor, SENSOR_MODE_RAW); Calibrate(); // initialisations and records colors of the reference bricks PlaySound (SOUND_DOUBLE_BEEP); while (true) { InjectBrick(); // feeds a new brick ReadColor(); // gets its color SortBrick (); // compares with reference bricks if(OutBin==0) // color = 0 = black: supply is empty so quit { Restore(); break; } GotoBin(OutBin); // positions rotating arm over the right output bin EjectBrick(); // and ejects brick Restore(); // returns arm to home position } PlaySound (SOUND_UP); } // pushes a new brick from chute to rotation arm under color sensor sub InjectBrick () { OnRev (Inject); Wait (80); OnFwd (Inject); Wait (80); Off(Inject); } // ejects the brick out of rotating arm sub EjectBrick () { OnRev (Eject); Wait (30); OnFwd (Eject); Wait (30); Off(Eject); } // returns arm to home position sub Restore () { OnRev (Rotate); until (HomeSensor==1); Off (Rotate); } // performs initialisations and records colors of the reference bricks void Calibrate () { int i; InitColSensor(); // sets color sensor to start position ReadColor(); // reads color value for no brick (black) r[0]=Red; // and sets them in position 0 g[0]=Green; b[0]=Blue; for(i=1; i