Go to Post Blasphemy! Everyone knows Mountain Dew is roboteer fuel. - Racer26 [more]
Home
Go Back   Chief Delphi > Technical > Programming > Java
CD-Media   CD-Spy  
portal register members calendar search Today's Posts Mark Forums Read FAQ rules

 
 
 
Thread Tools Rate Thread Display Modes
Prev Previous Post   Next Post Next
  #1   Spotlight this post!  
Unread 19-02-2016, 23:14
Cubby Cubby is offline
Registered User
FRC #4598
 
Join Date: Feb 2016
Location: Salt Lake City
Posts: 3
Cubby is an unknown quantity at this point
PID Problems

Hey guys! I'm a senior from Salt Lake just getting into FIRST. I am new to java and I'm having some trouble specifically with my PID loop. As I understand it the PIDController function should take input from my encoder and send output to my motor (arm). The code builds with no errors but I am seeing no motion in the arm motor, I am using an encoder value of 250 as my setpoint just for testing purposes. If anyone can lend me some advice as to how I'm screwing this up that would be greatly appreciated!
Code:
package org.usfirst.frc.team4598.robot;

import edu.wpi.first.wpilibj.CameraServer;
import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.CounterBase.EncodingType;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.PIDOutput;
import edu.wpi.first.wpilibj.Solenoid;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.DigitalInput;


/**
 * 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 // w/ joysticks	
{												
	CameraServer server;	
	Joystick controller = new Joystick(0);	
	Solenoid Punch = new Solenoid(1,3);
	Victor leftDrive0 = new Victor(0);	
	Victor leftDrive1 = new Victor(1);		
	Victor rightDrive0 = new Victor(2);			
	Victor rightDrive1 = new Victor(3);			
	Victor launchWheel0 = new Victor(4);		
	Victor launchWheel1 = new Victor(5);		
	Victor arm = new Victor(6);	//Shooter Arm
	Victor pnuarm = new Victor(7);				
												
	Encoder encoder = new Encoder(1,2,true,EncodingType.k4X);
									
	double Kp;					
	double Ki;					
	double Kd;					
																		
	PIDController armPID = new PIDController(Kp,Ki,Kd,encoder,arm);  //PID (P,I,D,INPUT,OUTPUT)
																		
	DigitalInput limitSwitchOut = new DigitalInput(3);				
	DigitalInput limitSwitchIn = new DigitalInput(4);				

	Compressor Compressy = new Compressor(1);						
	int autoLoopCounter;
	
    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.\
     */
	
    public void robotInit() 
    {															
		Compressy.setClosedLoopControl(true);					
    	CameraServer server = CameraServer.getInstance();		
    	server.setQuality(50);									
    	server.startAutomaticCapture("cam0");					
    	encoder.startLiveWindowMode();							
    	armPID.enable();										
    	armPID.setInputRange(0, 550);							    	    	   armPID.setOutputRange(-1, 1);						
    	armPID.setAbsoluteTolerance(5);
    	armPID.startLiveWindowMode();
    }
    
    /**
     * This function is run once each time the robot enters autonomous mode
     */
    
    public void autonomousInit() 
    {
    	
    }

    /**
     * This function is called periodically during autonomous
     */
    
    public void autonomousPeriodic() 
    {

    }
    
    /**
     * This function is called once each time the robot enters tele-operated mode
     */
    
    public void teleopInit() 
    {
    	
    }

    /**
     * This function is called periodically during operator control
     */
    
    public void teleopPeriodic()
    {
    	armPID.setSetpoint(250);
    	SmartDashboard.putNumber("L Trigger Value", controller.getRawAxis(2));
    	SmartDashboard.putNumber("Encoder Value", encoder.getRaw());
    	SmartDashboard.putNumber("Kp", Kp);
    	SmartDashboard.putNumber("Ki", Ki);
    	SmartDashboard.putNumber("Kd", Kd);
    	SmartDashboard.putNumber("ARMPID", armPID.get());
    	SmartDashboard.putNumber("ARMPID6", armPID.getSetpoint());
    	SmartDashboard.putBoolean("ARMPID7", armPID.onTarget());
    	
    	Kp = .1;
    	Ki = .1;
    	Kd = .1;
    	
    	//arm.set(controller.getRawAxis(2));
    	
    	if (controller.getRawButton(2))
    	{
    		pnuarm.set(.85);
    	}
    	else if (controller.getRawButton(3))
    	{
    		pnuarm.set(-.85);
    	}
    	else
    	{
    		pnuarm.set(0);
    	}
    	
    	if (controller.getRawAxis(3) > .5) 
    	{ 
    			launchWheel0.set(.5);
    			launchWheel1.set(-.5);	
    			
    	}
    	else if(controller.getRawButton(6))
    	{
    			launchWheel0.set(-1);
    			launchWheel1.set(1); 
    	}
    	else
    	{
    			launchWheel0.set(0);
    			launchWheel1.set(0);
    	}
    	
    	
    	if (controller.getRawButton(1)) 
    	{
    		Punch.set(true);
    	}
    	else
    	{
        	Punch.set(false);
    	}
    		
    	//Drive
    	leftDrive0.set(-controller.getRawAxis(1));
    	leftDrive1.set(-controller.getRawAxis(1));
    	rightDrive0.set(controller.getRawAxis(5));
    	rightDrive1.set(controller.getRawAxis(5));
    	arm.set(armPID.get());
    	//if  (controller.getRawButton(4))
    	//{
    		//if (encoder.get() < 530)
    		//{
    	    	//arm.set(controller.getRawAxis(2)+.4);
    		//}
    		//else
    		//{
    			//arm.set(0.1);
    		//}
    	//}
    	
    }
    
    /**
     * This function is called periodically during test mode
     */
    
    public void testPeriodic() 
    {
    	LiveWindow.run();
    }
    
}
Reply With Quote
 


Thread Tools
Display Modes Rate This Thread
Rate This Thread:

Posting Rules
You may not post new threads
You may not post replies
You may not post attachments
You may not edit your posts

vB code is On
Smilies are On
[IMG] code is On
HTML code is Off
Forum Jump


All times are GMT -5. The time now is 09:45.

The Chief Delphi Forums are sponsored by Innovation First International, Inc.


Powered by vBulletin® Version 3.6.4
Copyright ©2000 - 2017, Jelsoft Enterprises Ltd.
Copyright © Chief Delphi