import java.io.DataInputStream;
import java.io.DataOutputStream;
import java.io.IOException;
import java.util.ArrayList;
import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.UltrasonicSensor;
import lejos.nxt.comm.Bluetooth;
import lejos.nxt.comm.NXTConnection;
import lejos.robotics.localization.OdometryPoseProvider;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.robotics.navigation.Pose;
public class Drive {
private DifferentialPilot dp;
private OdometryPoseProvider od;
//private double globalHeading;
private int originX;
private int originY;
private UltrasonicSensor usLeft, usRight;
private final double wheelDiameter = 5.6;
// private final double leftWheel = 5.6;
// private final double rightWheel = 5.55;
private final double trackWidth = 18.61; // 17.22
private final int pixelDistance = 2;
private ArrayList<ArrayList<Integer>> chainArray;
private int imageHeight;
public static void main(String[] args){
Drive d = new Drive();
d.createDataStream();
d.chainArray = d.readArray();
d.writeToComputer("Driving");
d.run(d.testArray());
}
public ArrayList<ArrayList<Integer>> testArray() {
return chainArray;
}
public Drive(){
dp = new DifferentialPilot(wheelDiameter, trackWidth, Motor.A, Motor.B);
od = new OdometryPoseProvider(dp);
usLeft = new UltrasonicSensor(SensorPort.S3);
usRight = new UltrasonicSensor(SensorPort.S4);
//LCD.drawString(""+dp.getRotateSpeed(), 0, 4);
//LCD.drawString(""+dp.getTravelSpeed(), 0, 5);
//LCD.drawString(""+dp.getMaxTravelSpeed(), 0, 6);
dp.setRotateSpeed(120);
dp.setTravelSpeed(25);
dp.setAcceleration(30);
// LCD.drawString("x: " + x,0,4);
// LCD.drawString("y: " + y,0,5);
//writeToComputer(1);
//globalHeading = 0;
}
public void run(ArrayList<ArrayList<Integer>> chainArray){
raisePen();
precalibrate();
for (int i = 0; i < chainArray.size(); i++) {
int x = chainArray.get(i).get(0);
int y = imageHeight-chainArray.get(i).get(1);
writeToComputer("Starting new line, going to coordinate: " + "("+x+", "+y+")");
double[] distAngleArray = calcAngle(x,y);
// LCD.refresh();
// LCD.drawString("Turning to:" + distAngleArray[1], 0, 0);
dp.rotate(distAngleArray[1]);
// writeToComputer("FirstAngle", ""+distAngleArray[1]);
//globalHeading += distAngleArray[1];
// LCD.refresh();
// LCD.drawString("Driving" + distAngleArray[0], 0, 0);
dp.travel(distAngleArray[0]);
// writeToComputer("Driving Distance", ""+distAngleArray[0]);
dp.rotate(-distAngleArray[1]);
// writeToComputer("Distangle 2", ""+ (-distAngleArray[1]));
//globalHeading +=-distAngleArray[1];
lowerPen();
for (int j = 3; j < chainArray.get(i).size(); j++) {
int nextHeading = chainArray.get(i).get(j);
gotoHeading(nextHeading);
}
raisePen();
returnToStart();
// lowerPen();
writeToComputer("<!> recalibrate x");
recalibrate(originX);
writeToComputer("<!> recalibrate y");
recalibrate(originY);
dp.rotate(-180);
od.setPose(new Pose());
lowerPen();raisePen();
}
try {
System.exit(0);
Thread.sleep(2000);
} catch (InterruptedException e) {
}
}
public void gotoHeading(int nextHeading) {
//float currentHeading = (float)globalHeading
//float turnTo = -currentHeading;
Pose p = od.getPose();
float turnTo = -(float)p.getHeading();
double distance = 0;
//writeToComputer("Global heading: " + globalHeading);
if (nextHeading == 1) {
turnTo = giveShortestAngle(turnTo, 135, -225); distance = Math.sqrt(2);}
if (nextHeading == 2) {
turnTo = giveShortestAngle(turnTo, 90, -270);distance = 1;}
if (nextHeading == 3) {
turnTo = giveShortestAngle(turnTo, 45, -315);distance = Math.sqrt(2);}
if (nextHeading == 4) {
turnTo = giveShortestAngle(turnTo, 180, -180);distance = 1;}
if (nextHeading == 5) {
turnTo = giveShortestAngle(turnTo, 0, -360);distance = 1;}
if (nextHeading == 6) {
turnTo = giveShortestAngle(turnTo, 225, -135);distance = Math.sqrt(2);}
if (nextHeading == 7) {
turnTo = giveShortestAngle(turnTo, 270, -90);distance = 1;}
if (nextHeading == 8) {
turnTo = giveShortestAngle(turnTo, 315, -45);distance = Math.sqrt(2);}
// if (nextHeading == 6) {turnTo -= 135;distance = Math.sqrt(2);}
// if (nextHeading == 7) {turnTo -= 90;distance = 1;}
// if (nextHeading == 8) {turnTo -= 45;distance = Math.sqrt(2);}
dp.rotate(turnTo);
//globalHeading += turnTo;
dp.travel(distance*pixelDistance);
}
private float giveShortestAngle(float turnTo, float positive, float negative){
if(Math.abs(turnTo + positive)< Math.abs(turnTo + negative)){
return turnTo + positive;
}
else return turnTo + negative;
}
public double[] calcAngle(float x, float y) {
double c = Math.sqrt(Math.pow(x,2) + Math.pow(y,2));
double radian = Math.asin(y/c);
double angle = radian*180/Math.PI;
double distanceToDrive = c*pixelDistance;
double[] out = {distanceToDrive,angle};
return out;
}
public void returnToStart() {
Pose p = od.getPose();
double x = p.getX()/pixelDistance;
double y = p.getY()/pixelDistance;
double[] distAngle = calcAngle((float)x,(float)y);
//double currentHeading = globalHeading;
dp.rotate(-p.getHeading()-180+distAngle[1]);
dp.travel(distAngle[0]);
writeToComputer("<!> Going home and traveling: " +distAngle[0]);
writeToComputer("<!> Going home and rotating: " + (-p.getHeading()-180+distAngle[1]));
writeToComputer("<!> Going home and current heading: " + (p.getHeading()));
writeToComputer("<!> Going home and pose heading: " + p.getHeading());
dp.rotate(180-p.getHeading()+(p.getHeading()-distAngle[1]));
writeToComputer("<!> Pose heading after turn: " + p.getHeading());
writeToComputer(" ");
writeToComputer(" ");
}
public void raisePen() {
Motor.C.rotate(-30);
}
public void lowerPen() {
Motor.C.rotate(30);
}
public DataOutputStream dos;
public DataInputStream dis;
public NXTConnection connection;
public void createDataStream(){
LCD.drawString("waiting",0,0);
NXTConnection connection = Bluetooth.waitForConnection();
LCD.clear();
LCD.drawString("connected",0,0);
dos = connection.openDataOutputStream();
dis = connection.openDataInputStream();
}
public void killDataStream(){
connection.close();
}
public void writeToComputer(String message){
try {
String s = message + "%";
char[] cArray = s.toCharArray();
for (char c : cArray){
dos.writeChar(c);
dos.flush();
}
} catch (IOException e) {
e.printStackTrace();
}
}
public ArrayList<ArrayList<Integer>> readArray(){
ArrayList<ArrayList<Integer>> listOfArrays = new ArrayList<ArrayList<Integer>>();
char c = 'l';
String s = "";
try {
ArrayList<Integer> pointChainArray = new ArrayList<Integer>();
while((c = dis.readChar())!='%'){
if(c == '*'){
listOfArrays.add(pointChainArray);
pointChainArray = new ArrayList<Integer>();
}
else if(c == ','){
pointChainArray.add(Integer.parseInt(""+s));
s = "";
}
else {
s +=c;
}
}
s = "";
while((c = dis.readChar())!='%'){
s+=c;
}
imageHeight = Integer.parseInt(s);
Thread.sleep(3000);
} catch (Exception e) {
}
return listOfArrays;
}
public void precalibrate(){
dp.rotate(-91);
originX = usLeft.getDistance(); usSleep();
dp.rotate(-91);
originY = usLeft.getDistance(); usSleep();
writeToComputer("<!> precalibrating. Origin: "+originX+","+originY);
dp.rotate(-183);
Pose p = od.getPose();
writeToComputer("<!> precalibrating. End heading: " + p.getHeading());
}
public void recalibrate(int dist){
dp.rotate(-90);
int left = 0;
int right = 1;
boolean equal = false;
int i = 0;
while(!equal){
while(true){
left = usLeft.getDistance(); usSleep();
if(left != 255) break;
}
while(true){
right = usRight.getDistance(); usSleep();
if(right != 255) break;
}
writeToComputer("left: "+left+", right: "+right);
int numberDifference = (right-left);
if(numberDifference > 3) numberDifference = 3;
if(numberDifference < -3) numberDifference = -3;
writeToComputer("Turn: " + (numberDifference));
if(i == 10 || left == right) equal = true;
else if(left > right){
dp.rotate(numberDifference);
}
else dp.rotate(numberDifference);
i++;
}
int d = left-dist;
writeToComputer("origin : " + dist + ", distance measured: "+left);
writeToComputer("Traveling " + d);
dp.travel(left-dist);
}
public void usSleep(){
try {
Thread.sleep(300);
} catch (InterruptedException e) {
e.printStackTrace();
}
}
}
No comments:
Post a Comment