RedRobot2014/chassis.cpp at master · team3238/RedRobot2014 · 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
#include "chassis.h"
#include <WPILib.h>

chassis::chassis(UINT8 leftFrontTalonPort, UINT8 leftRearTalonPort, UINT8 rightFrontTalonPort, UINT8 rightRearTalonPort, UINT8 leftUltrasonicPort, UINT8 rightUltrasonicPort){
	LeftFrontTalon = new Talon(leftFrontTalonPort);
	LeftRearTalon = new Talon(leftRearTalonPort);
	RightFrontTalon = new Talon(rightFrontTalonPort);
	RightRearTalon = new Talon(rightRearTalonPort);
	Drivetrain = new RobotDrive(LeftFrontTalon, LeftRearTalon, RightFrontTalon, RightRearTalon);
	Drivetrain->SetInvertedMotor(RobotDrive::kFrontRightMotor, false);
	Drivetrain->SetInvertedMotor(RobotDrive::kFrontLeftMotor, true);
	Drivetrain->SetInvertedMotor(RobotDrive::kRearRightMotor, false);
	Drivetrain->SetInvertedMotor(RobotDrive::kRearLeftMotor, true);
	chassisState = mecanum;
}

void chassis::SetJoystickData(float x, float y, float twist){
	xValue = -(x);
	yValue = -(y);
	twistValue = -(twist);
}

void chassis::EnableAxisLock(){
	chassisState = axisLock;
}

void chassis::DisableAxisLock(){
	chassisState = mecanum;
}

void chassis::Idle(){
	switch(chassisState){

		case mecanum:
			if(xValue < 0) mappedX = -(xValue * xValue);
			else mappedX = xValue * xValue;
			if(yValue < 0) mappedY = -(yValue * yValue);
			else mappedY = yValue * yValue;
			if(twistValue < 0) mappedTwist = -(twistValue * twistValue);
			else mappedTwist = twistValue * twistValue;
			Drivetrain->MecanumDrive_Cartesian(mappedX, mappedY, mappedTwist, 0.0);
		break;

		case axisLock:

		break;
	}
}