Merge pull request #6 from NeuronRobotics/development

Release for Make Faire NYC
NeuronRobotics · Sep 18, 2014 · 560b98e · 560b98e
2 parents 93da888 + 835ee78
commit 560b98e
Show file tree

Hide file tree

Showing 87 changed files with 4,867 additions and 1,980 deletions.
diff --git a/javasdk/NRSDK/addons/com/neuronrobotics/addons/driving/AbstractRobotDrive.java b/javasdk/NRSDK/addons/com/neuronrobotics/addons/driving/AbstractRobotDrive.java
@@ -55,6 +55,10 @@ public void stopRobot(){
 		DriveStraight(0,0);
 	}
 
+	public RobotLocationData getRobotLocation(){
+		return new RobotLocationData(getCurrentX(), getCurrentY(), getCurrentOrentation());
+	}
+
 	public void setCurrentX(double currentX) {
 		//System.out.println("Current X is: "+currentX);
 		this.currentX = currentX;

diff --git a/javasdk/NRSDK/addons/com/neuronrobotics/addons/driving/AckermanBot.java b/javasdk/NRSDK/addons/com/neuronrobotics/addons/driving/AckermanBot.java
@@ -1,6 +1,8 @@
 package com.neuronrobotics.addons.driving;
 
 import com.neuronrobotics.sdk.addons.kinematics.ServoRotoryLink;
+import com.neuronrobotics.sdk.common.Log;
+import com.neuronrobotics.sdk.dyio.peripherals.DigitalOutputChannel;
 import com.neuronrobotics.sdk.pid.PIDChannel;
 import com.neuronrobotics.sdk.pid.PIDCommandException;
 import com.neuronrobotics.sdk.pid.PIDEvent;
@@ -15,7 +17,17 @@ public class AckermanBot extends AbstractRobotDrive {
 	protected double steeringAngle=0;
 	ServoRotoryLink steering;
 	PIDChannel drive;
+	PIDChannel lSteer;
+	PIDChannel rSteer;
+	PIDChannel bSteer;
+
+	boolean complexSteering=false;
+
 	private IAckermanBotKinematics ak = new AckermanDefaultKinematics();
+	private DigitalOutputChannel driveEnable;
+	private DigitalOutputChannel driveDirection;
+	private double scale = 360.0/4096.0;
+	private int currentEncoderReading;
 
 	protected AckermanBot(){
 
@@ -26,14 +38,38 @@ public AckermanBot(ServoRotoryLink  s,PIDChannel d) {
 		steering=s;
 	}
 
+	public AckermanBot(	PIDChannel drive,
+						PIDChannel lSteer,
+						PIDChannel rSteer,
+						PIDChannel bSteer, 
+						DigitalOutputChannel driveEnable, 
+						DigitalOutputChannel driveDirection) {
+		this.driveEnable = driveEnable;
+		this.driveDirection = driveDirection;
+		setPIDChanel(drive);
+
+		this.lSteer=lSteer;
+		this.rSteer=rSteer;
+		this.bSteer=bSteer;
+		complexSteering=true;
+		SetDriveVelocity(0);
+	}
+
 	protected void setPIDChanel(PIDChannel d){
 		drive=d;
 		drive.addPIDEventListener(this);
 	}
 
 	public void setSteeringHardwareAngle(double s) {
-		steering.setTargetAngle(s);
-		steering.flush(0);
+		if(complexSteering==false){
+			steering.setTargetAngle(s);
+			steering.flush(0);
+
+		}else{
+			this.lSteer.SetPIDSetPoint((int) (s/scale), 0);
+			this.rSteer.SetPIDSetPoint((int) (s/scale), 0);
+			this.bSteer.SetPIDSetPoint(0, 0);
+		}
 	}
 
 	public void setSteeringAngle(double s) {
@@ -58,16 +94,23 @@ public double getSteeringAngle() {
 		return steeringAngle;
 	}
 	protected void SetDriveDistance(int ticks, double seconds){
-		System.out.println("Seting PID set point of="+ticks);
-		drive.SetPIDSetPoint(ticks, seconds);
+		Log.debug("Seting PID set point of= "+ticks+" currently at "+currentEncoderReading);
+		//drive.SetPIDSetPoint(ticks, seconds);
+		driveDirection.setHigh(ticks< currentEncoderReading);
+		driveEnable.setHigh(false);
+		ThreadUtil.wait((int) (seconds*1000));
+		driveEnable.setHigh(true);
+		Log.debug("Arrived at= "+currentEncoderReading);
 	}
 	protected void SetDriveVelocity(int ticksPerSecond){
-		System.out.println("Seting PID Velocity set point of="+ticksPerSecond);
-		try {
-			drive.SetPDVelocity(ticksPerSecond, 0);
-		} catch (PIDCommandException e) {
-			e.printStackTrace();
+		Log.debug("Seting PID Velocity set point of="+ticksPerSecond);
+		if(ticksPerSecond>0){
+			driveDirection.setHigh(ticksPerSecond> 0);
+			driveEnable.setHigh(false);
+		}else{
+			driveEnable.setHigh(true);
 		}
+
 	}
 	protected void ResetDrivePosition(){
 		//Log.enableDebugPrint(true);
@@ -101,6 +144,7 @@ public double getMaxTicksPerSecond() {
 
 	@Override
 	public void onPIDEvent(PIDEvent e) {
+		currentEncoderReading = e.getValue();
 		setRobotLocationUpdate(ak.onPIDEvent(e,getSteeringAngle()));
 	}
 

diff --git a/javasdk/NRSDK/addons/com/neuronrobotics/addons/driving/AckermanConfiguration.java b/javasdk/NRSDK/addons/com/neuronrobotics/addons/driving/AckermanConfiguration.java
@@ -2,13 +2,13 @@
 
 public class AckermanConfiguration {
 
-	private double ticksPerRevolution = 128;//ticks
-	private double wheelDiameter = 2.54;//cm
+	private double ticksPerRevolution = 4096;//ticks
+	private double wheelDiameter = 8.0*2.54;//cm
 	private double cmPerRevolution = 2*Math.PI*wheelDiameter;
 	private double ticksToCm = ticksPerRevolution/cmPerRevolution;
 
 	private double maxTicksPerSeconds = 200;
-	private double wheelbase = 14.2;//cm
+	private double wheelbase = 16.5*2.54;//cm
 	private double servoToSteerAngle=1;
 
 	public void setMaxTicksPerSeconds(double maxTicksPerSeconds) {

diff --git a/javasdk/NRSDK/addons/com/neuronrobotics/addons/driving/NrMap.java b/javasdk/NRSDK/addons/com/neuronrobotics/addons/driving/NrMap.java
@@ -18,14 +18,14 @@ public class NrMap extends JPanel{
 	 */
 	private static final long serialVersionUID = -1487461776000494761L;
 
-	private double pixelToCm=10;
+	private double pixelToCm=100;
 	JLabel lab=new JLabel();
 	private BufferedImage display;
 
 	private ArrayList<userDefinedObsticles> obs = new ArrayList<userDefinedObsticles>();
 
-	protected static final double width = 600;
-	protected static final double height = 480;
+	protected static final double width = 1024;
+	protected static final double height = 1024;
 
 	/**
 	 * Instantiate a robot map using a default blank map. 
@@ -65,7 +65,7 @@ protected void setDisplay(BufferedImage d){
 
 	public BufferedImage getMap() {
 		if(display==null){
-			return new BufferedImage(600, 480,BufferedImage.TYPE_INT_RGB);
+			return new BufferedImage((int)width,(int) height,BufferedImage.TYPE_INT_RGB);
 		}
 		BufferedImage d = new BufferedImage(display.getWidth(), display.getHeight(),BufferedImage.TYPE_INT_RGB);
 		Graphics2D g =d.createGraphics();
@@ -79,11 +79,11 @@ public BufferedImage getMap() {
 	public void removeAllUserDefinedObsticles(){
 		obs.clear();
 	}
-	public void addUserDefinedObsticle(int x, int y, int size){
+	public void addUserDefinedObsticle(int x, int y, int size,ObsticleType type){
 		if(display==null){
-			display =  new BufferedImage(600, 480,BufferedImage.TYPE_INT_RGB);
+			display =  new BufferedImage((int)width,(int) height,BufferedImage.TYPE_INT_RGB);
 		}
-		obs.add(new userDefinedObsticles(x,y,size));
+		obs.add(new userDefinedObsticles(x,y,size,type));
 
 	}
 
@@ -99,14 +99,16 @@ public double getCmToPixel(double cm){
 	}
 
 	private class userDefinedObsticles{
-		public userDefinedObsticles(int x2, int y2, int size2) {
+		ObsticleType type;
+		public userDefinedObsticles(int x2, int y2, int size2,ObsticleType type) {
+			this.type = type;
 			setX(x2);
 			setY(y2);
 			setSize(size2);
 		}
 
 		public void drawUserObsticles(Graphics2D g) {
-			g.setColor(Color.pink);
+			g.setColor(type.getValue());
 			g.fillRect(getX()-(getSize()/2),getY()-(getSize()/2), getSize(),getSize());
 		}
 
@@ -138,5 +140,21 @@ public int getSize() {
 		private int y;
 		private int size;
 	}
+	public void setUserDefinedData(ArrayList<DataPoint> data,ObsticleType type) {
+		 //removeAllUserDefinedObsticles();
+		 for(DataPoint d:data){
+			 double pix =  getCmToPixel(d.getRange()/100);
+			 double centerX=(width/2);
+			 double centerY=(height/2);
+			 if(!(pix>centerX || pix>centerY )){
+				 double deltX = pix*Math.cos(Math.toRadians(d.getAngle()));
+				 double deltY = pix*Math.sin(Math.toRadians(d.getAngle()));
+				 addUserDefinedObsticle((int)(centerX+deltX), (int)(centerY+deltY), 2,type);
+			 }else{
+				 //System.out.println("Range too long: "+pix+" cm="+d.getRange()/100);
+			 }
+		 }
+		 updateMap();
+	}
 
 }
diff --git a/javasdk/NRSDK/addons/com/neuronrobotics/addons/driving/Rbe3002Robot.java b/javasdk/NRSDK/addons/com/neuronrobotics/addons/driving/Rbe3002Robot.java
@@ -24,33 +24,15 @@ public Rbe3002Robot() {
 															DyIOChannelMode.COUNT_IN_INT,//Input mode
 															10,//Output Channel
 															DyIOChannelMode.SERVO_OUT);//Output mode
-		PIDConfiguration lpid =new PIDConfiguration (	1,//PID group
-														true,//enabled
-														false,//inverted
-														true,//Async
-														KP,// Kp
-														KI,// Ki
-														KD,//Kd
-														0,//latch
-														false,//use latch
-														false);//stop on latch
+		PIDConfiguration lpid =new PIDConfiguration ();//stop on latch
 
 
 		DyPIDConfiguration rdypid = new DyPIDConfiguration(	2,//PID group 2
 															19,//Input channel number
 															DyIOChannelMode.COUNT_IN_INT,//Input mode
 															11,//Output Channel
 															DyIOChannelMode.SERVO_OUT);//Output mode
-		PIDConfiguration rpid =new PIDConfiguration (	2,//PID group
-														true,//enabled
-														true,//inverted
-														true,//Async
-														KP,// Kp
-														KI,// Ki
-														KD,//Kd
-														0,//latch
-														false,//use latch
-														false);//stop on latch
+		PIDConfiguration rpid =new PIDConfiguration ();
 		dyio.ConfigureDynamicPIDChannels(ldypid);
 		dyio.ConfigurePIDController(lpid);
 		dyio.ConfigureDynamicPIDChannels(rdypid);

diff --git a/javasdk/NRSDK/addons/com/neuronrobotics/addons/driving/RobotLocationData.java b/javasdk/NRSDK/addons/com/neuronrobotics/addons/driving/RobotLocationData.java
@@ -1,4 +1,7 @@
 package com.neuronrobotics.addons.driving;
+
+import com.neuronrobotics.sdk.addons.kinematics.math.TransformNR;
+
 /**
  * This class represents the Delta position of the robot in the robots co-ordinate system. 
  * Only the delta y, delta x and delta orentation relative to the robots current position
@@ -9,11 +12,21 @@
 
 public class RobotLocationData {
 	private double x,y,o;
+	private TransformNR arm = new TransformNR();
 	public RobotLocationData(double deltaX, double deltaY, double deltaOrentation){
 		 setX(deltaX);
 		 setY(deltaY);
 		 setO(deltaOrentation);
 	}
+
+	public void setArmLocation(TransformNR arm){
+		this.arm = arm;
+
+	}
+	public TransformNR getArmTransform(){
+		return arm;
+	}
+
 	public String toString() {
 		String s="delta: x="+x+" y="+y+" orentation="+o; 
 		return s;

diff --git a/javasdk/NRSDK/addons/com/neuronrobotics/addons/driving/SimpleDisplay.java b/javasdk/NRSDK/addons/com/neuronrobotics/addons/driving/SimpleDisplay.java
@@ -0,0 +1,51 @@
+package com.neuronrobotics.addons.driving;
+
+import java.awt.Color;
+import java.awt.color.CMMException;
+import java.util.ArrayList;
+
+import javax.swing.JFrame;
+
+import com.neuronrobotics.addons.driving.virtual.ObsticleType;
+import com.neuronrobotics.sdk.ui.ConnectionImageIconFactory;
+
+public class SimpleDisplay extends NrMap {
+
+	/**
+	 * 
+	 */
+	private static final long serialVersionUID = -7042174918507023465L;
+	private JFrame frame = new JFrame();
+	public SimpleDisplay(){
+        getFrame().setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
+        setBackground(Color.black);
+        getFrame().add(this);
+        getFrame().setSize((int)width+200,(int)height+200);
+        getFrame().setLocationRelativeTo(null);
+        getFrame().setVisible(true);
+        getFrame().setIconImage( ConnectionImageIconFactory.getIcon("images/hat.png").getImage());
+
+	}
+	public JFrame getFrame() {
+		return frame;
+	}
+	public void setFrame(JFrame frame) {
+		this.frame = frame;
+	}
+	public void setData(ArrayList<DataPoint> data) {
+		 //removeAllUserDefinedObsticles();
+		 for(DataPoint d:data){
+			 double pix =  getCmToPixel(d.getRange()/100);
+			 double centerX=(width/2);
+			 double centerY=(height/2);
+			 if(!(pix>centerX || pix>centerY )){
+				 double deltX = pix*Math.cos(Math.toRadians(d.getAngle()));
+				 double deltY = pix*Math.sin(Math.toRadians(d.getAngle()));
+				 addUserDefinedObsticle((int)(centerX+deltX), (int)(centerY+deltY), 2,ObsticleType.USERDEFINED);
+			 }else{
+				 //System.out.println("Range too long: "+pix+" cm="+d.getRange()/100);
+			 }
+		 }
+		 updateMap();
+	}
+}
diff --git a/javasdk/NRSDK/addons/com/neuronrobotics/addons/driving/virtual/ObsticleType.java b/javasdk/NRSDK/addons/com/neuronrobotics/addons/driving/virtual/ObsticleType.java
@@ -11,11 +11,17 @@ public enum ObsticleType {
 
 	WALL(Color.blue),
 
-	FIRE(Color.orange),
+	FIRE(Color.magenta),
 
-	USERDEFINED(Color.pink),
+	PINKBALL(Color.pink),
+	HOCKYPUCK(Color.red),
+	HOOKSAMPLE(Color.white),
+	ORANGEROD(Color.orange),
+	BASESTATION(Color.yellow),
 
-	NONE(Color.white);
+	USERDEFINED(Color.green),
+
+	NONE(Color.lightGray);
 
 	/** The Constant lookup. */
 	private static final Map<Color,ObsticleType > lookup = new HashMap<Color,ObsticleType >();

diff --git a/javasdk/NRSDK/addons/com/neuronrobotics/sdk/addons/kinematics/gui/DHViewer.java b/javasdk/NRSDK/addons/com/neuronrobotics/sdk/addons/kinematics/gui/DHViewer.java
@@ -36,6 +36,7 @@ public class DHViewer extends SimpleTransformViewer{
 	private static final long serialVersionUID = -7066991305201979906L;
 
 	public DHViewer (DHChain tk,double[] jointSpaceVector){
+
     	ArrayList<TransformNR> chain = tk.getChain(jointSpaceVector);
     	int i=0;
     	for(TransformNR t:chain){

diff --git a/javasdk/NRSDK/addons/com/neuronrobotics/sdk/addons/kinematics/gui/MatrixDisplayNR.java b/javasdk/NRSDK/addons/com/neuronrobotics/sdk/addons/kinematics/gui/MatrixDisplayNR.java
@@ -35,11 +35,12 @@ public MatrixDisplayNR(String name){
 //		int vColIndex = 0;
 //		TableColumn col = getTable().getColumnModel().getColumn(vColIndex);
 //		int width = 40;
-//		col.setPreferredWidth(width);		
-		getTable().getColumnModel().getColumn(0).setPreferredWidth(56);
-		getTable().getColumnModel().getColumn(1).setPreferredWidth(56);
-		getTable().getColumnModel().getColumn(2).setPreferredWidth(56);
-		getTable().getColumnModel().getColumn(3).setPreferredWidth(56);
+//		col.setPreferredWidth(width);	
+		int colWidth = 66;
+		getTable().getColumnModel().getColumn(0).setPreferredWidth(colWidth);
+		getTable().getColumnModel().getColumn(1).setPreferredWidth(colWidth);
+		getTable().getColumnModel().getColumn(2).setPreferredWidth(colWidth);
+		getTable().getColumnModel().getColumn(3).setPreferredWidth(colWidth);
 
 		add(getTable(),"wrap");
 		setEditable(false);