org.tahomarobotics.sim.model.Transmission Maven / Gradle / Ivy

Go to download
Show more of this group Show more artifacts with this name
Show all versions of bear-simulation Show documentation
robotics simulation for FRC robotics java development
The newest version!
/**
 * Copyright 2018 Tahoma Robotics - http://tahomarobotics.org - Bear Metal 2046 FRC Team
 * 
 * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated 
 * documentation files (the "Software"), to deal in the Software without restriction, including without 
 * limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the 
 * Software, and to permit persons to whom the Software is furnished to do so, subject to the following 
 * conditions:
 * 
 * The above copyright notice and this permission notice shall be included in all copies or substantial portions 
 * of the Software.
 * 
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED 
 * TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 
 * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF 
 * CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 
 * DEALINGS IN THE SOFTWARE.
 * 
 */
package org.tahomarobotics.sim.model;

public class Transmission {

	public enum Shift { HIGH, LOW };
	
	private final Motor fwdMotors[];
	
	private final Motor revMotors[];
	
	private final double highGearRatio;
	private final double lowGearRatio;
	
	private double torque;
	private double velocity;
	private double position;
		
	private Shift shift = Shift.LOW;
	
	public Transmission(Motor motors[], boolean reverse, double highGearRatio, double lowGearRatio) {
		this.fwdMotors = reverse ? new Motor[] {} : motors;
		this.revMotors = reverse ? motors : new Motor[] {};
		this.highGearRatio = highGearRatio;
		this.lowGearRatio = lowGearRatio;
	}

	public Transmission(Motor fwdMotors[], Motor revMotors[], double highGearRatio, double lowGearRatio) {
		this.fwdMotors = fwdMotors;
		this.revMotors = revMotors;
		this.highGearRatio = highGearRatio;
		this.lowGearRatio = lowGearRatio;
	}
	
	public double getTorque() {
		return torque;
	}
	
	public void shift(Shift shift) {
		this.shift = shift;
	}
	
	public void setVelocity(double velocity) {
		this.velocity = velocity;
	}
	
	public void setPosition(double position) {
		this.position = position;
	}
	
	public void update() {
		double gearRatio = shift == Shift.HIGH ? highGearRatio : lowGearRatio;
		
		double motorVelocity = velocity * gearRatio;
		
		double torque = 0;
		
		for(Motor motor : fwdMotors) {
			motor.setVelocity(motorVelocity);
			motor.update();
			torque += motor.getTorque();
		}
		for(Motor motor : revMotors) {
			motor.setVelocity(-motorVelocity);
			motor.update();
			torque -= motor.getTorque();
		}
		
		this.torque = torque * gearRatio;
	}

	public double getVelocity() {
		return velocity;
	}

	public double getPosition() {
		return position;
	}
	
	@Override
	public String toString() {
		return String.format("p:%6.3f v:%6.3f t:%6.3f", position, velocity, torque);
	}

}