Announcement

Collapse
No announcement yet.

Stuck on coding two MR Color Sensors - assigned different identifiers but not working

Collapse
X
 
  • Filter
  • Time
  • Show
Clear All
new posts

  • Stuck on coding two MR Color Sensors - assigned different identifiers but not working

    My team is currently trying to use two modern robotics color sensors to sense the red and blue lines on the field. They are placed under our robot almost at the very front. We found this: http://ftcforum.usfirst.org/showthre...Not-Turning-On and completed all of the steps on the bottom post. Both LEDs turned on and returned values when connected to the computer. Once we tried to use the code we had written, one of the LEDs did not turn on and did not return any values because of this and it missed the lines and did not do the calculations. Our code is posted below.

    Can you give us some advice as to what we are doing wrong?


    package com.qualcomm.ftcrobotcontroller.opmodes;

    import android.app.Activity;
    import android.graphics.Color;
    import android.view.View;

    import com.qualcomm.ftccommon.DbgLog;
    import com.qualcomm.ftcrobotcontroller.R;
    import com.qualcomm.robotcore.eventloop.opmode.LinearOpMo de;
    import com.qualcomm.robotcore.hardware.ColorSensor;
    import com.qualcomm.robotcore.hardware.DcMotor;
    import com.qualcomm.robotcore.hardware.DcMotorController;
    import com.qualcomm.robotcore.hardware.DeviceInterfaceMod ule;
    import com.qualcomm.robotcore.hardware.Servo;

    public class Autonomous_ColorSensor extends LinearOpMode {

    ColorSensor sensorRGB;
    ColorSensor sensorRGB2;
    DcMotor R;
    DcMotor L;
    Servo plowR;
    Servo plowL;
    DcMotor lift;
    Servo belt;
    Servo left;
    Servo right;
    Servo redClimber;
    Servo blueClimber;
    Servo left2;
    Servo right2;
    DeviceInterfaceModule cdim;


    //Variables for alligning robot to beacon
    double initEncod=-1;
    double finalEncod;
    double init2Encod;
    double yChange;
    double targetY=10;
    double targetX=10.5;
    double width=7.5;
    double yPlus;
    double xPlus;
    double targetAngle;
    double targetDistance;
    double encPerDegree=12.75;

    int ENCODER_CPR = 1120; //encoder counts per revolution
    double GEAR_RATIO = 2; //gear ratio
    double WHEEL_DIAMETER = 3.3; //diameter of wheel in inches
    double DISTANCE;

    public double Counts(double distance){
    //DISTANCE in inches
    this.DISTANCE=distance;
    double CIRCUMFERENCE = Math.PI * WHEEL_DIAMETER;
    double ROTATIONS = distance/CIRCUMFERENCE;
    double COUNTS = ENCODER_CPR*ROTATIONS*GEAR_RATIO;
    return COUNTS;
    }
    public double Distance(double COUNTS){
    double ROTATIONS=COUNTS/GEAR_RATIO/ENCODER_CPR;
    double CIRCUMFERENCE= Math.PI*WHEEL_DIAMETER;
    double distance=ROTATIONS*CIRCUMFERENCE;
    return distance;
    }


    @Override
    public void runOpMode() throws InterruptedException {
    hardwareMap.logDevices();
    cdim=hardwareMap.deviceInterfaceModule.get("cdim") ;
    sensorRGB = hardwareMap.colorSensor.get("lc");
    sensorRGB2=hardwareMap.colorSensor.get("rc");
    R = hardwareMap.dcMotor.get("R");
    L = hardwareMap.dcMotor.get("L");
    lift = hardwareMap.dcMotor.get("lift");
    plowL=hardwareMap.servo.get("plowL");
    plowR=hardwareMap.servo.get("plowR");
    belt=hardwareMap.servo.get("belt");
    left=hardwareMap.servo.get("left");
    right=hardwareMap.servo.get("right");
    redClimber=hardwareMap.servo.get("redClimber");
    blueClimber=hardwareMap.servo.get("blueClimber");
    left2=hardwareMap.servo.get("left2");
    right2=hardwareMap.servo.get("right2");
    sensorRGB.setI2cAddress(0x3c);//right
    sensorRGB2.setI2cAddress(0x6c);//left


    R.setDirection(DcMotor.Direction.REVERSE);
    R.setMode(DcMotorController.RunMode.RUN_USING_ENCO DERS);
    L.setMode(DcMotorController.RunMode.RUN_USING_ENCO DERS);

    sensorRGB.enableLed(true);
    sensorRGB2.enableLed(true);
    waitOneFullHardwareCycle();

    waitForStart();
    float hsvValues[] = {0F,0F,0F};
    final float values[] = hsvValues;
    lift.setPower(-.2);
    belt.setPosition(.5);
    redClimber.setPosition(.1);
    blueClimber.setPosition(.9);
    right.setPosition(.24);
    left.setPosition(.95);
    left2.setPosition(.9);
    right2.setPosition(.1);
    sleep(250);
    lift.setPower(0);
    plowR.setPosition(.97);
    plowL.setPosition(.12);


    while (opModeIsActive()) {
    sensorRGB.enableLed(true);
    sensorRGB2.enableLed(true);

    while(sensorRGB2.red()<1.5){
    L.setPower(.25);
    R.setPower(.25);
    telemetry.addData("constant red", sensorRGB.red());
    telemetry.addData("constant blue", sensorRGB.blue());
    telemetry.addData("constant red2", sensorRGB2.red());
    telemetry.addData("constant blue2", sensorRGB2.blue());
    telemetry.addData("initencod", initEncod);
    telemetry.addData("finalencod", finalEncod );
    if(sensorRGB.red()>2&& initEncod==-1){
    initEncod=R.getCurrentPosition();
    telemetry.addData("first trigger", sensorRGB2.red());
    sleep(5000);
    }
    if(sensorRGB2.red()>2){
    L.setPower(0);
    R.setPower(0);
    finalEncod=L.getCurrentPosition();
    init2Encod=R.getCurrentPosition();
    telemetry.addData("second trigger", sensorRGB.red());
    sleep(5000);
    }
    }

    telemetry.addData("constant red out", sensorRGB.red());
    telemetry.addData("constant blue out", sensorRGB.blue());

    sleep(5000);
    yChange=Distance(finalEncod-initEncod);
    yPlus=targetY-yChange;
    xPlus=targetX-(width-yChange);
    targetAngle= Math.toDegrees(Math.atan(xPlus / yPlus));
    targetDistance=Math.hypot(xPlus, yPlus);

    while(R.getCurrentPosition() < init2Encod+(targetAngle*encPerDegree)){
    R.setPower(-.1);
    waitOneFullHardwareCycle();
    }
    init2Encod=init2Encod+(targetAngle*encPerDegree);
    R.setPower(0);
    L.setPower(0);

    while(R.getCurrentPosition()<Counts(targetDistance )+init2Encod){
    R.setPower(.1);
    L.setPower(.1);
    waitOneFullHardwareCycle();
    }
    init2Encod=init2Encod+Counts(targetDistance);
    R.setPower(0);
    L.setPower(0);
    redClimber.setPosition(.9);



    telemetry.addData("Clear", sensorRGB.alpha());
    telemetry.addData("Red ", sensorRGB.red());
    telemetry.addData("Green", sensorRGB.green());
    telemetry.addData("Blue ", sensorRGB.blue());
    telemetry.addData("Hue", hsvValues[0]);
    telemetry.addData("Clear2", sensorRGB2.alpha());
    telemetry.addData("Red2", sensorRGB2.red());
    telemetry.addData("Green2", sensorRGB2.green());
    telemetry.addData("Blue2", sensorRGB2.blue());
    telemetry.addData("Hue2", hsvValues[0]);



    waitOneFullHardwareCycle();
    }
    }
    }
Working...
X