werobot/ftc2024-robotcode
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

h

Browse source
This commit is contained in:
GZod01 2024-04-06 10:41:59 +02:00
parent 868b380755
commit 028a7d5e04
1 changed files with 162 additions and 157 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

55
FTC2024WeRobotControl.java
View file

@ -58,14 +58,7 @@ public class FTC2024WeRobotControl {
this.timer = new ElapsedTime();
}
/*
* return a metre/sec speed
*
* @param motor_speed = (optional) double between 0 and 1; motor power; default
* to 1
*/
public void boxElv(){
public void boxElv() {
Parent.lmelevator.setVelocity(600);
Parent.rmelevator.setVelocity(600);
Parent.lmelevator.setTargetPosition(90);
@ -76,6 +69,12 @@ public class FTC2024WeRobotControl {
Parent.rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION);
Parent.rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION);
}
/*
* return a metre/sec speed
*
* @param motor_speed = (optional) double between 0 and 1; motor power; default
* to 1
*/
public double getSpeedFromMotorSpeed(double motor_speed) {
double speed_tour_par_minutes = this.tour_par_minutes * motor_speed;
@ -116,8 +115,14 @@ public class FTC2024WeRobotControl {
Parent.rm.setPower(0);
}
public void forward(double n_tiles){
this.forward(n_tiles,this.defaultspeed);
/*
* go forward
* when only one argument passed:
*
* @param n_tiles number of tiles
*/
public void forward(double n_tiles) {
this.forward(n_tiles, this.defaultspeed);
}
/*
@ -133,8 +138,8 @@ public class FTC2024WeRobotControl {
forward(n_tiles, -motor_speed);
}
public void backward(double n_tiles){
this.backward(n_tiles,this.defaultspeed);
public void backward(double n_tiles) {
this.backward(n_tiles, this.defaultspeed);
}
/*
@ -146,6 +151,7 @@ public class FTC2024WeRobotControl {
public void harvest(double motor_speed) {
Parent.harvestmotor.setPower(motor_speed);
}
public void harvest() {
this.harvest(1.0);
}
@ -158,34 +164,33 @@ public class FTC2024WeRobotControl {
* @param motor_speed = (optional) double between 0 and 1; motor power; default
* to 1
*/
public void rotate(double angle, double motor_speed){
public void rotate(double angle, double motor_speed) {
robotOrientation = Parent.imu.getRobotYawPitchRollAngles();
double start_yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
double anglerad = Math.toRadians(angle);
angle = Math.toDegrees(Math.atan2(Math.sin(anglerad),Math.cos(anglerad)));
angle = Math.toDegrees(Math.atan2(Math.sin(anglerad), Math.cos(anglerad)));
double left_multiplier = -((double) Math.signum(angle));
double right_multiplier = ((double) Math.signum(angle));
double m_power = motor_speed;
if(Math.abs(angle)==180){
angle = (double) ( ( (double) Math.signum(angle) ) * 179.9 );
if (Math.abs(angle) == 180) {
angle = (double) (((double) Math.signum(angle)) * 179.9);
}
while(Parent.opModeIsActive() && (Math.abs(robotOrientation.getYaw(AngleUnit.DEGREES) - start_yaw) < Math.abs(angle))){
while (Parent.opModeIsActive()
&& (Math.abs(robotOrientation.getYaw(AngleUnit.DEGREES) - start_yaw) < Math.abs(angle))) {
robotOrientation = Parent.imu.getRobotYawPitchRollAngles();
double yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
Parent.telemetry.addData("Yaw", yaw);
Parent.telemetry.update();
m_power = (Math.abs(robotOrientation.getYaw(AngleUnit.DEGREES)-start_yaw));//relative
Parent.lm.setPower(left_multiplier*m_power);
Parent.rm.setPower(right_multiplier*m_power);
m_power = (Math.abs(robotOrientation.getYaw(AngleUnit.DEGREES) - start_yaw));// relative
Parent.lm.setPower(left_multiplier * m_power);
Parent.rm.setPower(right_multiplier * m_power);
}
Parent.lm.setPower(0);
Parent.rm.setPower(0);
}
public void rotate(double angle){
this.rotate(angle,this.defaultspeed);
public void rotate(double angle) {
this.rotate(angle, this.defaultspeed);
}
public void test_forward_10_and_rotate_20deg(){
}
}