Programming problems

Hi my friends, I am new on robotics and I really need your help. I have trouble with programming, here is the code:#include <Servo.h>

// Define servo objects for 4DOF arm
Servo servo1; // Base rotation
Servo servo2; // Shoulder
Servo servo3; // Elbow
Servo servo4; // Wrist

// Pins for servos
int servoPin1 = 9;
int servoPin2 = 10;
int servoPin3 = 11;
int servoPin4 = 12;

// Pins for TCS3200 color sensor
int s0 = 2;
int s1 = 3;
int s2 = 4;
int s3 = 5;
int sensorOut = 6;

void setup() {
// Attach the servos to the specified pins
servo1.attach(servoPin1);
servo2.attach(servoPin2);
servo3.attach(servoPin3);
servo4.attach(servoPin4);

// Setup for TCS3200 color sensor
pinMode(s0, OUTPUT);
pinMode(s1, OUTPUT);
pinMode(s2, OUTPUT);
pinMode(s3, OUTPUT);
pinMode(sensorOut, INPUT);

// Set frequency scaling to 20% (can be adjusted as needed)
digitalWrite(s0, HIGH);
digitalWrite(s1, LOW);

Serial.begin(9600);
}

void loop() {
int redFrequency = getRedFrequency();
int greenFrequency = getGreenFrequency();
int blueFrequency = getBlueFrequency();

String detectedColor = detectColor(redFrequency, greenFrequency, blueFrequency);

if (detectedColor != "UNKNOWN") {
delay(2000); // 2-second delay after detecting the color

if (detectedColor == "RED") {
  moveArmToPosition(90);  // Move arm to 90 degrees for RED
  Serial.println("Color detected: RED");
} else if (detectedColor == "ORANGE") {
  moveArmToPosition(135);  // Move arm to 135 degrees for ORANGE
  Serial.println("Color detected: ORANGE");
} else if (detectedColor == "GREEN") {
  moveArmToPosition(180);  // Move arm to 180 degrees for GREEN
  Serial.println("Color detected: GREEN");
}

delay(1000);  // Wait for 1 second before returning to the default position
resetArmPosition();  // Return the arm to its default position

} else {
Serial.println("Color detected: UNKNOWN");
}

delay(500); // Add a delay to stabilize readings
}

int getRedFrequency() {
digitalWrite(s2, LOW);
digitalWrite(s3, LOW);
return pulseIn(sensorOut, LOW);
}

int getGreenFrequency() {
digitalWrite(s2, HIGH);
digitalWrite(s3, HIGH);
return pulseIn(sensorOut, LOW);
}

int getBlueFrequency() {
digitalWrite(s2, LOW);
digitalWrite(s3, HIGH);
return pulseIn(sensorOut, LOW);
}

String detectColor(int red, int green, int blue) {
if (red < blue && red < green && red > 100) {
return "RED";
} else if (red > green && red < blue && green < blue) {
return "ORANGE";
} else if (green < red && green < blue) {
return "GREEN";
} else {
return "UNKNOWN";
}
}

void moveArmToPosition(int angle) {
// Move each servo to the desired angle
servo1.write(angle); // Adjust base rotation
servo2.write(angle); // Adjust shoulder
servo3.write(angle); // Adjust elbow
servo4.write(angle); // Adjust wrist
}

void resetArmPosition() {
// Reset each servo to the default position (e.g., 0 degrees)
servo1.write(0);
servo2.write(0);
servo3.write(0);
servo4.write(0);
}"

The purpose is when the color sensor detects the color of the tomato, the arm will pick it up and it moves to a certain angle as what is given in the code. Could you please help me? I really need your help. The problem is the Servo Motors doesn't move accurately. The color sensor is accurate and I tested the Servos 1 by 1, they are fine. I already placed an external to support voltage. But the arm doesn't really function.

3 posts - 3 participants

Read full topic