Adding Code Fixes to the Robot

Tags: software and control
Personhours: 5
Adding Code Fixes to the Robot By Tycho

Task: Add code updates

These commits add said functionality:

  • Pre-game logic - joystick control
  • Fix PID settings
  • Autonomous resets motor
  • Jewel Arm functionality
  • Autonomous changes
  • Tests servos

These commits allow better QoL for our drivers, allow our robot to function more smoothly both in autonomous and during TeleOp, allows us to score the jewels, and lets us test servos.

Jewel Arm


package org.firstinspires.ftc.teamcode;

import com.qualcomm.robotcore.hardware.NormalizedColorSensor;
import com.qualcomm.robotcore.hardware.Servo;

/**
 * Created by 2938061 on 11/10/2017.
 */

public class JewelArm {

    private Servo servoJewel;
    private NormalizedColorSensor colorJewel;
    private int jewelUpPos;
    private int jewelDownPos;

    public JewelArm(Servo servoJewel, NormalizedColorSensor colorJewel, int jewelUpPos, int jewelDownPos){
        this.servoJewel = servoJewel;
        this.colorJewel = colorJewel;
        this.jewelUpPos = jewelUpPos;
        this.jewelDownPos = jewelDownPos;
    }

    public void liftArm(){
        servoJewel.setPosition(ServoNormalize(jewelUpPos));
    }
    public void lowerArm(){
        servoJewel.setPosition(ServoNormalize(jewelDownPos));
    }

    public static double ServoNormalize(int pulse){
        double normalized = (double)pulse;
        return (normalized - 750.0) / 1500.0; //convert mr servo controller pulse width to double on _0 - 1 scale
    }

}

