Skip to content
Candy Sorter

Code Deep Dive: Logic

The final piece! Let's explore the functions that make the robot move and the main loop that controls everything.

7. The Choreography (Movement Functions)

These functions are like dance moves. Instead of writing "Turn on pin 14, wait, turn off" every time, we give the move a name.

movements.cpp
1void CollectGems() {
2  TopServo.write(collectAngle + TopServoOffset);
3  delay(collectDelay);
4}
5
6void MoveToColorSensor() {
7  TopServo.write(sensorAngle + TopServoOffset);
8  delay(moveDelay);
9}
10
11void TopGems() {
12  TopServo.write(dropAngle + TopServoOffset);
13  delay(topDelay);
14}
15
16void MoveSlideTo(int angle) {
17  SlideServo.write(angle + SlideServoOffset);
18  delay(slideDelay);
19}

CollectGems

Moves the top servo to grab a gem from the hopper.

MoveToColorSensor

Swings the arm over to the "eye" for inspection.

TopGems

Drops the gem into the chute.

MoveSlideTo

Moves the ramp to the correct bucket angle.

8. The Brain (SenseColour)

This function is the heart of the robot. It looks at the gem, cleans up the data, and decides what color it is.

sense_colour.cpp
1int SenseColour() {
2  // 1. Read the raw data from sensor
3  tcs.getRawData(&r, &g, &b, &c);
4
5  // 2. Normalize (Brighten) the colors
6  uint16_t maxVal = max(r, max(g, b));
7  uint16_t normR = (maxVal > 0) ? (r * 255 / maxVal) : 0;
8  uint16_t normG = (maxVal > 0) ? (g * 255 / maxVal) : 0;
9  uint16_t normB = (maxVal > 0) ? (b * 255 / maxVal) : 0;
10
11  // 3. Find the best match
12  float minDistance = 999999;
13  int colorAngle = -1;
14  
15  for (Color color : colors) {
16    float distance = calculateDistance(normR, normG, normB, 
17                                        color.r, color.g, color.b);
18    if (distance < minDistance) {
19      minDistance = distance;
20      colorAngle = color.number;
21    }
22  }
23  
24  return colorAngle;
25}

Why Normalization?

Sometimes the room is dark, sometimes bright. Normalization scales colors so the brightest part is always 255.

This helps the robot recognize "Red" whether it's in shadow or under a flashlight!

9. The Main Loop (The Boss)

This is where the robot waits for your command and runs the sorting process.

main_loop.cpp
1void loop() {
2  if (Serial.available()) {
3    String input = Serial.readStringUntil('\n');
4    
5    if (input.equalsIgnoreCase("s")) {
6       // Start Sorting Loop
7       do {
8         CollectGems();
9         MoveToColorSensor();
10         angle = SenseColour();
11         MoveSlideTo(angle);
12         TopGems();
13       } while (angle >= 0);
14       
15       Serial.println("Sorting complete!");
16    }
17    
18    if (input.equalsIgnoreCase("c")) {
19       // Calibration mode
20    }
21    
22    if (input.equalsIgnoreCase("r")) {
23       // Read single color
24    }
25  }
26}

Info

The do-while loop keeps sorting until angle >= 0 becomes false — that's when "Blank" is detected with angle = -1!