forked from Techbrick/Stronghold
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Robot.cpp
105 lines (89 loc) · 3.01 KB
/
Robot.cpp
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
#include "Robot.h"
void threadTestFunction(bool* keepRunning)
{
int a = 0;
while (*keepRunning == true)
{
a++; a %= 10;
SmartDashboard::PutNumber("SmartDash Number", a);
Wait(1);
}
SmartDashboard::PutNumber("SmartDash Number", 99999);
}
void updatePositionFunction(bool *keepRunning, Joystick *driveStick, Position *position) { //will need to be updated if drivesticks are updated
bool movingForward = false;
while (*keepRunning == true) {
if (driveStick->GetY() > 0) {
movingForward = true;
}
position->Update(movingForward);
Wait(.02);
}
}
Robot::Robot() :
driveTrain(Constants::driveLeftTalonID, Constants::driveRightTalonID),
driveStick(Constants::driveJoystickChannel),
shooter(Constants::shooterLeftTalonID, Constants::shooterRightTalonID, Constants::shooterAimTalonID),
position()
{
driveTrain.SetExpiration(0.1); // safety feature
}
void Robot::OperatorControl() //teleop code
{
CameraServer::GetInstance()->SetQuality(50);
CameraServer::GetInstance()->StartAutomaticCapture("cam0");
bool testThreadRun = true;
bool updateThreadRun = true;
std::thread testThread(threadTestFunction, &testThreadRun);
std::thread updateThread(updatePositionFunction, &updateThreadRun, &driveStick, &position);
float throttle;
float moveValue;
float rotateValue;
float shooterSpeed;
float shooterAngle;
shooter.Enable();
driveTrain.Enable();
while(IsOperatorControl() && IsEnabled())
{
throttle = (((-driveStick.GetRawAxis(Constants::driveL2)) + 1.0)/2.0); //[0, 1]
moveValue = throttle * driveStick.GetY();
rotateValue = -driveStick.GetX();
shooterSpeed = -driveStick.GetRawAxis(Constants::driveRightStickY); //change
shooterAngle = 0; //change
SmartDashboard::PutNumber("Throttle Value", throttle);
SmartDashboard::PutNumber("Move Value", moveValue);
SmartDashboard::PutNumber("Rotate Value", rotateValue);
float shooterSpeed = -driveStick.GetRawAxis(Constants::driveRightStickY);
if (shooterSpeed < -0.35) {
shooterSpeed = -0.35;
}
driveTrain.ArcadeDrive(moveValue, rotateValue, true);
shooter.SetSpeed(shooterSpeed, -shooterSpeed);
if (driveStick.GetRawButton(Constants::calibrateButton)) {
position.Calibrate();
}
if (driveStick.GetRawButton(Constants::shootButton)) {
if (!shooter.HasBall()) {
shooter.LoadBall();
}
if (shooter.HasBall()) {
shooter.PrepareShooter(shooterAngle, shooterSpeed); //TODO: Change these. Get physics major's equation
}
Wait(1); //how long to wait to shoot. Will need to be adjusted with testing
if (shooter.Angle() > shooterAngle - 2 && shooter.Angle() < shooterAngle + 2 && shooter.WheelSpeed() == shooterSpeed) {
shooter.Shoot();
}
}
SmartDashboard::PutNumber("xPos", position.GetX());
SmartDashboard::PutNumber("yPos", position.GetY());
SmartDashboard::PutString("Version", "Noah 2/14/16, 11:16 PM");
}
shooter.Disable();
driveTrain.Disable();
testThreadRun = false;
testThread.join();
updateThreadRun = false;
updateThread.join();
driveTrain.SetSafetyEnabled(true);
}
START_ROBOT_CLASS(Robot);