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

319
FTC2024WeRobotControl.java
View file

@ -18,174 +18,179 @@ import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.util.ElapsedTime; import com.qualcomm.robotcore.util.ElapsedTime;
public class FTC2024WeRobotControl { public class FTC2024WeRobotControl {
/* /*
* The Parent class {@see FTC2024WeRobotControl constructor} * The Parent class {@see FTC2024WeRobotControl constructor}
*/ */
private Ftc2024_autonome_api Parent; private Ftc2024_autonome_api Parent;
/* /*
* the wheel width in metres * the wheel width in metres
*/ */
private final double wheel_width = 9.0e-2; // metres private final double wheel_width = 9.0e-2; // metres
/* /*
* the wheel perimeter in meter * the wheel perimeter in meter
*/ */
private final double wheel_perimeter = Math.PI * wheel_width; private final double wheel_perimeter = Math.PI * wheel_width;
/* /*
* the rpm at max power of the motors * the rpm at max power of the motors
*/ */
private final double tour_par_minutes = 300.0; private final double tour_par_minutes = 300.0;
/* /*
* the width size of the tiles on the ground in metres * the width size of the tiles on the ground in metres
*/ */
private final double ground_tiles_width = 61.0e-2; // metres private final double ground_tiles_width = 61.0e-2; // metres
// //
private final double defaultspeed = 0.6; private final double defaultspeed = 0.6;
private ElapsedTime timer; private ElapsedTime timer;
private YawPitchRollAngles robotOrientation; private YawPitchRollAngles robotOrientation;
/* /*
* construct the FTC2024WeRobotControl class, the WeRobot Robot Controller Class * construct the FTC2024WeRobotControl class, the WeRobot Robot Controller Class
* for the FTC2024 * for the FTC2024
* *
* @param Parent = the parent class, use the "this" keyword if you are * @param Parent = the parent class, use the "this" keyword if you are
* constructing the class directly in * constructing the class directly in
*/ */
public FTC2024WeRobotControl(Ftc2024_autonome_api Parent) { public FTC2024WeRobotControl(Ftc2024_autonome_api Parent) {
this.Parent = Parent; this.Parent = Parent;
this.timer = new ElapsedTime(); this.timer = new ElapsedTime();
} }
/* public void boxElv() {
* return a metre/sec speed Parent.lmelevator.setVelocity(600);
* Parent.rmelevator.setVelocity(600);
* @param motor_speed = (optional) double between 0 and 1; motor power; default Parent.lmelevator.setTargetPosition(90);
* to 1 Parent.rmelevator.setTargetPosition(90);
*/ Parent.rotation.setVelocity(600);
Parent.rotation.setTargetPosition(-50);
public void boxElv(){ Parent.lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION);
Parent.lmelevator.setVelocity(600); Parent.rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION);
Parent.rmelevator.setVelocity(600); Parent.rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION);
Parent.lmelevator.setTargetPosition(90); }
Parent.rmelevator.setTargetPosition(90); /*
Parent.rotation.setVelocity(600); * return a metre/sec speed
Parent.rotation.setTargetPosition(-50); *
Parent.lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); * @param motor_speed = (optional) double between 0 and 1; motor power; default
Parent.rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); * to 1
Parent.rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION); */
}
public double getSpeedFromMotorSpeed(double motor_speed) {
double speed_tour_par_minutes = this.tour_par_minutes * motor_speed;
double speed = (speed_tour_par_minutes / 60) * this.wheel_perimeter;
return speed;
}
/* public double getSpeedFromMotorSpeed(double motor_speed) {
* return the needed time for a distance double speed_tour_par_minutes = this.tour_par_minutes * motor_speed;
* double speed = (speed_tour_par_minutes / 60) * this.wheel_perimeter;
* @param dist = distance in metre return speed;
* }
* @param motor_speed = (optional) double between 0 and 1; motor power; default
* to 1
*/
public double time_for_dist(double dist, double motor_speed) {
double speed = getSpeedFromMotorSpeed(motor_speed);
return (dist / speed);
}
/* /*
* go forward * return the needed time for a distance
* *
* @param n_tiles = the number of tiles (a double because it can be 0.5 or 1.5 * @param dist = distance in metre
* etc.) *
* * @param motor_speed = (optional) double between 0 and 1; motor power; default
* @param motor_speed = (optional) double between 0 and 1; motor power; default * to 1
* to 1 */
*/ public double time_for_dist(double dist, double motor_speed) {
public void forward(double n_tiles, double motor_speed) { double speed = getSpeedFromMotorSpeed(motor_speed);
double total_time = time_for_dist(n_tiles * ground_tiles_width, motor_speed); return (dist / speed);
timer.reset(); }
while (Parent.opModeIsActive() && timer.seconds() < total_time) {
Parent.lm.setPower(motor_speed);
Parent.rm.setPower(motor_speed);
}
Parent.lm.setPower(0);
Parent.rm.setPower(0);
}
public void forward(double n_tiles){ /*
this.forward(n_tiles,this.defaultspeed); * go forward
} *
* @param n_tiles = the number of tiles (a double because it can be 0.5 or 1.5
* etc.)
*
* @param motor_speed = (optional) double between 0 and 1; motor power; default
* to 1
*/
public void forward(double n_tiles, double motor_speed) {
double total_time = time_for_dist(n_tiles * ground_tiles_width, motor_speed);
timer.reset();
while (Parent.opModeIsActive() && timer.seconds() < total_time) {
Parent.lm.setPower(motor_speed);
Parent.rm.setPower(motor_speed);
}
Parent.lm.setPower(0);
Parent.rm.setPower(0);
}
/* /*
* go backward * go forward
* * when only one argument passed:
* @param n_tiles = the number of tiles (a double because it can be 0.5 or 1.5 *
* etc.) * @param n_tiles number of tiles
* */
* @param motor_speed = (optional) double between 0 and 1; motor power; default public void forward(double n_tiles) {
* to 1 this.forward(n_tiles, this.defaultspeed);
*/ }
public void backward(double n_tiles, double motor_speed) {
forward(n_tiles, -motor_speed);
}
public void backward(double n_tiles){ /*
this.backward(n_tiles,this.defaultspeed); * go backward
} *
* @param n_tiles = the number of tiles (a double because it can be 0.5 or 1.5
* etc.)
*
* @param motor_speed = (optional) double between 0 and 1; motor power; default
* to 1
*/
public void backward(double n_tiles, double motor_speed) {
forward(n_tiles, -motor_speed);
}
/* public void backward(double n_tiles) {
* harvest this.backward(n_tiles, this.defaultspeed);
* }
* @param motor_speed = (optional) double between 0 and 1; motor power; default
* to 1
*/
public void harvest(double motor_speed) {
Parent.harvestmotor.setPower(motor_speed);
}
public void harvest() {
this.harvest(1.0);
}
/* /*
* rotate * harvest
* *
* @param angle = the angle to rotate (in degrees) * @param motor_speed = (optional) double between 0 and 1; motor power; default
* * to 1
* @param motor_speed = (optional) double between 0 and 1; motor power; default */
* to 1 public void harvest(double motor_speed) {
*/ Parent.harvestmotor.setPower(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)));
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 );
}
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);
}
Parent.lm.setPower(0);
Parent.rm.setPower(0);
}
public void rotate(double angle){
this.rotate(angle,this.defaultspeed);
}
public void test_forward_10_and_rotate_20deg(){ public void harvest() {
this.harvest(1.0);
}
} /*
} * rotate
*
* @param angle = the angle to rotate (in degrees)
*
* @param motor_speed = (optional) double between 0 and 1; motor power; default
* to 1
*/
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)));
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);
}
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);
}
Parent.lm.setPower(0);
Parent.rm.setPower(0);
}
public void rotate(double angle) {
this.rotate(angle, this.defaultspeed);
}
}