Skip to content

Commit

Permalink
Motion Profiling Commit
Browse files Browse the repository at this point in the history 
- Added trapezoidal motion profiling.
- TODO: setup straight drive code with gyro.
  • Loading branch information
@samcrane8
samcrane8 committed Feb 11, 2014
1 parent 02b2d4a commit 1277fdc
Show file tree
Hide file tree
Showing 5 changed files with 162 additions and 105 deletions.
20 changes: 10 additions & 10 deletions WiredCats2014/nbproject/private/private.xml
View file
Original file line number Diff line number Diff line change
@@ -1,10 +1,10 @@
<?xml version="1.0" encoding="UTF-8"?>
<project-private xmlns="http://www.netbeans.org/ns/project-private/1">
<editor-bookmarks xmlns="http://www.netbeans.org/ns/editor-bookmarks/2" lastBookmarkId="0"/>
<open-files xmlns="http://www.netbeans.org/ns/projectui-open-files/2">
<group>
<file>file:/C:/Users/WiredCats/Documents/GitHub/WiredCats2014/WiredCats2014/src/edu/wpi/first/wpilibj/templates/RobotMap.java</file>
<file>file:/C:/Users/WiredCats/Documents/GitHub/WiredCats2014/WiredCats2014/src/edu/wpi/first/wpilibj/templates/subsystems/SubSystemDrive.java</file>
</group>
</open-files>
</project-private>
<?xml version="1.0" encoding="UTF-8"?>
<project-private xmlns="http://www.netbeans.org/ns/project-private/1">
<editor-bookmarks xmlns="http://www.netbeans.org/ns/editor-bookmarks/2" lastBookmarkId="0"/>
<open-files xmlns="http://www.netbeans.org/ns/projectui-open-files/2">
<group>
<file>file:/C:/Users/WiredCats/Documents/GitHub/WiredCats2014/WiredCats2014/src/edu/wpi/first/wpilibj/templates/RobotMap.java</file>
<file>file:/C:/Users/WiredCats/Documents/GitHub/WiredCats2014/WiredCats2014/src/edu/wpi/first/wpilibj/templates/subsystems/SubSystemDrive.java</file>
</group>
</open-files>
</project-private>
174 changes: 88 additions & 86 deletions WiredCats2014/nbproject/project.bak.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,89 +3,91 @@
This is a sample netbeans project file for a Sun Spot Application project.
You may edit it freely, it doesn't affect the ant-powered build.
--><project xmlns="http://www.netbeans.org/ns/project/1">
<type>org.netbeans.modules.ant.freeform</type>
<configuration>
<general-data xmlns="http://www.netbeans.org/ns/freeform-project/1">
<name>WiredCats2415</name>
<properties>
<property-file>${user.home}/.sunspotfrc.properties</property-file>
<property-file>build.properties</property-file>
<property-file>${sunspot.home}/default.properties</property-file>
</properties>
<folders>
<source-folder>
<label>src</label>
<type>java</type>
<location>src</location>
</source-folder>
</folders>
<ide-actions>
<action name="build">
<target>jar-app</target>
</action>
<action name="clean">
<target>clean</target>
</action>
<action name="run">
<target>deploy</target>
<target>run</target>
</action>
<action name="rebuild">
<target>clean</target>
<target>jar-app</target>
</action>
<action name="debug">
<target>deploy</target>
<target>debug-run</target>
</action>
<action name="javadoc">
<target>javadoc</target>
</action>
</ide-actions>
<export>
<type>folder</type>
<location>build</location>
<build-target>jar-app</build-target>
</export>
<view>
<items>
<source-folder style="packages">
<label>src</label>
<location>src</location>
</source-folder>
<source-file>
<location>build.xml</location>
</source-file>
</items>
<context-menu>
<ide-action name="build"/>
<ide-action name="clean"/>
<ide-action name="run"/>
<ide-action name="rebuild"/>
<ide-action name="debug"/>
<ide-action name="javadoc"/>
<action>
<label>Sun SPOT-deploy</label>
<target>deploy</target>
</action>
<action>
<label>Sun SPOT-jar-deploy</label>
<target>jar-deploy</target>
</action>
<separator/>
</context-menu>
</view>
<subprojects/>
</general-data>
<java-data xmlns="http://www.netbeans.org/ns/freeform-project-java/1">
<compilation-unit>
<package-root>src</package-root>
<classpath mode="boot">${sunspot.home}\lib\squawk.jar</classpath>
<classpath mode="compile">${sunspot.home}\lib\wpilibj.jar;${sunspot.home}\lib\networktables-crio.jar</classpath>
<built-to>build</built-to>
<source-level>1.4</source-level>
</compilation-unit>
</java-data>
</configuration>
</project>
-->
<project xmlns="http://www.netbeans.org/ns/project/1">
<type>org.netbeans.modules.ant.freeform</type>
<configuration>
<general-data xmlns="http://www.netbeans.org/ns/freeform-project/1">
<name>WiredCats2014</name>
<properties>
<property-file>${user.home}/.sunspotfrc.properties</property-file>
<property-file>build.properties</property-file>
<property-file>${sunspot.home}/default.properties</property-file>
</properties>
<folders>
<source-folder>
<label>src</label>
<type>java</type>
<location>src</location>
</source-folder>
</folders>
<ide-actions>
<action name="build">
<target>jar-app</target>
</action>
<action name="clean">
<target>clean</target>
</action>
<action name="run">
<target>deploy</target>
<target>run</target>
</action>
<action name="rebuild">
<target>clean</target>
<target>jar-app</target>
</action>
<action name="debug">
<target>deploy</target>
<target>debug-run</target>
</action>
<action name="javadoc">
<target>javadoc</target>
</action>
</ide-actions>
<export>
<type>folder</type>
<location>build</location>
<build-target>jar-app</build-target>
</export>
<view>
<items>
<source-folder style="packages">
<label>src</label>
<location>src</location>
</source-folder>
<source-file>
<location>build.xml</location>
</source-file>
</items>
<context-menu>
<ide-action name="build"/>
<ide-action name="clean"/>
<ide-action name="run"/>
<ide-action name="rebuild"/>
<ide-action name="debug"/>
<ide-action name="javadoc"/>
<action>
<label>Sun SPOT-deploy</label>
<target>deploy</target>
</action>
<action>
<label>Sun SPOT-jar-deploy</label>
<target>jar-deploy</target>
</action>
<separator/>
</context-menu>
</view>
<subprojects/>
</general-data>
<java-data xmlns="http://www.netbeans.org/ns/freeform-project-java/1">
<compilation-unit>
<package-root>src</package-root>
<classpath mode="boot">${sunspot.home}\lib\squawk.jar</classpath>
<classpath mode="compile">${sunspot.home}\lib\wpilibj.jar;${sunspot.home}\lib\networktables-crio.jar</classpath>
<built-to>build</built-to>
<source-level>1.4</source-level>
</compilation-unit>
</java-data>
</configuration>
</project>
4 changes: 2 additions & 2 deletions WiredCats2014/nbproject/project.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -83,8 +83,8 @@
<java-data xmlns="http://www.netbeans.org/ns/freeform-project-java/1">
<compilation-unit>
<package-root>src</package-root>
<classpath mode="boot">${sunspot.home}\lib\squawk.jar</classpath>
<classpath mode="compile">${sunspot.home}\lib\wpilibj.jar;${sunspot.home}\lib\networktables-crio.jar</classpath>
<classpath mode="boot">${sunspot.home}/lib/squawk.jar</classpath>
<classpath mode="compile">${sunspot.home}/lib/wpilibj.jar:${sunspot.home}/lib/networktables-crio.jar</classpath>
<built-to>build</built-to>
<source-level>1.4</source-level>
</compilation-unit>
Expand Down
54 changes: 52 additions & 2 deletions ...src/edu/wpi/first/wpilibj/templates/commands/AutonomousCommands/CommandStraightDrive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@

