Hey guys,
I have code that uses a vision sensor to find a green ball center it on the x axis and using the y axis and the center of said green ball to move forward until it is roughly inside our claw. This is where the problem begins. The claw closes then opens which is fine i guess, however it does not seem to close again and the claw just goes up and goes in circles. All help and suggestions are welcome
Thanks, Lugburz
PS: CODE DOWN BELOW
#include "robot-config.h"
void OpenClaw(){
//Claw open
ClawMotor.setStopping(brakeType::hold);
ClawMotor.setMaxTorque(50,percentUnits::pct);
ClawMotor.rotateFor(2,timeUnits::sec,20,velocityUnits::pct);
}
void CloseClaw(){
//Claw close
ClawMotor.setStopping(brakeType::hold);
ClawMotor.setMaxTorque(50,percentUnits::pct);
ClawMotor.rotateFor(2,timeUnits::sec,-20,velocityUnits::pct);
}
void ArmUp(){
// Arm up
ArmMotor.setStopping(brakeType::hold);
ArmMotor.setTimeout(2,timeUnits::sec);
ArmMotor.rotateFor(400,rotationUnits::deg,20,velocityUnits::pct);
}
void ArmDown(){
//Arm down
ArmMotor.setStopping(brakeType::coast);
ArmMotor.setTimeout(2,timeUnits::sec);
ArmMotor.rotateFor(-400,rotationUnits::deg,20,velocityUnits::pct);
}
int main(){
//Store Vision Sensor Field of View Centerpoint and desired Offset as vairables
int centerFOV = 180;
int offsetX = 15; //Increase or decrease this value to adjust precision of the turn
int centerFOVy = 135;
int offsetY=20;
while(true){
Brain.Screen.clearLine();
VisionSensor.takeSnapshot(SIG_GREEN);
Brain.Screen.clearLine();
//If the Vision Sensor detects a matching colored object...
if(VisionSensor.largestObject.exists){
if(VisionSensor.largestObject.centerX > centerFOV + offsetX){ //...if the object is off to the right, turn right...
RightMotor.spin(directionType::rev, 20, velocityUnits::pct);
LeftMotor.spin(directionType::fwd, 20, velocityUnits::pct);
}else if(VisionSensor.largestObject.centerX < centerFOV - offsetX){ //...else, if the object is off to the left, turn left...
RightMotor.spin(directionType::fwd, 20, velocityUnits::pct);
LeftMotor.spin(directionType::rev, 20, velocityUnits::pct);
}
else
{ //...else (it is roughly centered) stop turning.
RightMotor.stop(brakeType::brake);
LeftMotor.stop(brakeType::brake);
}
if(VisionSensor.largestObject.centerY<centerFOVy+offsetY)
{
OpenClaw();
ArmDown();
if(VisionSensor.largestObject.centerY<100)
{
RightMotor.spin(directionType::fwd,20,velocityUnits::pct);
LeftMotor.spin(directionType::fwd,20,velocityUnits::pct);
}
else if(100<VisionSensor.largestObject.centerY<centerFOVy+1)
{
RightMotor.spin(directionType::fwd,10,velocityUnits::pct);
LeftMotor.spin(directionType::fwd,10,velocityUnits::pct);
}
}
CloseClaw();
ArmUp();
}
}
}