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

hello

Browse source
This commit is contained in:
GZod01 2024-03-08 17:39:35 +01:00
parent 13a84719ce
commit 38080d8c41
2 changed files with 81 additions and 49 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

127
FTC2024WeRobotControl.java
View file

@ -39,81 +39,112 @@ public class FTC2024WeRobotControl {
*/ */
private final double ground_tiles_width = 61.0e-2; // metres private final double ground_tiles_width = 61.0e-2; // metres
/* /*
* construct the FTC2024WeRobotControl class, the WeRobot Robot Controller Class for the FTC2024 * construct the FTC2024WeRobotControl class, the WeRobot Robot Controller Class
* @param Parent = the parent class, use the "this" keyword if you are constructing the class directly in * for the FTC2024
*
* @param Parent = the parent class, use the "this" keyword if you are
* constructing the class directly in
*/ */
public FTC2024WeRobotControl(YawPitchhRollAngle Parent){
this.Parent = Parent; public FTC2024WeRobotControl(YawPitchhRollAngle Parent) {
this.Parent = Parent;
} }
/* /*
* return a metre/sec speed * return a metre/sec speed
* @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 double getSpeedFromMotorSpeed(double motor_speed = 1.0){ public double getSpeedFromMotorSpeed(double motor_speed) {
double speed_tour_par_minutes = this.tour_par_minutes*motor_speed; double speed_tour_par_minutes = this.tour_par_minutes * motor_speed;
double speed = (speed_tour_par_minutes/60)*this.wheel_perimeter; double speed = (speed_tour_par_minutes / 60) * this.wheel_perimeter;
return speed; return speed;
} }
/* /*
* return the needed time for a distance * return the needed time for a distance
*
* @param dist = distance in metre * @param dist = distance in metre
* @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 double time_for_dist(double dist, double motor_speed=1.0){ public double time_for_dist(double dist, double motor_speed) {
double speed = getSpeedFromMotorSpeed(motor_speed); double speed = getSpeedFromMotorSpeed(motor_speed);
return (dist/speed); return (dist / speed);
} }
/* /*
* go forward * 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 * @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 = 1.0){ public void forward(double n_tiles, double motor_speed) {
double total_time = time_for_dist(n_tiles*ground_tiles_width, motor_speed); double total_time = time_for_dist(n_tiles * ground_tiles_width, motor_speed);
double start_time = Parent.runtime.seconds(); double start_time = Parent.runtime.seconds();
while( Parent.opModeIsActive() && ((Parent.runtime.seconds()-start_time)<total_time)){ while (Parent.opModeIsActive() && ((Parent.runtime.seconds() - start_time) < total_time)) {
Parent.lm.setPower(motor_speed); Parent.lm.setPower(motor_speed);
Parent.rm.setPower(motor_speed); Parent.rm.setPower(motor_speed);
} }
Parent.lm.setPower(0); Parent.lm.setPower(0);
Parent.rm.setPower(0); Parent.rm.setPower(0);
} }
/* /*
* go backward * 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 * @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= 1.0){ public void backward(double n_tiles, double motor_speed) {
forward(n_tiles, -motor_speed); forward(n_tiles, -motor_speed);
} }
/* /*
* harvest * harvest
* @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=0.0){ public void harvest(double motor_speed) {
Parent.harvestmotor.setPower(motor_speed); Parent.harvestmotor.setPower(motor_speed);
} }
/* /*
* harvest * harvest
*
* @param angle = the angle to rotate (in degrees) * @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 rotate(double angle, double motor_speed=1.0){ public void rotate(double angle, double motor_speed){
Parent.robotOrientation = Parent.imu.getRobotYawPitchRollAngles(); Parent.robotOrientation = Parent.imu.getRobotYawPitchRollAngles();
double start_yaw = Parent.robotOrientation.getYaw(AngleUnit.DEGREES); double start_yaw = Parent.robotOrientation.getYaw(AngleUnit.DEGREES);
angle = 200.0; angle = 200.0;
double anglerad = Math.toRadians(angle); 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 left_multiplier = -((double) Math.signum(angle));
double right_multiplier = ((double) Math.signum(angle)); double right_multiplier = ((double) Math.signum(angle));
double m_power = motor_speed; double m_power = motor_speed;
while(Parent.opModeIsActive() && (Math.abs(Parent.robotOrientation.getYaw(AngleUnit.DEGREES) - start_yaw) < Math.abs(angle)){ while(Parent.opModeIsActive() && (Math.abs(Parent.robotOrientation.getYaw(AngleUnit.DEGREES) - start_yaw) < Math.abs(angle))){
Parent.robotOrientation = Parent.imu.getRobotYawPitchRollAngles(); Parent.robotOrientation = Parent.imu.getRobotYawPitchRollAngles();
m_power = (Math.abs(Parent.robotOrientation.getYaw(AngleUnit.DEGREES)-start_yaw));//relative m_power = (Math.abs(Parent.robotOrientation.getYaw(AngleUnit.DEGREES)-start_yaw));//relative
Parent.lm.setPower(left_multiplier*m_power); Parent.lm.setPower(left_multiplier*m_power);
Parent.rm.setPower(right_multiplier*m_power); Parent.rm.setPower(right_multiplier*m_power);
} }
Parent.lm.setPower(0); Parent.lm.setPower(0);
Parent.rm.setPower(0); Parent.rm.setPower(0);
}
public void test_forward_10_and_rotate_20deg(){
} }
} }

3
ftc2024_autonome_api.java
View file

@ -80,7 +80,8 @@ public class ftc2024_autonome_api extends LinearOpMode {
robot.harvest(0); robot.harvest(0);
break; break;
case ("r4d"): case ("r4d"):
break; break;
case ("r2d"): case ("r2d"):