-
Notifications
You must be signed in to change notification settings - Fork 3
/
VisionRobot.java
113 lines (95 loc) · 3.41 KB
/
VisionRobot.java
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
package org.usfirst.frc.team2643.robot;
/*
* Written by Adley and Niko
*
*/
import edu.wpi.first.wpilibj.CameraServer;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.SampleRobot;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.command.Scheduler;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.networktables.NetworkTable;
import java.io.IOException;
import com.ni.vision.NIVision;
import com.ni.vision.NIVision.Image;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.vision.AxisCamera;
public class VisionRobot extends Robot
{
Command autonomousCommand;
SendableChooser chooser;
//NetworkTable table;
static int session;
static Image frame;
static AxisCamera camera;
double[] defaultValue = new double [0];
double[] areas;
static double[] centerXs;
static double centerX;
static double width;
double distance;
boolean position = false;
final double MAGIC_DISTANCE_NUMBER = 1.08;
final double MAGIC_AREA_NUMBER = 1.429;
public static int state = 0;
public static int temp = 0;
//NetworkTable grip = NetworkTable.getTable("GRIP/myContoursReport");
static NetworkTable table = NetworkTable.getTable("GRIP");
public static double getDistance()
{
double[] widths = table.getNumberArray("myContoursReport/width", new double[0]);
int temp = 0;
for(int i = 0;i < widths.length; i++)
{
if(widths[i] > widths[temp])
{
temp = i;
}
}
//temp is the position of the largest number in widths
System.out.println("width: " + widths[temp] + "\ndistance: " + ((140.0/118.0)*(1.08*(20*640)/widths[temp])));
//Timer.delay(2);
return (140.0/118.0)*(1.08*(20*640)/widths[temp]);
}
public static void alignRobot(){
temp = 0;
for(int i = 0; i < centerXs.length; i++)
{
if(centerXs[i] > centerXs[temp])
{
temp = i;
}
}
//temp is the position of the largest number in center x's
System.out.println("Center X: " + centerXs[temp]);
if(centerXs[temp] <= 310)
{
frontRightMotor.set(0.22);
frontLeftMotor.set(0.22);
backRightMotor.set(0.22);
backLeftMotor.set(0.22);
System.out.println("Turning left");
}
else if(centerXs[temp] >= 330)
{
frontRightMotor.set(-0.22);
frontLeftMotor.set(-0.22);
backRightMotor.set(-0.22);
backLeftMotor.set(-0.22);
System.out.println("Turing Right");
}
else
{
frontRightMotor.set(0);
frontLeftMotor.set(0);
backRightMotor.set(0);
backLeftMotor.set(0);
System.out.println("Stop and state is 1");
}
temp = 0;
}
}