-
Notifications
You must be signed in to change notification settings - Fork 0
/
Arm.java
184 lines (162 loc) · 6.68 KB
/
Arm.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
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
package frc.robot.component;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import com.revrobotics.RelativeEncoder;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc.robot.Robot;
public class Arm {
private static final double ArmencoderPulse = 42;// do the number of turns calculate
private static final double Armgearing = 198;
private static CANSparkMax ArmMotorleft;// rotate arm
private static CANSparkMax ArmMotorright;
private static MotorControllerGroup Arm;
private static WPI_TalonSRX lineMotor;// take up and pay off device
private static final int armL = 15;
private static final int armR = 16;
private static final int line = 17;
private static RelativeEncoder ArmEncoder;
private static Double kAP = 0.35;
private static Double kAI = 0.0;
private static Double kAD = 0.0;
private static PIDController ArmPID;
private static Double kLP = 0.3;
private static Double kLI = 0.0;
private static Double kLD = 0.0;
private static PIDController LinePID;
private static double lineLengthModify = 0;
private static Double rotate;
public static void init() {
ArmMotorleft = new CANSparkMax(armL, MotorType.kBrushless);
ArmMotorright = new CANSparkMax(armR, MotorType.kBrushless);
Arm = new MotorControllerGroup(ArmMotorleft, ArmMotorright);
ArmMotorleft.setInverted(true);
// ArmEncoder = ArmMotorleft.getEncoder();
lineMotor = new WPI_TalonSRX(line);
LinePID = new PIDController(kLP, kLI, kLD);
// ArmPID = new PIDController(kAP, kAI, kAD);
lineMotor.configClearPositionOnQuadIdx(true, 10);
lineMotor.setInverted(true);
// ArmMotorleft.getEncoder().setPosition(0);
// ArmMotorright.getEncoder().setPosition(0);
LinePID.setSetpoint(19.5);
ArmPID.setSetpoint(68.5);
// SmartDashboard.putNumber("arm_kP", kAP);
SmartDashboard.putNumber("line_kP", kLP);
}
public static void teleop() {
// rotate = (Robot.xbox.getLeftTriggerAxis() - Robot.xbox.getRightTriggerAxis())
// * 0.2;
// Arm.set(rotate);
// kP = SmartDashboard.getNumber("arm_kP", kAP);
// ArmPID.setP(kAP);
kLP = SmartDashboard.getNumber("line_kP", kLP);
lineMotor.configClearPositionOnQuadIdx(false, 10);
double angle = positionToDegree();// get the angular position
double length = positionToLength(); // get length position
// take up and pay off device
if (Robot.xbox.getYButtonPressed()) {
LinePID.setSetpoint(52.52);
lineLengthModify = 0;
} else if (length > 122 * (1 / Math.cos(35.2)) - 58) {
lineMotor.set(-0.5);
} else if (length > 122 * (1 / Math.cos(angle)) - 58) {
lineMotor.set(-0.5);
} else if (Robot.xbox.getPOV() == 0) {
lineLengthModify += 0.5;
} else if (Robot.xbox.getPOV() == 180) {
lineLengthModify -= 0.5;
} else {
lineMotor.set(0);
}
if (lineMotor.getSelectedSensorPosition() < 0) {
lineMotor.configClearPositionOnQuadIdx(true, 10);
} else {
lineMotor.configClearPositionOnQuadIdx(false, 10);
}
LinePID.setSetpoint(LinePID.getSetpoint() + lineLengthModify);
// if (Robot.xbox.getXButton()) {
// ArmPID.setSetpoint(35.2);
// } else if (Robot.xbox.getYButton()) {
// ArmPID.setSetpoint(68.5);
// length = 0;
// } else {
// double armAngleModify = (Robot.xbox.getLeftTriggerAxis() -
// Robot.xbox.getRightTriggerAxis()) * 0.01;
// ArmPID.setSetpoint(ArmPID.getSetpoint() + armAngleModify);
// }
// SmartDashboard.putNumber("setpoint", ArmPID.getSetpoint());
// SmartDashboard.putNumber("current", positionToDegree());
// SmartDashboard.putNumber("arm enc", ArmEncoder.getPosition());
// SmartDashboard.putNumber("angle",angle);
SmartDashboard.putNumber("line enc", lineMotor.getSelectedSensorPosition());
// SmartDashboard.putNumber("length",length);
SmartDashboard.putNumber("line length", positionToLength());
Controlloop();
}
public static void Controlloop() {
// var ArmVolt = ArmPID.calculate(positionToDegree());
// if (Math.abs(ArmVolt) > 10) {
// ArmVolt = 10 * (ArmVolt > 0 ? 1 : -1);
// }
// Arm.setVoltage(ArmVolt);
// SmartDashboard.putNumber("ArmVolt", ArmVolt);
var LineVolt = LinePID.calculate(positionToLength());
if (Math.abs(LineVolt) > 10) {
LineVolt = LineVolt > 0 ? 10 : -10;
}
lineMotor.setVoltage(LineVolt);
SmartDashboard.putNumber("LineVolt", LineVolt);
}
// do the number of turns calculate(to a particular angle)
public static double positionToDegree() {
double armRate = ArmEncoder.getPosition() * 360 / (Armgearing *
ArmencoderPulse);
return armRate;
}
// do the number of turns calculate(to a particular length)
public static double positionToLength() {
double x = lineMotor.getSelectedSensorPosition();
if (x < 0) {
lineMotor.configClearPositionOnQuadIdx(true, 10);
} else {
lineMotor.configClearPositionOnQuadIdx(false, 10);
}
double length = 0.00473 * x - 0.0000000348 * x * x + 19.5;
return length;
}
public static int autoArmControl(int modeLine, int modeArm) {
switch (modeLine) {
case 0:// the beginning position
LinePID.setSetpoint(19.5);
break;
case 2: // the second level
LinePID.setSetpoint(52.52);
break;
case 3:// the third level
LinePID.setSetpoint(105.65);// the third level
break;
default:
break;
}
switch (modeArm) {
case 0:// the beginning position
ArmPID.setSetpoint(68.5);
break;
case 1: // the second level
ArmPID.setSetpoint(-10);
break;
case 2:// the third level
ArmPID.setSetpoint(35.5);
break;
default:
break;
}
Controlloop();
SmartDashboard.putNumber("line enc", lineMotor.getSelectedSensorPosition());
SmartDashboard.putNumber("line length", positionToLength());
return 0;
}
}