Skip to content
This repository has been archived by the owner on Aug 26, 2021. It is now read-only.

Commit

Permalink
Merge pull request #12 from BotsBurgh/pid-tuning-auto-path
Browse files Browse the repository at this point in the history 
Pid tuning auto path
  • Loading branch information
@VarunDamarla
VarunDamarla committed Mar 28, 2021
2 parents ec89aaa + 97395f4 commit aacf76c
Show file tree
Hide file tree
Showing 40 changed files with 904 additions and 184 deletions.
2 changes: 2 additions & 0 deletions .travis.yml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,8 @@ licenses:
- google-gdk-license-.+
before_install:
- yes | sdkmanager --update
env:
- VUFORIA_KEY="\"\""
android:
components:
- tools
Expand Down
9 changes: 7 additions & 2 deletions TeamCode/build.gradle
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,13 +26,18 @@ dependencies {
}

buildscript {
ext.kotlinVersion = '1.4.21'
ext.kotlinVersion = '1.4.30'
repositories {
jcenter()
google()
}
dependencies {
classpath 'com.android.tools.build:gradle:4.1.2'
classpath 'com.android.tools.build:gradle:4.1.3'
classpath "org.jetbrains.kotlin:kotlin-gradle-plugin:$kotlinVersion"
}
}

android {
defaultConfig {
}
}
27 changes: 25 additions & 2 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,8 @@

import com.qualcomm.robotcore.hardware.PIDFCoefficients;

import org.firstinspires.ftc.teamcode.API.Sensor;