package edu.wpi.first.wpilibj.templates.commands.AutonomousCommands;

import Utilities.WiredVector;
import edu.wpi.first.wpilibj.templates.commands.CommandBase;

/**
Expand All @@ -15,18 +16,67 @@
*/
public class CommandStraightDrive extends CommandBase {

//sinusoidal motion profiling.

private WiredVector speeds;

//In polar coordinates
private float destination;
private float currPosition;
private float startingPosition;

public CommandStraightDrive(float distance){

speeds = new WiredVector();
currPosition = 0;
destination = distance;
startingPosition = drivesubsystem.getEncoderValue();

}

protected void initialize() {
}

}

protected void execute() {


currPosition = drivesubsystem.getEncoderValue();
float currVelocity = drivesubsystem.getSpeed();

float power = drivesubsystem.turnPID.pid(getDesiredVelocity(), currVelocity);

//STRAIGHT DRIVE CODE YET TO BE CODED.
//TODO
drivesubsystem.setLeftRight(power, power);
}

public float getDesiredVelocity(){
if ( currPosition < .2*destination){
return (float)(drivesubsystem.MAX_VELOCITY/(.2*destination))*currPosition;
} else if ( currPosition > .8*destination){
return -(float)((drivesubsystem.MAX_VELOCITY/(.2*destination))*(currPosition-(.8f*destination)));
} else {
return drivesubsystem.MAX_VELOCITY;
}
}

