From bfd70698537056ee48c13e5592fba2e9321c5233 Mon Sep 17 00:00:00 2001
From: GZod01 <gzod01@gzod01.fr>
Date: Sun, 10 Mar 2024 17:21:15 +0100
Subject: [PATCH] add test motor speed

---
 FTC2024WeRobotControl.java | 17 +++++++++++++++++
 1 file changed, 17 insertions(+)

diff --git a/FTC2024WeRobotControl.java b/FTC2024WeRobotControl.java
index fe78d18..f52809b 100644
--- a/FTC2024WeRobotControl.java
+++ b/FTC2024WeRobotControl.java
@@ -63,11 +63,20 @@ public class FTC2024WeRobotControl {
      * to 1
      */
     public double getSpeedFromMotorSpeed(double motor_speed) {
+        testMotorSpeed(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;
     }
 
+    /*
+     * test if motorSpeed is >= 0
+     */
+    public void testMotorSpeed(double motor_speed){
+        if(!((double)motor_speed>=0.0)){
+            throw new Exception("Motor Speed MUST be >= 0");
+        }
+    }
     /*
      * return the needed time for a distance
      * 
@@ -77,6 +86,7 @@ public class FTC2024WeRobotControl {
      * to 1
      */
     public double time_for_dist(double dist, double motor_speed) {
+        testMotorSpeed(motor_speed);
         double speed = getSpeedFromMotorSpeed(motor_speed);
         return (dist / speed);
     }
@@ -91,6 +101,7 @@ public class FTC2024WeRobotControl {
      * to 1
      */
     public void forward(double n_tiles, double motor_speed) {
+        testMotorSpeed(motor_speed);
         double total_time = time_for_dist(n_tiles * ground_tiles_width, motor_speed);
         timer.reset();
         while (Parent.opModeIsActive() && timer.seconds() < total_time) {
@@ -115,6 +126,7 @@ public class FTC2024WeRobotControl {
      * to 1
      */
     public void backward(double n_tiles, double motor_speed) {
+        testMotorSpeed(motor_speed);
         forward(n_tiles, -motor_speed);
     }
     
@@ -129,8 +141,12 @@ public class FTC2024WeRobotControl {
      * to 1
      */
     public void harvest(double motor_speed) {
+        testMotorSpeed(motor_speed);
         Parent.harvestmotor.setPower(motor_speed);
     }
+    public void harvest(){
+        this.harvest(1);
+    }
 
     /*
      * harvest
@@ -141,6 +157,7 @@ public class FTC2024WeRobotControl {
      * to 1
      */
     public void rotate(double angle, double motor_speed){
+        testMotorSpeed(motor_speed);
         robotOrientation = Parent.imu.getRobotYawPitchRollAngles();
         double start_yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
         angle = 200.0;