FTC2019/FTC2018Design.txt at master · Aztechs157/FTC2019 · 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
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
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
import packages

DcMotor driveMotors[] = [new DcMotor, new DcMotor, new DcMotor, new DcMotor]
DcMotor miscMotors[] = [new DcMotor, new DcMotor]
boolean inverseControls = false
boolean intakeOut = false
boolean actuator = false

function driveMode(controller input1)
    if input1.back == true && inverseControls == false then
        inverseControls = true
    else if input1.back == true && inverseControls == true then
        inverseControls == false
    end if
    return inverseControls
end function driveMode

function setmovement(controller input1, inverseControls)
    real motor1 = 0
    real motor2 = 0
    real motor3 = 0
    real motor4 = 0

    if inverseControls == true then
        real x = input1.leftStickX
        real y = input1.leftStickY
    else
        real x = input1.rightStickX
        real y = input1.rightStickY

    if x >= 0 && y >= 0 then
        motor1 = y - x
        motor2 = min(-x, -y)
        motor3 = x - y
        motor4 = max(x, y)
    else if x >= 0 && y < 0 then
        motor1 = min(-x, y)
        motor2 = |y| - x
        motor3 = max(x, -y)
        motor4 = x - |y|
    else if x < 0 && y >= 0 then
        motor1 = max(-x, y)
        motor2 = |x| - y
        motor3 = min(x, -y)
        motor4 = y - |x|
    else if x < 0 && y < 0 then
        motor1 = |x| - |y|
        motor2 = max(-x, -y)
        motor3 = |y| - |x|
        motor4 = min(x, y)
    else
        motor1 = 0
        motor2 = 0
        motor3 = 0
        motor4 = 0
    end if
    real motorTotal = motor1 + motor2 + motor3 + motor4
    real motors[] = [motor1, motor2, motor3, motor4, motorTotal]

    return motors[]
end function setmovement

function negative(real n)
    newN = -|n|
    return newN
end function

function turning(real motors[], controller input1, inverseControls)
    if inverseControls == true then
        real x = input1.leftStickX
        real y = input1.leftStickY
    else
        real x = input1.rightStickX
        real y = input1.rightStickY

    real turningRate = input1.rightTrigger - input1.leftTrigger

    if motors[4] == 0
        motors[0] = turningRate
        motors[1] = turningRate
        motors[2] = turningRate
        motors[3] = turningRate
    else
        if turningRate >= 0
            if -x >= |y| then
                motors[0] = motors[0] - 2(y - negative(x))turningRate + (1 + x) * turningRate
            else if y > |x| then
                motor[0] += (1 - y) * turningRate
            else if x > |y| then
                motor[0] += (1 - x) * turningRate
            else
                motor[0] += (1 + y) * turningRate
            end if

            if |x| < -y then
                motors[1] = motors[1] + 2(y + |x|)turningRate + (1 + y) * turningRate
            else if y > |x| then
                motor[0] += (1 - y) * turningRate
            else if x > |y| then
                motor[0] += (1 - x) * turningRate
            else
                motor[0] += (1 + x) * turningRate
            end if

            if x > |y| then
                motors[2] = motors[2] + 2(|y| - x)turningRate
            end if

            if |x| < y
                motors[3] = motors[3] - 2(y - |x|)turningRate
            end if

        else
            if -x >=|y|
                motors[0] = motors[0] + 2(negative(y) - x)turningRate
            end if

            if |x| < y
                motors[1] = motors[1] - 2(negative(y) + x)turningRate
            end if

            if x > |y|
                motors[2] = motors[2] - 2(y - |x|)turningRate
            end if

            if negative(y) > x
                motors[3] = motors[3] - 2(negative(y) - x)turningRate
            end if

    return motors[]
end function turning

procedure drive(real motors[])
    driveMotors[0].setPower(motors[0])
    driveMotors[1].setPower(motors[1])
    driveMotors[2].setPower(motors[2])
    driveMotors[3].setPower(motors[3])
end procedure drive

procedure actuator(controller input1)
	real target
	if input1.buttonX == true then
		actuator = !actuator
	end if
	if actuator then
		target = 100
	else
		target = 0
	end if
	output pid(current, target) to actuator motor
end procedure actuator

							procedure actuator(controller input1)
								boolean actuator = false

								if input1.buttonX == true and actuator == false then
									set actuator motor to 1
									wait for the actuator to get to full extension
									set actuator motor to 0
									actuator = true
			doesn't work ->
								else if input1.buttonX == true and actuator == true then
									set actuator motor to -1
									wait for the actuator to fully retract
									set actuator motor to 0
									actuator = false
								end if
							end procedure actuator

procedure intakeposition(controller input2, Servo servos[], boolean intakeOut)
    if input2.buttonY == true && intakeOut == false then
        servo.setPosition(servos[2]) = 90
        servo.setPosition(servos[3]) = 90
        intakeOut = true
    else if input2.buttonY == true && intakeOut == true
        servo.setPosition(servos[2]) = 0
        servo.setPosition(servos[3]) = 0
        intakeOut = false
    end if
end procedure intakeposition

procedure intake(controller input2, Servo servos[])
    if input2.rightTrigger > 0 && input2.buttonA then
        servos[0] = (1 - input2.rightTrigger) * 90
        servos[1] = input2.rightTrigger * 90 + 90
    else if input2.rightTrigger > 0 then
        servos[0] = input2.rightTrigger * 90 + 90
        servos[1] = (1 - input2.rightTrigger) * 90
    end if
end procedure intake

procedure augur(controller input2, miscMotors[])
     if input2.leftTrigger > 0 && input2.buttonB then
        miscMotors[1].setPower(-input2.leftTrigger)
    else if input2.rightTrigger > 0 then
        miscMotors[1].setPower(input2.leftTrigger)
    end if
end procedure augur

while opModeIsActive do
    driveMode(input1)
    setmovement(input1, inverseControls)
    turning(motors[], input1, inverseControls)
    drive(motors[])
    actuator(input1)
    intakeposition(input2, servos[], intakeOut)
    intake(input2, servos[])
    augur(input2, miscMotors[])
end while