From 450d3c39973798bbb406878e67f0b7053fd78f25 Mon Sep 17 00:00:00 2001 From: Cole Deck Date: Thu, 11 Apr 2019 21:53:19 -0500 Subject: [PATCH] fix auton parking --- main.c | 116 ++++++++++++++++++++++++++++----------------------------- 1 file changed, 57 insertions(+), 59 deletions(-) diff --git a/main.c b/main.c index 015dc68..317ee71 100644 --- a/main.c +++ b/main.c @@ -1,4 +1,6 @@ #pragma config(I2C_Usage, I2C1, i2cSensors) +#pragma config(Sensor, dgtl1, , sensorSONAR_inch) +#pragma config(Sensor, dgtl3, , sensorSONAR_inch) #pragma config(Sensor, I2C_1, , sensorQuadEncoderOnI2CPort, , AutoAssign ) #pragma config(Sensor, I2C_2, , sensorQuadEncoderOnI2CPort, , AutoAssign ) #pragma config(Motor, port1, , tmotorVex393_HBridge, openLoop) @@ -23,27 +25,27 @@ // Set motor maximum speed, this allows for tweaking the speed of the robot with one change. #define MAX_AUTO_SPEED 100 -/* During the development of the autonomous portion of our code, we found that the robot - would have issues turning and driving at MAX_SPEED causing it to turn too much, and - not driving straight. After limiting the driving speed to 100, we found that the robot - was able to drive more consistently. +/* During the development of the autonomous portion of our code, we found that the robot + would have issues turning and driving at MAX_SPEED causing it to turn too much, and + not driving straight. After limiting the driving speed to 100, we found that the robot + was able to drive more consistently. */ #define STOP 0 // Defines the value for when a motor is stopped. - + #define DEADZONE 10 -/* Defines the deadzone of the VEX controller. With our controllers, a value of 10 allowed - for the motors to completely lose power when the joystick is let go. -*/ +/* Defines the deadzone of the VEX controller. With our controllers, a value of 10 allowed + for the motors to completely lose power when the joystick is let go. +*/ #define DRIVE_OFFSET 10 -/* Defines the offset used to correct curves while the robot is driving straight during the +/* Defines the offset used to correct curves while the robot is driving straight during the driveTiles(float numberOfTiles, bool direction) function. */ #define TILE 1206 -/* Definition for Rotation points per tile. +/* Definition for Rotation points per tile. Each tile is 23.4 inches wide. @@ -53,20 +55,20 @@ 2 * pi * r is circumference of the wheel - 13.0376 inches - There are 627.2 points in a revolution with the vex direct motor encoders - according to robotc - developers: “The 2-wire 393 motor measures 627.2 counts per revolution of the output shaft in its - default high-torque configuration and 392 counts per revolution of the output shaft in its + There are 627.2 points in a revolution with the vex direct motor encoders - according to robotc + developers: “The 2-wire 393 motor measures 627.2 counts per revolution of the output shaft in its + default high-torque configuration and 392 counts per revolution of the output shaft in its modified high-speed configuration.” - So if we do 1 revolution * distance / radius we get 627.2 * 23.4 / 13.0376 = 1206. - When the integrated motor encoder reports a movement of 1206, that means the robot has moved 1 tile. + So if we do 1 revolution * distance / radius we get 627.2 * 23.4 / 13.0376 = 1206. + When the integrated motor encoder reports a movement of 1206, that means the robot has moved 1 tile. */ // How much the wheels should spin in a 90 degree turn #define POINTS_PER_TURN 320 -/* Using trial and error, we found that our robot will make a 90 degree turn when the integrated motor +/* Using trial and error, we found that our robot will make a 90 degree turn when the integrated motor encoders report a distance of 320 while spinning in place. */ @@ -74,9 +76,9 @@ // definitions for driveTiles() #define FORWARD true #define REVERSE false -/* When the function driveTiles(float numberOfTiles, bool direction) is called, one of the explicit - parameters is a boolean for direction, where true is forward, and false is reverse. Using these - definitions in our code, it is clearer to us and readers as to what that parameter is for. +/* When the function driveTiles(float numberOfTiles, bool direction) is called, one of the explicit + parameters is a boolean for direction, where true is forward, and false is reverse. Using these + definitions in our code, it is clearer to us and readers as to what that parameter is for. */ void stopDriving() { @@ -86,7 +88,7 @@ void stopDriving() { motor[driveRF] = STOP; } // Explicit Parameters: None -// Output: All four driving motors will be stopped, stopping the robot’s movements immediately. +// Output: All four driving motors will be stopped, stopping the robot’s movements immediately. @@ -105,8 +107,8 @@ void shootBall() { motor[shoot] = STOP; } // Explicit Parameters: None -// Output: The 2 motors connected to the shoot port will turn on for 1.25 seconds, which is -// precisely the amount of time needed for the motors to pull back and release the launcher. +// Output: The 2 motors connected to the shoot port will turn on for 1.25 seconds, which is +// precisely the amount of time needed for the motors to pull back and release the launcher. void turntoRight(float turns) { @@ -119,9 +121,9 @@ void turntoRight(float turns) { } stopDriving(); } -// Explicit Parameters: A floating point number turns will control how much the robot will turn to the right. -// When turns is set to 1, the robot will turn exactly 90 degrees. Since it is a floating point number, we can -// specify decimal amounts to turns to allow for any angle of a turn. +// Explicit Parameters: A floating point number turns will control how much the robot will turn to the right. +// When turns is set to 1, the robot will turn exactly 90 degrees. Since it is a floating point number, we can +// specify decimal amounts to turns to allow for any angle of a turn. // Output: The robot will turn by (turns * 90) degrees to the right. void turntoLeft(float turns) { @@ -134,9 +136,9 @@ void turntoLeft(float turns) { } stopDriving(); } -// Explicit Parameters: A floating point number turns will control how much the robot will turn to the left. -// When turns is set to 1, the robot will turn exactly 90 degrees. Since it is a floating point number, we can -// specify decimal amounts to turns to allow for any angle of a turn. +// Explicit Parameters: A floating point number turns will control how much the robot will turn to the left. +// When turns is set to 1, the robot will turn exactly 90 degrees. Since it is a floating point number, we can +// specify decimal amounts to turns to allow for any angle of a turn. // Output: The robot will turn by (turns * 90) degrees to the left. @@ -152,8 +154,8 @@ void ballIn() { } // Explicit Parameters: None // Output: These three functions manage the ball lift and, conveniently, the same motors in reverse will -// flip a cap. flipOn() will spin the motors in the direction needed to flip caps, ballIn() will spin the -// motors in the direction needed to collect and pick up balls, and ballOff() will turn off the motors. +// flip a cap. flipOn() will spin the motors in the direction needed to flip caps, ballIn() will spin the +// motors in the direction needed to collect and pick up balls, and ballOff() will turn off the motors. void joystickDrive() { @@ -175,10 +177,10 @@ void joystickDrive() { } } // Explicit Parameters: None -// Output: The robot will drive based on the values read from the 2 joysticks on the controller. However, +// Output: The robot will drive based on the values read from the 2 joysticks on the controller. However, // if the joystick’s value is inside the DEADZONE (10) then the robot will not move. This prevents wasted // battery and motor overheating when the robot is not supposed to be moving. This is necessary because when -// the joysticks are let go they don’t read a value of exactly zero, it’s usually off by a few. +// the joysticks are let go they don’t read a value of exactly zero, it’s usually off by a few. void buttonChecks() { @@ -196,7 +198,7 @@ void buttonChecks() { } // No need for reverse on the ball launcher! } // Explicit Parameters: None -// Output: When the corresponding buttons are pressed, various features will be activated, such as the cap +// Output: When the corresponding buttons are pressed, various features will be activated, such as the cap // flipper or the ball launcher. When the buttons are released, the action is stopped. @@ -251,14 +253,14 @@ void driveTiles(float numberOfTiles, bool direction) { } stopDriving(); } -// Explicit Parameters: A floating point number numberOfTurns that represents the number of tiles that the -// robot is to drive. Since it is a floating point number, we can move by half or any fraction movement. -// There is also the boolean value direction that controls which way the robot is to move. true is for forward, -// and false is for reverse. -// Output: The robot will drive the specified amount of tiles, in the specified direction. If the robot is not driving +// Explicit Parameters: A floating point number numberOfTurns that represents the number of tiles that the +// robot is to drive. Since it is a floating point number, we can move by half or any fraction movement. +// There is also the boolean value direction that controls which way the robot is to move. true is for forward, +// and false is for reverse. +// Output: The robot will drive the specified amount of tiles, in the specified direction. If the robot is not driving // straight, the speeds of the left and right motors can be offset from each other to cancel out any slight drifts -// to the left or right. We subtract 200 from the distance no matter what here, because the robot moves -// that much after it is told to stop. +// to the left or right. We subtract 200 from the distance no matter what here, because the robot moves +// that much after it is told to stop. task autonomous() { turntoRight(0.03); @@ -276,23 +278,20 @@ task autonomous() { driveTiles(1, REVERSE); turntoRight(1); driveTiles(0.6, REVERSE); - driveTiles(2.1, FORWARD); // Flip the other cap without turning on the spinner + driveTiles(2, FORWARD); // Flip the other cap without turning on the spinner flipOn(); // So we can pick up the ball that's under it! driveTiles(0.5, FORWARD); ballIn(); - driveTiles(0.1, REVERSE); + driveTiles(0.3, REVERSE); wait(3); - driveTiles(0.1, REVERSE); - turntoLeft(1); - driveTiles(0.2, REVERSE); + driveTiles(0.5, REVERSE); + turntoLeft(0.75); + //driveTiles(0.2, REVERSE); wait(3); ballOff(); shootBall(); - driveTiles(0.05, REVERSE); - driveTiles(0.33, FORWARD); - wait(2); - turntoRight(1); - driveTiles(2.2, REVERSE); + turntoRight(0.75); + driveTiles(1.9, REVERSE); turntoLeft(1); driveTiles(1, REVERSE); turntoRight(1); @@ -302,15 +301,15 @@ task autonomous() { /* This is the autonomous task. Here’s the path of the robot, described in words instead of code: -1. Start at the red tile closest to the flags. +1. Start at the red tile closest to the flags. 2. Turn a tiny bit to the right to aim, and shoot the top flag with our preload. -3. Re-align ourselves and drive to hit the bottom flag. +3. Re-align ourselves and drive to hit the bottom flag. 4. Back up 1 tile, turn right, and back into the wall to align the robot. -5. Drive forward with the flipper turned on, and flip the cap from blue to red. +5. Drive forward with the flipper turned on, and flip the cap from blue to red. 6. Back up, turn left, reverse 1 tile, turn right, back into the wall again to align ourselves. @@ -326,17 +325,16 @@ This is the autonomous task. Here’s the path of the robot, described in words 12. Turn to face the parking platforms, and reverse into the wall to align ourselves again. -13. Climb to the middle parking platform and stop. +13. Climb to the middle parking platform and stop. */ - + task usercontrol() { while (true) { joystickDrive(); buttonChecks(); } } -// When the driver is in control, this task runs. For the entire duration of the driver control period, we need -// to be able to control the robot, so we put everything in a while loop. The task calls 2 previously mentioned -// functions, joystickDrive() and buttonChecks(). - +// When the driver is in control, this task runs. For the entire duration of the driver control period, we need +// to be able to control the robot, so we put everything in a while loop. The task calls 2 previously mentioned +// functions, joystickDrive() and buttonChecks(). \ No newline at end of file