Autonomus



in list Meeting Updates

Last Updated

Feb 17 at 10:27 am

Description

 Edit

package org.usfirst.frc.team4085.robot;

import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.command.Scheduler;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;

import org.usfirst.frc.team4085.robot.commands.ExampleCommand;
import org.usfirst.frc.team4085.robot.subsystems.ExampleSubsystem;

import edu.wpi.first.wpilibj.RobotDrive.MotorType;
import edu.wpi.first.wpilibj.CANTalon;
import edu.wpi.first.wpilibj.Joystick;
//import edu.wpi.first.wpilibj.buttons.;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Solenoid;
import edu.wpi.first.wpilibj.Talon;
import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.CameraServer;
import edu.wpi.first.wpilibj.AnalogInput;
import edu.wpi.first.wpilibj.smartdashboard.;
import edu.wpi.first.wpilibj.Timer;

/* The VM is configured to automatically run this class, and to call the
functions corresponding to each mode, as described in the IterativeRobot documentation. If you change the name of this class or the package after
creating this project, you must also update the manifest file in the resource directory.
*/
public class Robot extends IterativeRobot {

public static final ExampleSubsystem exampleSubsystem = new ExampleSubsystem(); public static OI oi; CANTalon dmotor1 = new CANTalon(1); CANTalon dmotor2 = new CANTalon(2); CANTalon dmotor3 = new CANTalon(3); CANTalon dmotor4 = new CANTalon(4); CANTalon wmotor1 = new CANTalon(5); CANTalon lmotor1 = new CANTalon(6); CANTalon wmotor2 = new CANTalon(7); Solenoid solenoid1 = new Solenoid(1); // Solenoid Left Solenoid solenoid2 = new Solenoid(2); // Solenoid Right Solenoid solenoid3 = new Solenoid(3); // Solenoid Lift Compressor comp = new Compressor(); RobotDrive rdrive = new RobotDrive(dmotor1,dmotor2,dmotor3,dmotor4); Joystick Xbox = new Joystick(0); Joystick lStick = new Joystick(1); Joystick rStick = new Joystick(2); Command autonomousCommand; AnalogInput sonar = new AnalogInput(0); SmartDashboard smart = new SmartDashboard(); /** * This function is run when the robot is first started up and should be * used for any initialization code. */ public void robotInit() { oi = new OI(); // instantiate the command used for the autonomous period autonomousCommand = new ExampleCommand(); rdrive.setInvertedMotor(MotorType.kFrontRight, true); rdrive.setInvertedMotor(MotorType.kRearRight, true); rdrive.setInvertedMotor(MotorType.kFrontLeft, false); rdrive.setInvertedMotor(MotorType.kRearLeft, false); //compressor start comp.start();

/* CameraServer.getInstance().setQuality(1/1000);

CameraServer.getInstance().startAutomaticCapture("cam0"); */ } public void disabledPeriodic() { Scheduler.getInstance().run(); } public void autonomousInit() { // schedule the autonomous command (example) if (autonomousCommand != null) autonomousCommand.start(); } /** * This function is called periodically during autonomous */ public void autonomousPeriodic() { //start of autonomous Scheduler.getInstance().run(); smart.putString("Encoder Position", "" + dmotor1.getEncPosition()); double P; // Initalizes Position Varible dmotor1.setPosition(0);// Resets Encoder to 0 P = dmotor1.getEncPosition();//Defines P //fork arm at 0 and closed //robot not moving //set Encoders to 0 //tanks are full //open forks solenoid3.set(true); //forks setting true opens the forks Timer.delay(1); //open wheel arms solenoid1.set(true); //left arm setting true closes wheel arm Timer.delay(1); solenoid2.set(true); //right arm setting true closes wheel arm Timer.delay(1); //start left wheel arm clockwise //start right wheel arm counter clockwise wmotor1.set(-1); wmotor2.set(1); //opposite direction dmotor1.setPosition(0);// Resets Encoder to 0 //move forward //rdrive.mecanumDrive_Polar(1, 0, 0);//moves robot forward rdrive.mecanumDrive_Polar(-1, 0, 0); double pineapple = 25; //max value do{ Timer.delay(1); //Delays Robot Action set break point P = dmotor1.getEncPosition();//Defines P if (P < (-1 * pineapple) || P > pineapple){ P = 50; //going the wrong direction STOP } } while(P <= pineapple); //STOP rdrive.mecanumDrive_Polar(0, 0, 0); //NEEDS TO BE TESTED //move all dmotors //close wheel arms solenoid1.set(false); //left arm setting true closes wheel arm Timer.delay(1); solenoid2.set(false); //right arm setting true closes wheel arm Timer.delay(1); //stop both wheel arm motors wmotor1.set(0); wmotor2.set(0); //close forks solenoid3.set(true); //forks setting true opens the forks Timer.delay(1); //rotate and run across the field //not determined yet dmotor1.setPosition(0);// Resets Encoder to 0 rdrive.mecanumDrive_Polar(1, 0, .5);// Turns Robot 90 degrees

pineapple = 25; //max value

do{ Timer.delay(1); //Delays Robot Action set break point P = dmotor1.getEncPosition();//Defines P if (P < (-1 * pineapple) || P > pineapple){ P = 50; //going the wrong direction STOP } } while(P <= pineapple); //STOP rdrive.mecanumDrive_Polar(0, 0, 0); //move across the field dmotor1.setPosition(0);// Resets Encoder to 0 rdrive.mecanumDrive_Polar(1, 0, .5);// Turns Robot 90 degrees

pineapple = 25; //max value

do{ Timer.delay(1); //Delays Robot Action set break point P = dmotor1.getEncPosition();//Defines P if (P < (-1 * pineapple) || P > pineapple){ P = 50; //going the wrong direction STOP } } while(P <= pineapple); //STOP rdrive.mecanumDrive_Polar(0, 0, 0); //open forks open wheel arms solenoid3.set(true); //forks setting true opens the forks Timer.delay(1); //solenoid 3 fork arms solenoid1.set(true); //left arm setting true closes wheel arm Timer.delay(1); solenoid2.set(true); //right arm setting true closes wheel arm Timer.delay(1); dmotor1.setPosition(0);// Resets Encoder to 0 //backwards 6 inches //move across the field dmotor1.setPosition(0);// Resets Encoder to 0 rdrive.mecanumDrive_Polar(1, 0, .5);// Turns Robot 90 degrees //not correct value

pineapple = 25; //max value

do{ Timer.delay(1); //Delays Robot Action set break point P = dmotor1.getEncPosition();//Defines P if (P < (-1 * pineapple) || P > pineapple){ P = 50; //going the wrong direction STOP } } while(P <= pineapple); //STOP rdrive.mecanumDrive_Polar(0, 0, 0); //turn off wheel motors wmotor1.set(0); wmotor2.set(0); //opposite direction //stop /* Scheduler.getInstance().run(); smart.putString("Encoder Position", "" + dmotor1.getEncPosition()); solenoid1.set(true); Timer.delay(.5); solenoid2.set(true); Timer.delay(.5); solenoid3.set(true); Timer.delay(.5); double P; dmotor1.setPosition(0); P = dmotor1.getEncPosition(); rdrive.mecanumDrive_Polar(-1, 0, 0); if(P <= -475){ Timer.delay(.5); rdrive.mecanumDrive_Polar(0, 0, 0); solenoid1.set(false); solenoid2.set(false); wmotor1.set(-1); wmotor2.set(-1); Timer.delay(.5); solenoid3.set(false); lmotor1.set(1); Timer.delay(60); //rdrive.mecanumDrive_Polar(1, -90, 0); } */ } public void OliviasAutonomous() { //start of autonomous Scheduler.getInstance().run(); smart.putString("Encoder Position", "" + dmotor1.getEncPosition()); //fork arm at 0 and closed //robot not moving //set Encoders to 0 //tanks are full //open forks solenoid3.set(true); //forks setting true opens the forks Timer.delay(.5); //open wheel arms solenoid1.set(true); //left arm setting true closes wheel arm Timer.delay(.5); solenoid2.set(true); //right arm setting true closes wheel arm Timer.delay(.5); //start left wheel arm clockwise //start right wheel arm counter clockwise wmotor1.set(-1); wmotor2.set(1); //opposite direction //open forks solenoid3.set(false); //forks setting true opens the forks Timer.delay(.5); //move all dmotors( forward -475)and stop //close wheel arms solenoid1.set(false); //left arm setting true closes wheel arm Timer.delay(.5); solenoid2.set(false); //right arm setting true closes wheel arm Timer.delay(.5); //turn off wheel motors //close forks solenoid3.set(true); //forks setting true opens the forks Timer.delay(.5); //open wheel arms //lift motor 18 inches //rotate the robot //rdrive.mecanumDrive_Polar(1, -90, 0); //move all dmotors( forward -475)and stop //wheel motors on //close forks with delay //delay to load tote //stop both wheel arm motors wmotor1.set(0); wmotor2.set(0); //run across the bump //rdrive.mecanumDrive_Polar(1, -90, 0); //move all dmotors( forward -475)and stop //open forks solenoid3.set(true); //forks setting true opens the forks Timer.delay(.5); //open wheel arms solenoid1.set(true); //left arm setting true closes wheel arm Timer.delay(.5); solenoid2.set(true); //right arm setting true closes wheel arm Timer.delay(.5); //backwards 6 inches //move all dmotors(back 25)and stop //turn off wheel motors wmotor1.set(0); wmotor2.set(0); //opposite direction //stop } public void teleopInit() { // This makes sure that the autonomous stops running when // teleop starts running. If you want the autonomous to // continue until interrupted by another command, remove // this line or comment it out. if (autonomousCommand != null) autonomousCommand.cancel(); dmotor1.setPosition(0); } /** * This function is called when the disabled button is hit. * You can use it to reset subsystems before shutting down. */ public void disabledInit(){ } /** * This function is called periodically during operator control */ public void teleopPeriodic() { /* if(rStick.getRawButton(5)) { dmotor1.setPosition(0); } if(lStick.getRawButton(4)) { int end = 475; int position1 = dmotor1.getEncPosition(); if(position1 >= end){ dmotor1.set(0); dmotor2.set(0); dmotor3.set(0); dmotor4.set(0); } if(position1 <= end){ dmotor1.set(1); dmotor2.set(1); dmotor3.set(-1); dmotor4.set(-1); end = dmotor1.getEncPosition(); } } smart.putString("Encoder Position", "" + dmotor1.getEncPosition()); */ Scheduler.getInstance().run(); //Compressor comp.start(); //Solenoid Control double X1, X2; X1 = lStick.getX(); X2 = rStick.getX(); //Left Arm if(X1 > .5){ solenoid1.set(false); } //if(X1 = 0) solenoid1.set(false); if(X1 < -.5){ solenoid1.set(true); } //Right Arm if(X2 > .5){ solenoid2.set(true); } //if(X2 = 0){ solenoid2.set(false); // if(X2 < -.5){ solenoid2.set(false); } if(rStick.getRawButton(1)){ solenoid3.set(true); } if(lStick.getRawButton(1)){ solenoid3.set(false); } //Lift Control if(lStick.getRawButton(3)){ lmotor1.set(1); } if(rStick.getRawButton(3)){ lmotor1.set(-1); } if(lStick.getRawButton(2)){ lmotor1.set(0); } //Wheel Motor Control double Y1, Y2; Y1 = lStick.getY(); Y2 = rStick.getY(); wmotor1.set(Y2*-1); wmotor2.set(Y1); //Drive Control double X,Y,Z; X = Xbox.getRawAxis(0); Y = Xbox.getRawAxis(1); Z = Xbox.getRawAxis(4); if(Math.abs(X) < .2){ X = 0; } if(Math.abs(Y) < .2){ Y = 0; } double mag = Math.sqrt((X*X)+(Y*Y)); //Mecanum Drive Statement rdrive.mecanumDrive_Polar(mag*-1, Xbox.getDirectionDegrees(), Z); } /** * This function is called periodically during test mode */ public void testPeriodic() { LiveWindow.run(); } /** * This function is called move or rotate the robot * P = position of encoder * magnitude is mecanum magnitude * direction is mecanum direction * rotation is mecanum rotation * maxvalue is movement stoping value stops the robot */ public void moverobot (double P, double magnitude, double direction, double rotation, double maxvalue) { //move forward //rdrive.mecanumDrive_Polar(1, 0, 0);//moves robot forward rdrive.mecanumDrive_Polar(magnitude, direction, rotation); //double pineapple = 25; //max value do{ Timer.delay(1); //Delays Robot Action set break point P = dmotor1.getEncPosition();//Defines P if (P < (-1 * maxvalue) || P > maxvalue){ P = 50; //going the wrong direction STOP } } while(P <= maxvalue); //STOP rdrive.mecanumDrive_Polar(0, 0, 0); //NEEDS TO BE TESTED }

}