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;
}
}

Written on November 10, 2017