/**
* Returns the average current speed of the drivetrain in feet/second.
*/
public float getAverageSpeed(){
//TODO
float sum = 0;
speeds.addVal(drivesubsystem.getSpeed());
for (int i = 0; i < speeds.size(); i++){
sum+= speeds.getVal(i);
}
if (speeds.size() > 10) speeds.removeFirst();
return sum / speeds.size();
}

protected boolean isFinished() {
return false;
return destination - currPosition < 0;
}

protected void end() {
Expand Down
15 changes: 10 additions & 5 deletions WiredCats2014/src/edu/wpi/first/wpilibj/templates/subsystems/SubSystemDrive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,15 +3,12 @@

import Utilities.ChezyGyro;
import Utilities.PID;
import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.Accelerometer;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Solenoid;
import edu.wpi.first.wpilibj.Talon;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.templates.RobotMap;
import edu.wpi.first.wpilibj.templates.commands.CommandArcadeDrive;
import edu.wpi.first.wpilibj.templates.commands.CommandTankDrive;

/**
*
Expand All @@ -22,7 +19,7 @@ public class SubSystemDrive extends Subsystem {
public static final int TICKS_PER_REVOLUTION = 120;
public static final int WHEEL_RADIUS = 2;
public static final float WHEEL_CIRCUMFERENCE_FEET = (float)((WHEEL_RADIUS*2*Math.PI)/12);
public static final float TICKS_TO_FEET_PER_SECOND = WHEEL_CIRCUMFERENCE_FEET / TICKS_PER_REVOLUTION;
public static final float TICKS_TO_FEET = WHEEL_CIRCUMFERENCE_FEET / TICKS_PER_REVOLUTION;

private Talon left = new Talon(RobotMap.DRIVE_LEFT_MOTOR_1);
private Talon left2 = new Talon(RobotMap.DRIVE_LEFT_MOTOR_2);
Expand All @@ -45,6 +42,8 @@ public class SubSystemDrive extends Subsystem {
public PID straightPID;
public PID turnPID;

public float MAX_VELOCITY = 16;

public void init(){
gyro.initGyro();
leftEncoder.start();
Expand Down Expand Up @@ -79,14 +78,20 @@ public void setLowSpeed(){
highSpeedSolenoid.set(false);
}

public float getEncoderValue(){
int ticks = (leftEncoder.getRaw() + -rightEncoder.getRaw());
ticks /= 2;
return (TICKS_TO_FEET * ticks);
}

/**
* Returns the speed of the robot in feet per second.
* @return
*/
public float getSpeed(){
float ticks = (float)(leftEncoder.getRate() + -rightEncoder.getRate());
ticks /= 2;
return TICKS_TO_FEET_PER_SECOND * ticks;
return TICKS_TO_FEET * ticks;
}

public float getAngle(){
Expand Down

0 comments on commit 1277fdc

Please sign in to comment.