Autonomous

		public void autonomous(){
        switch(autoState){
            case 0: //scan vuforia target and deploy jewel arm
                robot.jewel.lowerArm();
                autoTimer = futureTime(1.5f);
                if(autoTimer < System.nanoTime()) {
                    relicCase = getRelicCodex();
                    jewelMatches = robot.doesJewelMatch(isBlue);
                    autoState++;
                }
                break;
            case 1: //small turn to knock off jewel
                if ((isBlue && jewelMatches)||(!isBlue && !jewelMatches)){
                    if(robot.RotateIMU(10, .5)){
                        robot.resetMotors(true);
                    }
                }
                else{
                    if(robot.RotateIMU(350, .5)){
                        robot.resetMotors(true);
                    }
                }
                break;
            case 2: //lift jewel arm
                robot.jewel.liftArm();
                autoTimer = futureTime(1.5f);
                if(autoTimer < System.nanoTime()) {
                    jewelMatches = robot.doesJewelMatch(isBlue);
                    autoState++;
                }
            case 3: //turn parallel to the wall
                if(isBlue){
                    if(robot.RotateIMU(270, 2.0)){
                        robot.resetMotors(true);
                        autoState++;
                    }
                }
                else{
                    if(robot.RotateIMU(90, 2.0)){
                        robot.resetMotors(true);
                        autoState++;
                    }
                }
                autoState++;
                break;
            case 4: //drive off the balance stone
                if(robot.driveForward(true, .3, .5)) {
                    robot.resetMotors(true);
                    autoState++;
                }
                break;
            case 5: //re-orient robot
                if(isBlue){
                    if(robot.RotateIMU(270, 1.0)){
                        robot.resetMotors(true);
                        autoState++;
                    }
                }
                else{
                    if(robot.RotateIMU(90, 1.0)){
                        robot.resetMotors(true);
                        autoState++;
                    }
                }
                break;
            case 6: //drive to proper crypto box column based on vuforia target
                switch (relicCase) {
                    case 0:
                        if(robot.driveForward(true, .5, .35)) {
                            robot.resetMotors(true);
                            autoState++;
                        }
                        break;
                    case 1:
                        if(robot.driveForward(true, .75, .35)) {
                            robot.resetMotors(true);
                            autoState++;
                        }
                        autoState++;
                        break;
                    case 2:
                        if(robot.driveForward(true, 1.0, .35)) {
                            robot.resetMotors(true);
                            autoState++;
                        }
                        autoState++;
                        break;
                }
                break;
            case 7: //turn to crypto box
                if(isBlue){
                    if(robot.RotateIMU(315, 1.5)){
                        robot.resetMotors(true);
                        autoState++;
                    }
                }
                else{
                    if(robot.RotateIMU(45, 1.5)){
                        robot.resetMotors(true);
                        autoState++;
                    }
                }
                break;
            case 8: //deposit glyph
                if(robot.driveForward(true, 1.0, .50)) {
                    robot.resetMotors(true);
                    robot.glyphSystem.ReleaseGrip();
                    autoState++;
                }
                break;
            case 9: //back away from crypto box
                if(robot.driveForward(false, .5, .50)){
                    robot.resetMotors(true);
                    autoState++;
                }
                break;
            default:
                robot.resetMotors(true);
                autoState = 0;
                active = false;
                state = 0;
                break;
        }
    }
    public void autonomous2 (){

        switch(autoState){
            case 0: //scan vuforia target and deploy jewel arm
                robot.jewel.lowerArm();
                autoTimer = futureTime(1.5f);
                if(autoTimer < System.nanoTime()) {
                    relicCase = getRelicCodex();
                    jewelMatches = robot.doesJewelMatch(isBlue);
                    autoState++;
                }
                break;
            case 1: //small turn to knock off jewel
                if ((isBlue && jewelMatches)||(!isBlue && !jewelMatches)){
                    if(robot.RotateIMU(10, .5)){
                        robot.resetMotors(true);
                    }
                }
                else{
                    if(robot.RotateIMU(350, .5)){
                        robot.resetMotors(true);
                    }
                }
                break;
            case 2: //lift jewel arm
                robot.jewel.liftArm();
                autoTimer = futureTime(1.5f);
                if(autoTimer < System.nanoTime()) {
                    jewelMatches = robot.doesJewelMatch(isBlue);
                    autoState++;
                }
            case 3: //turn parallel to the wall
                if(isBlue){
                    if(robot.RotateIMU(270, 2.0)){
                        robot.resetMotors(true);
                        autoState++;
                    }
                }
                else{
                    if(robot.RotateIMU(90, 2.0)){
                        robot.resetMotors(true);
                        autoState++;
                    }
                }
                autoState++;
                break;
            case 4: //drive off the balance stone
                if(robot.driveForward(true, .3, .5)) {
                    robot.resetMotors(true);
                    autoState++;
                }
                break;
            case 5: //re-orient robot
                if(isBlue){
                    if(robot.RotateIMU(270, 1.0)){
                        robot.resetMotors(true);
                        autoState++;
                    }
                }
                else{
                    if(robot.RotateIMU(90, 1.0)){
                        robot.resetMotors(true);
                        autoState++;
                    }
                }
                break;
            case 6: //drive to proper crypto box column based on vuforia target
                switch (relicCase) {
                    case 0:
                        if(robot.driveStrafe(true, .00, .35)) {
                            robot.resetMotors(true);
                            autoState++;
                        }
                        break;
                    case 1:
                        if(robot.driveStrafe(true, .25, .35)) {
                            robot.resetMotors(true);
                            autoState++;
                        }
                        autoState++;
                        break;
                    case 2:
                        if(robot.driveStrafe(true, .50, .35)) {
                            robot.resetMotors(true);
                            autoState++;
                        }
                        autoState++;
                        break;
                }
                break;
            case 7: //turn to crypto box
                if(isBlue){
                    if(robot.RotateIMU(215, 1.5)){
                        robot.resetMotors(true);
                        autoState++;
                    }
                }
                else{
                    if(robot.RotateIMU(135, 1.5)){
                        robot.resetMotors(true);
                        autoState++;
                    }
                }
                break;
            case 8: //deposit glyph
                if(robot.driveForward(true, 1.0, .50)) {
                    robot.resetMotors(true);
                    robot.glyphSystem.ReleaseGrip();
                    autoState++;
                }
                break;
            case 9: //back away from crypto box
                if(robot.driveForward(false, .5, .50)){
                    robot.resetMotors(true);
                    autoState++;
                }
                break;
            default:
                robot.resetMotors(true);
                autoState = 0;
                active = false;
                state = 0;
                break;
        }
    }

Date | November 10, 2017