BlueRobot2014/robot.cpp at master · team3238/BlueRobot2014 · GitHub

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
#include <WPILib.h>
#include "robot.h"
#include "portnums.h"

robot::robot(void){
	joystick = new Joystick(joystickPort);
	theChassis = new chassis(leftFrontTalonPort, leftRearTalonPort, rightFrontTalonPort, rightRearTalonPort);
	theCatapult = new catapult(catapultTalonOnePort, catapultTalonTwoPort, catapultEncoderPortA, catapultEncoderPortB, catapultLimitSwitchPort);
	theCollector = new collector(liftingTalonPort, rollerTalonPort, ballSensorPort, upperLimitSensorPort, lowerLimitSensorPort);
}

void robot::RobotInit() {
	theCatapult->SetMotorPower(-1.0);
	insight_ballDistance.setHeader("Count:");
	insight.registerData(insight_ballDistance, 1);
	insight.startDisplay();
}

void robot::DisabledInit() {
	theCollector->ReInit();
	theCatapult->ReInit();
}

void robot::AutonomousInit() {

}

void robot::TeleopInit() {

}

void robot::DisabledPeriodic() {
	theCatapult->SetStoppingPoint(0);
	int ballDistance = theCatapult->GetEncoderCount();
	insight_ballDistance.setData(ballDistance);
}

void robot::AutonomousPeriodic() {

}

void robot::TeleopPeriodic() {
	float x = joystick->GetRawAxis(1);
	float y = joystick->GetRawAxis(2);
	float twist = joystick->GetRawAxis(3);
	theChassis->SetJoystickData(x, y, -twist);
	if(joystick->GetRawButton(1)){
		theCatapult->SetStoppingPoint(185);
		theCatapult->Fire();
	}
	if(joystick->GetRawButton(2)){
		theCollector->Run();
	}
	if(joystick->GetRawButton(3)){
		theCollector->Disable();
	}
	if(joystick->GetRawButton(4)){
		theCollector->ReInit();
	}
	theCatapult->Idle();
	theCollector->Idle();
	theChassis->Idle();
}


START_ROBOT_CLASS(robot);