Skip to content
This repository has been archived by the owner on Nov 10, 2023. It is now read-only.

Commit

Permalink
test auto drive to position
Browse files Browse the repository at this point in the history
has known bugs => dont use
  • Loading branch information
RobotIGS-Building committed Sep 26, 2023
1 parent acdf7f1 commit eba59fd
Show file tree
Hide file tree
Showing 5 changed files with 72 additions and 6 deletions.
Original file line number Diff line number Diff line change
@@ -0,0 +1,41 @@
package org.firstinspires.ftc.teamcode.OpModes.Autonomous.Examples;

import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;

import org.firstinspires.ftc.teamcode.OpModes.Autonomous.BaseAutonomous;
import org.firstinspires.ftc.teamcode.Tools.Chassis.Chassi;
import org.firstinspires.ftc.teamcode.Tools.Chassis.MecanumChassi;
import org.firstinspires.ftc.teamcode.Tools.DTypes.Position2D;
import org.firstinspires.ftc.teamcode.Tools.FieldNavigation;
import org.firstinspires.ftc.teamcode.Tools.Robot;

@Autonomous(name="Test Coordinates Driving", group="Examples")
//@Disabled
public class TestCoordinateDriving extends BaseAutonomous {
private Robot robot;
private Chassi chassi;
private FieldNavigation navi;

@Override
public void initialize() {
navi = new FieldNavigation();
chassi = new MecanumChassi();
chassi.setRotationAxis(1);
chassi.populateMotorArray(hardwareMap);

robot = new Robot(navi, chassi);
}

@Override
public void run() {
robot.drive(new Position2D(100.0, 0.0));

while (opModeIsActive()) {
robot.step();
telemetry.addLine(chassi.debug());
telemetry.addLine(navi.debug());
telemetry.update();
}
}
}
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
package org.firstinspires.ftc.teamcode.OpModes.TeleOp.Examples;

import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;

import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
Expand All @@ -13,6 +14,7 @@
import java.util.Date;

@TeleOp(name="FullControl", group="Examples")
//@Disabled
public class FullControl extends BaseTeleOp {
private Robot robot;
private FieldNavigation navi;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -115,12 +115,14 @@ public Position2D getNormalization() {

// one dim
else if (this.x == 0.0)
return new Position2D(0.0,1.0);
return new Position2D(0.0,this.y>0?1.0:-1.0);
else if (this.y == 0.0)
return new Position2D(1.0,0.0);
return new Position2D(this.x>0?1.0:-1.0,0.0);

// normalize
double alpha = Math.atan(this.y / this.x);
return new Position2D(Math.cos(alpha),Math.sin(alpha));
boolean x = this.x > 0;
boolean y = this.y > 0;
return new Position2D((x?1:.1)*Math.cos(alpha),(y?1:-1)*Math.sin(alpha));
}
}
Original file line number Diff line number Diff line change
@@ -1,5 +1,7 @@
package org.firstinspires.ftc.teamcode.Tools;

import android.annotation.SuppressLint;

import org.firstinspires.ftc.teamcode.Tools.DTypes.Velocity;
import org.firstinspires.ftc.teamcode.Tools.DTypes.Position2D;

Expand All @@ -13,6 +15,8 @@ public class FieldNavigation {
private double driving_accuracy;
private Velocity velocity;

public Position2D distance;

/**
* create new FieldNavigation object with given position and rotation
* @param position position of the robot in CM
Expand Down Expand Up @@ -129,14 +133,25 @@ public void stop(){


public String debug() {
return String.format("--- FieldNavigation Debug ---\nposition :: x=%+3.1f y=%+3.1f\n",position.getX(),position.getY());
String ret = "--- FieldNavigation Debug ---\n";
ret += String.format("driving :: %s\ntarget position :: x=%+3.1f y=%+3.1f\n",
(this.driving?"True":"False"), target_position.getX(), target_position.getY());
ret += String.format("position :: x=%+3.1f y=%+3.1f\n",
position.getX(), position.getY());
ret += String.format("velocity :: x=%+1.2f y=%+1.2f wz=%+1.2f\n",
velocity.getVX(), velocity.getVY(), velocity.getWZ());

ret += String.format("DIST :: %f %f\n", distance.getX(), distance.getY());

return ret;
}

/**
* refresh
*/
public void step() {
Position2D distance;
double quad = 0;

distance = target_position.copy();
distance.subract(position);
Expand All @@ -146,8 +161,12 @@ public void step() {
stop();

distance = distance.getNormalization();
velocity.setVX(distance.getX());
velocity.setVY(distance.getY());

distance.rotate(-this.rotation);
this.distance = distance;
velocity.set(distance.getX()*0.3, distance.getY()*0.3, 0.0);
} else {
velocity.set(0.0,0.0,0.0); // TODO remove this
}
}
}
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,8 @@
import org.firstinspires.ftc.teamcode.Tools.Chassis.Chassi;
import org.firstinspires.ftc.teamcode.Tools.DTypes.Position2D;

// TODO isb for driving etc

public class Robot {
public FieldNavigation navi;
public Chassi chassi;
Expand Down

0 comments on commit eba59fd

Please sign in to comment.