public class Constants {
/*
* These are motor constants that should be listed online for your motors.
Expand Down Expand Up @@ -50,8 +52,8 @@ public class Constants {
* acceleration values are required, and the jerk values are optional (setting a jerk of 0.0
* forces acceleration-limited profiling). All distance units are inches.
*/
public static double MAX_VEL = 40.0;
public static double MAX_ACCEL = 40.0;
public static double MAX_VEL = 30.0;
public static double MAX_ACCEL = 20.0;
public static double MAX_ANG_VEL = Math.toRadians(180.0);
public static double MAX_ANG_ACCEL = Math.toRadians(180.0);

Expand All @@ -67,4 +69,25 @@ public static double getMotorVelocityF(double ticksPerSecond) {
// see https://docs.google.com/document/d/1tyWrXDfMidwYyP_5H4mZyVgaEswhOC35gvdmP-V-5hA/edit#heading=h.61g9ixenznbx
return 32767 / ticksPerSecond;
}

// Color sensor configuration
public static final double PARK_RED_FUDGE = 250;
public static final double PARK_GREEN_FUDGE = 150;
public static final double PARK_BLUE_FUDGE = 150;

public static final double ARM_RED_FUDGE = 20*2;
public static final double ARM_GREEN_FUDGE = 15*2;
public static final double ARM_BLUE_FUDGE = 15*2;

public static final Sensor.Colors ARM_EXTEND_COLOR = Sensor.Colors.RED;
public static final Sensor.Colors ARM_CLOSE_COLOR = Sensor.Colors.BLUE;

// Motor power config
public static final double MOTOR_ARM_POWER = 0.6;

// Drive Adjustments
public static final double MOTOR_BL_POWERMOD = 0.94;
public static final double MOTOR_BR_POWERMOD = 0.90;
public static final double MOTOR_FL_POWERMOD = 1.00;
public static final double MOTOR_FR_POWERMOD = 0.92;
}
5 changes: 3 additions & 2 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Naming.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,19 +11,20 @@ public class Naming {
public static final String MOTOR_BL = "bl";
public static final String MOTOR_FR = "fr";
public static final String MOTOR_BR = "br";
public static final String MOTOR_WOBBLE_ARM = "wobbleArm";

public static final String SERVO_WOBBLE_GRABBER = "wobbleGrabber";
public static final String SERVO_WOBBLE_ARM = "wobbleArm";
public static final String SERVO_LAUNCHER = "launcher";

public static final String COLOR_SENSOR_PARK = "parkSensor";
public static final String COLOR_SENSOR_ARM = "armSensor";

public static final String GYRO_0 = "imu";
public static final String GYRO_1 = "imu 1";

public static final String WEBCAM_0 = "Webcam 1";

public static final String ENCODER_LATERAL = "lateralEncoder";
public static final String ENCODER_LATERAL = "fr";
public static final String ENCODER_LEFT = "leftEncoder";
public static final String ENCODER_RIGHT = "rightEncoder";

Expand Down
5 changes: 0 additions & 5 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/VuForiaKey.java.sample
View file

This file was deleted.

22 changes: 22 additions & 0 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/HW/SmartColorSensor.java
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,22 @@
package org.firstinspires.ftc.teamcode.API.HW;

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

import lombok.Getter;
import lombok.Setter;

public class SmartColorSensor {
@Getter NormalizedColorSensor sensor;

public SmartColorSensor(NormalizedColorSensor sensor) {
this.sensor = sensor;
}

// Color sensor config
@Getter @Setter double redFudge, greenFudge, blueFudge = 1;

public NormalizedRGBA getNormalizedColors() {
return sensor.getNormalizedColors();
}
}
1 change: 0 additions & 1 deletion TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/HW/SmartServo.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,6 @@
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.hardware.ServoImplEx;


import lombok.Getter;
import lombok.Setter;

Expand Down
40 changes: 26 additions & 14 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/InitRobot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,10 +10,13 @@
import com.qualcomm.robotcore.hardware.Servo;

import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
import org.firstinspires.ftc.teamcode.API.Config.Constants;
import org.firstinspires.ftc.teamcode.API.Config.Naming;
import org.firstinspires.ftc.teamcode.API.HW.Encoder;
import org.firstinspires.ftc.teamcode.API.HW.SmartColorSensor;
import org.firstinspires.ftc.teamcode.API.HW.SmartMotor;
import org.firstinspires.ftc.teamcode.API.HW.SmartServo;
import org.jetbrains.annotations.NotNull;

import java.util.HashMap;
import java.util.Objects;
Expand All @@ -28,7 +31,7 @@ public class InitRobot {
private static SmartMotor fr;

// TODO: JavaDoc
public static void init(LinearOpMode l) {
public static void init(@NotNull LinearOpMode l) {
/*
* ####### ######
* # ##### # ##### # # ###### # #### # #
Expand All @@ -49,9 +52,11 @@ public static void init(LinearOpMode l) {
SmartMotor flywheel = new SmartMotor(l.hardwareMap.get(DcMotorEx.class, Naming.MOTOR_FLYWHEEL));
SmartMotor intake = new SmartMotor(l.hardwareMap.get(DcMotorEx.class, Naming.ENCODER_LEFT));
SmartMotor intake2 = new SmartMotor(l.hardwareMap.get(DcMotorEx.class, Naming.MOTOR_INTAKE2));
SmartMotor wobbleArm = new SmartMotor(l.hardwareMap.get(DcMotorEx.class, Naming.MOTOR_WOBBLE_ARM));
flywheel.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
intake.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
intake2.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
wobbleArm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);

HashMap<String, SmartMotor> motors = new HashMap<>();
motors.put(Naming.MOTOR_BL, bl);
Expand All @@ -63,23 +68,18 @@ public static void init(LinearOpMode l) {
motors.put(Naming.MOTOR_FLYWHEEL, flywheel);
motors.put(Naming.MOTOR_INTAKE, intake);
motors.put(Naming.MOTOR_INTAKE2, intake2);
motors.put(Naming.MOTOR_WOBBLE_ARM, wobbleArm);

// Get servos
SmartServo wobbleArm = new SmartServo(l.hardwareMap.get(Servo.class, Naming.SERVO_WOBBLE_ARM));
SmartServo wobbleGrabber = new SmartServo(l.hardwareMap.get(Servo.class, Naming.SERVO_WOBBLE_GRABBER));
SmartServo launcher = new SmartServo(l.hardwareMap.get(Servo.class, Naming.SERVO_LAUNCHER));

// Add servos into the list
HashMap<String, SmartServo> servos = new HashMap<>();
servos.put(Naming.SERVO_WOBBLE_ARM, wobbleArm);
//servos.put(Naming.SERVO_WOBBLE_ARM, wobbleArm);
servos.put(Naming.SERVO_WOBBLE_GRABBER, wobbleGrabber);
servos.put(Naming.SERVO_LAUNCHER, launcher);

// Get CRServos

// Set direction of CRServos
//intake2.setDirection(CRServo.Direction.REVERSE);

// Add CRServos into the list
HashMap<String, CRServo> crServos = new HashMap<>();

Expand Down Expand Up @@ -109,15 +109,26 @@ public static void init(LinearOpMode l) {
fr.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
}

// Switch direction of servo
//rotate.setDirection(Servo.Direction.REVERSE);
bl.setPowerModifier(Constants.MOTOR_BL_POWERMOD);
br.setPowerModifier(Constants.MOTOR_BR_POWERMOD);
fl.setPowerModifier(Constants.MOTOR_FL_POWERMOD);
fr.setPowerModifier(Constants.MOTOR_FR_POWERMOD);

// Get color sensors
NormalizedColorSensor parkSensor = (NormalizedColorSensor)l.hardwareMap.get(ColorSensor.class, Naming.COLOR_SENSOR_PARK);
SmartColorSensor parkSensor = new SmartColorSensor((NormalizedColorSensor)l.hardwareMap.get(ColorSensor.class, Naming.COLOR_SENSOR_PARK));
parkSensor.setRedFudge(Constants.PARK_RED_FUDGE);
parkSensor.setGreenFudge(Constants.PARK_GREEN_FUDGE);
parkSensor.setBlueFudge(Constants.PARK_BLUE_FUDGE);

SmartColorSensor armSensor = new SmartColorSensor((NormalizedColorSensor)l.hardwareMap.get(ColorSensor.class, Naming.COLOR_SENSOR_ARM));
armSensor.setRedFudge(Constants.ARM_RED_FUDGE);
armSensor.setGreenFudge(Constants.ARM_GREEN_FUDGE);
armSensor.setBlueFudge(Constants.ARM_BLUE_FUDGE);

// Add color sensors into list
HashMap<String, NormalizedColorSensor> colorSensors = new HashMap<>();
HashMap<String, SmartColorSensor> colorSensors = new HashMap<>();
colorSensors.put(Naming.COLOR_SENSOR_PARK, parkSensor);
colorSensors.put(Naming.COLOR_SENSOR_ARM, armSensor);

// Get webcams
WebcamName webcam1 = l.hardwareMap.get(WebcamName.class, Naming.WEBCAM_0);
Expand Down Expand Up @@ -161,6 +172,8 @@ public static void init(LinearOpMode l) {
Robot.movement = movement;
Robot.sensor = sensor;
Robot.linearOpMode = l;

Robot.state = new StateMachine();

// Send power to servos so they don't move
for (String key : servos.keySet()) {
Expand All @@ -172,5 +185,4 @@ public static void init(LinearOpMode l) {
Robot.sensor.initGyro(key);
}
}
}

}
35 changes: 12 additions & 23 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Movement.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,21 +40,13 @@ public class Movement {
private final static double ELEVATOR_POWER = 1.00;

// Motor configuration
private final static double INTAKE2_THRESH = 0.8;
private final static double INTAKE2_THRESH = 1.0;

// Servo configuration
private final static int SERVO_SLEEP = 10; // Milliseconds
private final static double SERVO_STEP = 0.01; // on a scale of 0-1
private final static double GRABBER_OPEN = 0; // on a scale of 0-1
private final static double GRABBER_CLOSE = 0.65; // on a scale of 0-1
private final static double WOBBLE_IN = 0.75; // on a scale of 0-1
private final static double WOBBLE_OUT = 0.3; // on a scale of 0-1
private final static double WOBBLE_GRAB = 1;
private final static double WOBBLE_GRAB = 0.3;
private final static double WOBBLE_RELEASE = 0;
private final static double SWIVEL_OPEN = 0; // on a scale of 0-1
private final static double SWIVEL_CLOSE = 1; // on a scale of 0-1
private final static double FOUNDATION_OPEN = 0.3;
private final static double FOUNDATION_CLOSE = 0.95;
private final static double LAUNCHER_OPEN = 0.6;
private final static double LAUNCHER_CLOSE = 1;

Expand All @@ -64,7 +56,7 @@ public class Movement {

// Getters

public SmartMotor getMotor(String id) {
public static SmartMotor getMotor(String id) {
return motors.get(id);
}

Expand All @@ -90,6 +82,15 @@ public void move4x4(double flPower, double frPower, double blPower, double brPow
Objects.requireNonNull(motors.get(Naming.MOTOR_BR)).setPower(brPower);
}

/**
* Moves each of the four motors individually. Best for mecanum drives.
* @param power Power to all the wheels
*/
public void move1x4(double power) {
move4x4(power,power,power,power);
}


/**
* Move the base with four motors based on individual sides, not wheels
* @param lPower Power to the left side
Expand Down Expand Up @@ -172,18 +173,6 @@ public void moveIntake(double intakePower) {
Objects.requireNonNull(motors.get(Naming.MOTOR_INTAKE2)).setPower(intakePower*INTAKE2_THRESH);
}

/**
* Opens the grabber based on a boolean assignment
* @param command tue to open
*/
public void moveWobble(boolean command) {
if (command) {
Objects.requireNonNull(servos.get(Naming.SERVO_WOBBLE_ARM)).setPosition(WOBBLE_IN); // Opens the grabber
} else {
Objects.requireNonNull(servos.get(Naming.SERVO_WOBBLE_ARM)).setPosition(WOBBLE_OUT); // Closes the grabber
}
}

/**
* Opens the grabber based on a boolean assignment
* @param command tue to open
Expand Down
Loading

0 comments on commit aacf76c

Please sign in to comment.