From 40d8d6cb6e73aea91030eeb578604be19e648320 Mon Sep 17 00:00:00 2001 From: Cole Deck Date: Thu, 11 Apr 2019 22:02:50 -0500 Subject: [PATCH] added ultrasonic code --- main.c | 240 +-------------------------------------------------------- 1 file changed, 1 insertion(+), 239 deletions(-) diff --git a/main.c b/main.c index 317ee71..423f3aa 100644 --- a/main.c +++ b/main.c @@ -1,242 +1,4 @@ -#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) -#pragma config(Motor, port2, shoot, tmotorVex393_MC29, openLoop, reversed) -#pragma config(Motor, port3, driveLB, tmotorVex393_MC29, openLoop, reversed, encoderPort, I2C_2) -#pragma config(Motor, port4, driveLF, tmotorVex393_MC29, openLoop, reversed) -#pragma config(Motor, port5, driveRB, tmotorVex393_MC29, openLoop, reversed, encoderPort, I2C_1) -#pragma config(Motor, port6, driveRF, tmotorVex393_MC29, openLoop) -#pragma config(Motor, port7, bintake, tmotorVex393_MC29, openLoop, reversed) -#pragma config(Motor, port8, , tmotorVex393_MC29, openLoop) -#pragma config(Motor, port9, , tmotorVex393_MC29, openLoop) -#pragma config(Motor, port10, , tmotorVex393_HBridge, openLoop) -//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*// -#pragma platform(VEX2) -#pragma competitionControl(Competition) -#include "Vex_Competition_Includes.c" - -// Definitions here! - -#define MAX_SPEED 127 -// 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. -*/ - -#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. -*/ - -#define DRIVE_OFFSET 10 -/* 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. - - Each tile is 23.4 inches wide. - - - The exact radius of 4" omni wheels, using dial calipers: 2.075 inches - - 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 - 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. -*/ - - -// 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 - encoders report a distance of 320 while spinning in place. -*/ - - -// 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. -*/ - -void stopDriving() { - motor[driveLB] = STOP; - motor[driveLF] = STOP; - motor[driveRB] = STOP; - motor[driveRF] = STOP; -} -// Explicit Parameters: None -// Output: All four driving motors will be stopped, stopping the robot’s movements immediately. - - - -void clearEnc() { // Reset driving motor encoder values to 0 - nMotorEncoder[driveRB] = 0; - nMotorEncoder[driveLB] = 0; -} -// Explicit Parameters: None -// Output: Resets the driving encoders to 0, for use in other autonomous functions. - - - -void shootBall() { - motor[shoot] = MAX_SPEED; - wait(1.25); // Shooting takes 1.25 seconds - 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. - - -void turntoRight(float turns) { - clearEnc(); - while(turns * POINTS_PER_TURN > nMotorEncoder[driveLB]){ - motor[driveLB] = MAX_AUTO_SPEED; - motor[driveLF] = MAX_AUTO_SPEED; - motor[driveRB] = -MAX_AUTO_SPEED; - motor[driveRF] = -MAX_AUTO_SPEED; - } - 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. -// Output: The robot will turn by (turns * 90) degrees to the right. - -void turntoLeft(float turns) { - clearEnc(); - while(turns * POINTS_PER_TURN > nMotorEncoder[driveRB]){ - motor[driveLB] = -MAX_AUTO_SPEED; - motor[driveLF] = -MAX_AUTO_SPEED; - motor[driveRB] = MAX_AUTO_SPEED; - motor[driveRF] = MAX_AUTO_SPEED; - } - 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. -// Output: The robot will turn by (turns * 90) degrees to the left. - - - -void flipOn() { - motor[bintake] = -MAX_SPEED; -} -void ballOff() { - motor[bintake] = STOP; -} -void ballIn() { - motor[bintake] = MAX_SPEED; -} -// 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. - - -void joystickDrive() { - if(abs(vexRT[Ch3]) > DEADZONE) { - motor[driveLB] = vexRT[Ch3]; - motor[driveLF] = vexRT[Ch3]; - } - else { - motor[driveLB] = STOP; - motor[driveLF] = STOP; - } - if(abs(vexRT[Ch2]) > DEADZONE) { - motor[driveRB] = vexRT[Ch2]; - motor[driveRF] = vexRT[Ch2]; - } - else { - motor[driveRB] = STOP; - motor[driveRF] = STOP; - } -} -// Explicit Parameters: None -// 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. - - -void buttonChecks() { - if (vexRT[Btn5U] == 1) { - ballIn(); - } - else if (vexRT[Btn5D] == 1) { - flipOn(); - } - else { - ballOff(); - } - if (vexRT[Btn8D] == 1) { - shootBall(); - } // 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 -// flipper or the ball launcher. When the buttons are released, the action is stopped. - - -void pre_auton() { - /* Set bStopTasksBetweenModes to false if you want to keep user created tasks - running between Autonomous and Driver controlled modes. You will need to - manage all user created tasks if set to false. */ - bStopTasksBetweenModes = true; -} -// Auto-generated ROBOTC autonomous function - -void driveTiles(float numberOfTiles, bool direction) { - // when direction is true, move forward, otherwise go in reverse - clearEnc(); - while(direction == FORWARD && numberOfTiles * TILE - 200 > nMotorEncoder[driveRB]) { - if(abs(nMotorEncoder[driveRB]) - DRIVE_OFFSET > nMotorEncoder[driveLB]) { - motor[driveLB] = MAX_AUTO_SPEED; - motor[driveLF] = MAX_AUTO_SPEED; - motor[driveRB] = MAX_AUTO_SPEED - DRIVE_OFFSET; - motor[driveRF] = MAX_AUTO_SPEED - DRIVE_OFFSET; - } - if(abs(nMotorEncoder[driveLB]) - DRIVE_OFFSET > nMotorEncoder[driveRB]) { - motor[driveLB] = MAX_AUTO_SPEED - DRIVE_OFFSET; - motor[driveLF] = MAX_AUTO_SPEED - DRIVE_OFFSET; - motor[driveRB] = MAX_AUTO_SPEED; - motor[driveRF] = MAX_AUTO_SPEED; - } else { - motor[driveLB] = MAX_AUTO_SPEED; - motor[driveLF] = MAX_AUTO_SPEED; - motor[driveRB] = MAX_AUTO_SPEED; - motor[driveRF] = MAX_AUTO_SPEED; - } - } - while(direction == REVERSE && numberOfTiles * TILE - 200 > -nMotorEncoder[driveRB]) { - if(abs(nMotorEncoder[driveRB]) - DRIVE_OFFSET > nMotorEncoder[driveLB]) { - motor[driveLB] = -MAX_AUTO_SPEED; - motor[driveLF] = -MAX_AUTO_SPEED; - motor[driveRB] = -MAX_AUTO_SPEED + DRIVE_OFFSET; motor[driveRF] = -MAX_AUTO_SPEED + DRIVE_OFFSET; } if(abs(nMotorEncoder[driveLB]) - DRIVE_OFFSET > nMotorEncoder[driveLB]) { @@ -289,7 +51,7 @@ task autonomous() { //driveTiles(0.2, REVERSE); wait(3); ballOff(); - shootBall(); + shootBall(); turntoRight(0.75); driveTiles(1.9, REVERSE); turntoLeft(1);