Skip to content

Commit 4e2746c

Browse files
MichaelCrandallMichaelCrandall
authored
Update Robot.cpp
1 parent 8a6feb4 commit 4e2746c

File tree

1 file changed

+79
-47
lines changed

1 file changed

+79
-47
lines changed

‎main/cpp/Robot.cpp

Lines changed: 79 additions & 47 deletions
Original file line numberDiff line numberDiff line change
@@ -10,67 +10,74 @@
1010
#include "Robot.h"
1111

1212
void Robot::RobotInit() {
13-
compressor.Stop ();
13+
UsbCamera front = CameraServer::GetInstance()->StartAutomaticCapture(0);
14+
CameraServer::GetInstance()->GetVideo();
15+
//CameraServer::GetInstance()->SetSize(1);
16+
front.SetResolution(100,200);
17+
compressor.Stop();
1418
timer.Start();
1519
}
1620

1721
void Robot::RobotPeriodic() {
18-
//Vision
19-
UsbCamera floorCamera = CameraServer::GetInstance()->StartAutomaticCapture(0);
20-
UsbCamera driverCamera = CameraServer::GetInstance()->StartAutomaticCapture(1);
21-
driverCamera.SetResolution(320, 240);
22-
floorCamera.SetResolution(640, 480);
23-
//The rest of these do not name a specific camera need to fix that
24-
CvSink cvSink = CameraServer::GetInstance()->GetVideo();
25-
CvSource outputStream = CameraServer::GetInstance()->PutVideo("Rectangle", 640, 480);
26-
Mat mat;
27-
while (true) {
28-
if (cvSink.GrabFrame(mat) == 0) {
29-
outputStream.NotifyError(cvSink.GetError());
30-
}
31-
rectangle(mat, Point(100, 100), Point(400, 400), Scalar(255, 255, 255), 5);
32-
outputStream.PutFrame(mat);
33-
}
34-
35-
//Compressor
3622
if (fightStick.GetRawButton(8) == 1) {
3723
compressor.Start();
3824
} else {
3925
compressor.Stop();
4026
}
41-
27+
//Vision
28+
4229
//Drive
43-
if (flightStick.GetRawButton(2) == 1) { //Slow
30+
/*if (flightStick.GetRawButton(2) == 1) { //Slow
31+
Drive.CurvatureDrive(flightStick.GetRawAxis(1) * -1 * .5, flightStick.GetRawAxis(0) * .5, flightStick.GetRawButton(1));
32+
} else if (flightStick.GetRawButton(11) == 1) { // Fast
33+
Drive.CurvatureDrive(flightStick.GetRawAxis(1) * -1, flightStick.GetRawAxis(0) * .7, flightStick.GetRawButton(1));
34+
} else { //Normal
35+
Drive.CurvatureDrive(flightStick.GetRawAxis(1) * -1 * .7, flightStick.GetRawAxis(0) * .5, flightStick.GetRawButton(1));
36+
}
37+
/*if (flightStick.GetRawButton(2) == 1) { //Slow
4438
if (flightStick.GetRawAxis(1) > 0 || flightStick.GetRawAxis(1) < 0) {
45-
if (0<timer.Get()<(f/2)) {
46-
Drive.CurvatureDrive(acceleration(flightStick.GetRawAxis(1)) * -1 * speedSlow, flightStick.GetRawAxis(2) * turningSpeedSlow, flightStick.GetRawButton(1));
39+
if (timer.Get()>0 && timer.Get()<3) {
40+
Drive.CurvatureDrive(acceleration(flightStick.GetRawAxis(1)) * -.3, flightStick.GetRawAxis(0) * .3, flightStick.GetRawButton(1));
4741
} else {
48-
Drive.CurvatureDrive(flightStick.GetRawAxis(1) * -1 * speedSlow, flightStick.GetRawAxis(2) * turningSpeedSlow, flightStick.GetRawButton(1));
42+
Drive.CurvatureDrive(flightStick.GetRawAxis(1) * -.3, flightStick.GetRawAxis(3) * .3, flightStick.GetRawButton(1));
4943
}
5044
} else {
5145
timer.Reset();
5246
}
53-
} else if (flightStick.GetRawButton(11/*Will Change*/) == 1) { // Fast
47+
} else if (flightStick.GetRawButton(11) == 1) { // Fast
5448
if (flightStick.GetRawAxis(1) > 0 || flightStick.GetRawAxis(1) < 0) {
55-
if (0<timer.Get()<(f/2)) {
56-
Drive.CurvatureDrive(acceleration(flightStick.GetRawAxis(1)) * -1 * speedFast, flightStick.GetRawAxis(2) * turningSpeedFast, flightStick.GetRawButton(1));
49+
if (timer.Get()>0 && timer.Get()<3) {
50+
Drive.CurvatureDrive(acceleration(flightStick.GetRawAxis(1)) * -1, flightStick.GetRawAxis(0) * 1, flightStick.GetRawButton(1));
5751
} else {
58-
Drive.CurvatureDrive(flightStick.GetRawAxis(1) * -1 * speedFast, flightStick.GetRawAxis(2) * turningSpeedFast, flightStick.GetRawButton(1));
52+
Drive.CurvatureDrive(flightStick.GetRawAxis(1) * -1, flightStick.GetRawAxis(0) * 1, flightStick.GetRawButton(1));
5953
}
6054
} else {
6155
timer.Reset();
6256
}
6357
} else { //Normal
64-
if (flightStick.GetRawAxis(1) > 0 || flightStick.GetRawAxis(1) < 0) {
65-
if (0<timer.Get()<(f/2)) {
66-
Drive.CurvatureDrive(acceleration(flightStick.GetRawAxis(1)) * -1 * speedNormal, flightStick.GetRawAxis(2) * turningSpeedSlow, flightStick.GetRawButton(1));
58+
59+
/*if (flightStick.GetRawAxis(0) > 0 || flightStick.GetRawAxis(0) < 0) {
60+
Drive.CurvatureDrive(flightStick.GetRawAxis(1) * 0, flightStick.GetRawAxis(0) * .5, flightStick.GetRawButton(1));
61+
}*/
62+
/*if (flightStick.GetRawAxis(1) > 0 || flightStick.GetRawAxis(1) < 0) {
63+
if (timer.Get()>0 && timer.Get()<3) {
64+
Drive.CurvatureDrive(acceleration(flightStick.GetRawAxis(1)) * -.7, flightStick.GetRawAxis(0) * .5, flightStick.GetRawButton(1));
6765
} else {
68-
Drive.CurvatureDrive(flightStick.GetRawAxis(1) * -1 * speedNormal, flightStick.GetRawAxis(2) * turningSpeedSlow, flightStick.GetRawButton(1));
66+
Drive.CurvatureDrive(flightStick.GetRawAxis(1) * -.7, flightStick.GetRawAxis(0) * .5, flightStick.GetRawButton(1));
6967
}
7068
} else {
7169
timer.Reset();
7270
}
73-
}
71+
}*/
72+
if (flightStick.GetRawButton(1) == 1) {
73+
Drive.ArcadeDrive(flightStick.GetRawAxis(1) * -1, flightStick.GetRawAxis(0) * 1);
74+
} else {
75+
Drive.ArcadeDrive(flightStick.GetRawAxis(1) * -.7, flightStick.GetRawAxis(0) * .7);
76+
}
77+
78+
79+
//timer.HasPeriodPassed(5)
80+
7481
//HAB Lift(???)
7582
//Font
7683
if (flightStick.GetRawButton(7) == 1) {
@@ -92,31 +99,56 @@ void Robot::RobotPeriodic() {
9299
//Forklift Thomas special clutch idea for forklift
93100
//POV is 0=UP 90=Right 180=Down 270=Left and 45 for all the angles
94101
//.GetPOV("0") is the number for the POV system things switch
95-
if (fightStick.GetPOV(0) == 315) { //Up
102+
if (fightStick.GetRawButton(6) == 1) {
103+
if (fightStick.GetPOV(0) == 0) {
104+
if (fightStick.GetRawButton(5) == 1) {
105+
m_forklift.Set(-.5);
106+
} else if (fightStick.GetRawButton(5) == 0) {
107+
m_forklift.Set(-.4);
108+
} else {
109+
m_forklift.Set(-.4);
110+
}
111+
} else if (fightStick.GetPOV(0) == 180) {
112+
if (fightStick.GetRawButton(5) == 1) {
113+
m_forklift.Set(-.03);
114+
} else if (fightStick.GetRawButton(5) == 0) {
115+
m_forklift.Set(-.04);//.005 is too small
116+
} else {
117+
m_forklift.Set(-.04);
118+
}
119+
} else {
120+
m_forklift.Set(-.2);
121+
}
122+
} else {
123+
m_forklift.Set(0);
124+
}
125+
/*if (fightStick.GetPOV(0) == 315) { //Up
96126
if (fightStick.GetRawButton(5) == 1) {
97-
m_forklift.Set(upSpeedFast);
127+
m_forklift.Set(-.4);
98128
} else {
99-
m_forklift.Set(upSpeedNormal);
129+
m_forklift.Set(1);
130+
//-.4
100131
}
101132
} else if (fightStick.GetPOV(0) == 225) { //Down
102133
if (fightStick.GetRawButton(5) == 1) {
103-
m_forklift.Set(downSpeedFast);
134+
m_forklift.Set(-.03);
104135
} else {
105-
m_forklift.Set(downSpeedNormal);
136+
m_forklift.Set(1);
137+
//-.04
106138
}
107-
} else if (fightStick.GetPOV(0) == 270) { //Stall
108-
m_forklift.Set(stall);
139+
} else if (fightStick.GetPOV(0) == 270) { //-.2
140+
m_forklift.Set(-.2);
109141
} else {
110142
m_forklift.Set(0);
111-
}
143+
}*/
112144

113145
//Ball Intake
114146
if (fightStick.GetRawButton(1) == 1) {
115-
m_leftIntake.Set(flywheelIntake);//+
116-
m_rightIntake.Set(-1 * flywheelIntake);
147+
m_leftIntake.Set(.5);//+
148+
m_rightIntake.Set(-1 * .5);
117149
} else if (fightStick.GetRawButton(3) == 1) {
118-
m_leftIntake.Set(-1 * flywheelOutake);
119-
m_rightIntake.Set(flywheelOutake);
150+
m_leftIntake.Set(-1 * .5);
151+
m_rightIntake.Set(.5);
120152
} else {
121153
m_leftIntake.Set(0);
122154
m_rightIntake.Set(0);
@@ -134,8 +166,8 @@ void Robot::RobotPeriodic() {
134166
s_panelLauncherBottom.Set(DoubleSolenoid::kForward);
135167
s_panelLauncherTop.Set(DoubleSolenoid::kForward);
136168
}
137-
SmartDashboard::PutNumber("Pi Value", pi.GetVoltage());
138-
//Add delay
169+
SmartDashboard::PutNumber("Pi Value", pi.GetVoltage());
170+
//Add delay and I do not know if this will display the PI value
139171
}
140172

141173
void Robot::AutonomousInit() {

0 commit comments

Comments
 (0)