1+ package ca .mcgill .ecse211 .project ;
2+
3+ import static ca .mcgill .ecse211 .project .Resources .ACCELERATION ;
4+ import static ca .mcgill .ecse211 .project .Resources .BASE_WIDTH ;
5+ import static ca .mcgill .ecse211 .project .Resources .DIST_TO_DEG ;
6+ import static ca .mcgill .ecse211 .project .Resources .TILE_SIZE ;
7+ import static ca .mcgill .ecse211 .project .Resources .WHEEL_RAD ;
8+ import static ca .mcgill .ecse211 .project .Resources .leftMotor ;
9+ import static ca .mcgill .ecse211 .project .Resources .rightMotor ;
10+
11+ /**
12+ *
13+ * Driver has a few methods that assist in controlling the motors
14+ */
15+ public class Driver {
16+ /**
17+ * Moves the robot straight for the given distance.
18+ *
19+ * @param distance in feet (tile sizes), may be negative
20+ */
21+ public static void moveStraightFor (double distance ) {
22+ leftMotor .rotate (convertDistance (distance * TILE_SIZE ), true );
23+ rightMotor .rotate (convertDistance (distance * TILE_SIZE ), false );
24+ }
25+
26+ /**
27+ * Turns the robot by a specified angle. Note that this method is different from
28+ * {@code Navigation.turnTo()}. For example, if the robot is facing 90 degrees, calling
29+ * {@code turnBy(90)} will make the robot turn to 180 degrees, but calling
30+ * {@code Navigation.turnTo(90)} should do nothing (since the robot is already at 90 degrees).
31+ *
32+ * @param angle the angle by which to turn, in degrees
33+ */
34+ public static void turnBy (double angle ) {
35+ leftMotor .rotate (convertAngle (angle ), true );
36+ rightMotor .rotate (-convertAngle (angle ), false );
37+ }
38+
39+ /**
40+ * Turns the robot by a specified angle. Note that this method is different from
41+ * {@code Navigation.turnTo()}. For example, if the robot is facing 90 degrees, calling
42+ * {@code turnBy(90)} will make the robot turn to 180 degrees, but calling
43+ * {@code Navigation.turnTo(90)} should do nothing (since the robot is already at 90 degrees).
44+ *
45+ * @param angle the angle by which to turn, in degrees
46+ * @param returnImmediately whether the method should run in parallel
47+ */
48+ public static void turnBy (double angle , boolean returnImmediately ) {
49+ leftMotor .rotate (convertAngle (angle ), true );
50+ rightMotor .rotate (-convertAngle (angle ), returnImmediately );
51+ }
52+
53+ /**
54+ * Converts input distance to the total rotation of each wheel needed to cover that distance.
55+ *
56+ * @param distance the input distance
57+ * @return the wheel rotations necessary to cover the distance
58+ */
59+ public static int convertDistance (double distance ) {
60+ return (int ) ((180.0 * distance ) / (Math .PI * WHEEL_RAD ));
61+ }
62+ /**
63+ * This method takes in the total distance needed to travel and transforms it
64+ * into the number of wheel rotations needed
65+ *
66+ * @param distance
67+ * @return distance in wheel rotations.
68+ *
69+ */
70+ public static int convertDistance (double radius , double distance ) {
71+ return (int ) ((180.0 * distance ) / (Math .PI * radius ));
72+ }
73+ /**
74+ * converts radians into degrees.
75+ *
76+ * @param angle
77+ * @return wheel rotations needed
78+ *
79+ */
80+ @ SuppressWarnings ("unused" )
81+ public static int convertAngle (double radius , double width , double angle ) {
82+ return convertDistance (radius , Math .PI * width * angle / 360.0 );
83+ }
84+ /**
85+ * Converts input angle to the total rotation of each wheel needed to rotate the robot by that
86+ * angle.
87+ *
88+ * @param angle the input angle
89+ * @return the wheel rotations necessary to rotate the robot by the angle
90+ */
91+ public static int convertAngle (double angle ) {
92+ return convertDistance (Math .PI * BASE_WIDTH * angle / 360.0 );
93+ }
94+
95+ /**
96+ * Stops both motors.
97+ */
98+ public static void stopMotors () {
99+ leftMotor .stop ();
100+ rightMotor .stop ();
101+ }
102+
103+ /**
104+ * Sets the speed of both motors to the same values.
105+ *
106+ * @param speed the speed in degrees per second
107+ */
108+ public static void setSpeed (int speed ) {
109+ setSpeeds (speed , speed );
110+ }
111+
112+ /**
113+ * Rotates forward in a straight line for specified distance.
114+ *
115+ * @param distance the distance
116+ * @param speed the speed in deg/s
117+ */
118+ public static void moveDistFwd (int distance , int speed ) {
119+ // Motor commands block by default (i.e. they return only when motion is complete).
120+ // To get both motors synchronized, use the non-blocking method for leftMotor
121+ // so that it returns immediately. The blocking form is used for rightMotor so
122+ // that this method returns when motion is complete.
123+
124+ int rotationAngle = distance * DIST_TO_DEG / 100 ; // Convert linear distance to turns
125+ leftMotor .setSpeed (speed ); // Roll both motors forward
126+ rightMotor .setSpeed (speed );
127+ leftMotor .setAcceleration (ACCELERATION );
128+ rightMotor .setAcceleration (ACCELERATION );
129+ leftMotor .rotate (rotationAngle , true ); // Rotate left motor - DO NOT BLOCK
130+ rightMotor .rotate (rotationAngle ); // Rotate right motor
131+ }
132+
133+
134+ /**
135+ * Rotates forward in a straight line for specified distance.
136+ *
137+ * @param distance the distance
138+ * @param returnImmediately whether the method should run in parallel
139+ */
140+ public static void moveDistFwd (double distance , boolean returnImmediately ) {
141+ // Motor commands block by default (i.e. they return only when motion is complete).
142+ // To get both motors synchronized, use the non-blocking method for leftMotor
143+ // so that it returns immediately. The blocking form is used for rightMotor so
144+ // that this method returns when motion is complete.
145+
146+ int rotationAngle = (int ) (distance * DIST_TO_DEG / 100 ); // Convert linear distance to turns
147+ leftMotor .rotate (rotationAngle , true ); // Rotate left motor - DO NOT BLOCK
148+ rightMotor .rotate (rotationAngle , returnImmediately ); // Rotate right motor
149+ }
150+
151+ /**
152+ * Sets the speed of both motors to different values.
153+ *
154+ * @param leftSpeed the speed of the left motor in degrees per second
155+ * @param rightSpeed the speed of the right motor in degrees per second
156+ */
157+ public static void setSpeeds (int leftSpeed , int rightSpeed ) {
158+ leftMotor .setSpeed (leftSpeed );
159+ rightMotor .setSpeed (rightSpeed );
160+ }
161+
162+ /**
163+ * Sets the acceleration of both motors.
164+ *
165+ * @param acceleration the acceleration in degrees per second squared
166+ */
167+ public static void setAcceleration (int acceleration ) {
168+ leftMotor .setAcceleration (acceleration );
169+ rightMotor .setAcceleration (acceleration );
170+ }
171+
172+ }
0 commit comments