Robot-Code-2020/utilities.py at master · RavenRoboticsTeam1288/Robot-Code-2020 · 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
import wpilib
from wpilib import Encoder
from wpilib import Ultrasonic
import ctre
import wpilib

class utilities():
    def driveNumInches(robot, direction, speed1, num):
        print("Got to DNI")
        ticksNeeded = num * (100/2.67)
        if robot.drivetrainController.frontLeft.getSelectedSensorPosition(0) < ticksNeeded:
            robot.drivetrainController.left.set(speed1)
            #robot.drivetrainController.frontLeft.set(speed1)
            #robot.drivetrainController.rearLeft.set(speed1)
        else:
            robot.drivetrainController.left.set(0)
            #robot.drivetrainController.frontLeft.set(0)
            #robot.drivetrainController.rearLeft.set(0)
            #robot.drivetrainController.encoderLeft.reset()
        if robot.drivetrainController.frontRight.getSelectedSensorPosition(0) < ticksNeeded:
            robot.drivetrainController.right.set(speed1)
            #robot.drivetrainController.frontRight.set(speed1)
            #robot.drivetrainController.rearRight.set(speed1)
        else:
            #robot.right.set(0)
            #robot.drivetrainController.frontRight.set(0)
            #robot.drivetrainController.rearRight.set(0)
            robot.drivetrainController.right.set(0)
            robot.drivetrainController.left.set(0)
            #robot.drivetrainController.encoderRight.reset()
        if robot.drivetrainController.frontLeft.getSelectedSensorPosition(0) >= ticksNeeded and robot.drivetrainController.frontRight.getSelectedSensorPosition(0) >= ticksNeeded:
            return True

    def BallIndex(robot):
        if Shooter.IndexSensor3:
            pass
        else:
            robot.ConveyorMotor1.set(.1)
            robot.ConveyorMotor2.set(.1)
            if Shooter.IndexSensor2:
                if Shooter.IndexTimer > .25:
                    pass
                else:
                    if Shooter.IndexSensor2:
                        robot.ConveyorMotor1.set(0)
                        robot.ConveyorMotor2.set(0)
                        robot.IndexTimer.stop()
                        robot.IndexTimer.reset()
                        robot.IndexRunning = False
                        print("Ball Indexed")

                    if robot.PrintTimer.hasPeriodPassed(1):
                        print(int(robot.IndexTimer.get()))

    def ultrasonicAim(robot, distFt):
        print("ultrasonicAim")
        distGoalLow = distFt * 12.0 - 1.0
        distGoalHigh = distFt * 12.0 + 1.0
        value = robot.ultrasonic.getVoltage() * 4.8
        if value > distGoalHigh:
            robot.drivetrainController.left.set(.1)
            #robot.drivetrainController.frontLeft.set(.1)
            #robot.drivetrainController.frontRight.set(.1)
            #robot.drivetrainController.rearLeft.set(.1)
            #robot.drivetrainController.rearRight.set(.1)
        elif value < distGoalLow:
            robot.drivetrainController.right.set(.1)
            #robot.drivetrainController.frontLeft.set(-.1)
            #robot.drivetrainController.frontRight.set(-.1)
            #robot.drivetrainController.rearLeft.set(-.1)
            #robot.drivetrainController.rearRight.set(-.1)
        else:
            robot.drivetrainController.left.set(0)
            robot.drivetrainController.right.set(0)
            #robot.drivetrainController.frontLeft.set(0)
            #robot.drivetrainController.frontRight.set(0)
            #robot.drivetrainController.rearLeft.set(0)
            #robot.drivetrainController.rearRight.set(0)
            return True


    def turnNumDegrees(robot, num):
        print("turnNumDegrees")
        #currentPos = robot.gyro.getAngle()
        numMin = num - .1
        numMax = num + .1
        angle = robot.gyro.getAngle()
        if angle < 0:
            angles + 360

        if angle > 360:
            angle - 360

        value = abs(angle - num)
        print(angle)

        if angle < numMin or angle > numMax:
            #if value >= 180: #turn left
            if ((value < 0 and abs(value) > 180) or (value > 0 and abs(value) < 180)): #TURN LEFT
                robot.drivetrainController.right.set(0)
                robot.drivetrainController.left.set(.1)
                #robot.drivetrainController.frontLeft.set(-1 * speed)
                #robot.drivetrainController.frontRight.set(-1 * speed)
                #robot.drivetrainController.rearLeft.set(-1 * speed)
                #robot.drivetrainController.rearRight.set(-1 * speed)

            #if value <= 180: #turn right
            elif ((value <= 0 and abs(value) <= 180) or (value >= 0 and abs(value) >= 180)): #TURN RIGHT
                robot.drivetrainController.right.set(.1)
                robot.drivetrainController.left.set(0)
                #robot.drivetrainController.frontLeft.set(1 * speed)
                #robot.drivetrainController.frontRight.set(1 * speed)
                #robot.drivetrainController.rearLeft.set(1 * speed)
                #robot.drivetrainController.rearRight.set(1 * speed)

        else:
            robot.drivetrainController.right.set(0)
            robot.drivetrainController.left.set(0)
            #robot.drivetrainController.frontLeft.set(0)
            #robot.drivetrainController.frontRight.set(0)
            #robot.drivetrainController.rearLeft.set(0)
            #robot.drivetrainController.rearRight.set(0)
            return True

    def ControlPanelDriving(robot):
        #while button held, this runs- ultrasonic check and then encoder driving. use DriveNumInches? slow speed.
        print("dunno problem")
        if robot.AutoTimer.hasPeriodPassed(.1):
            print("got here.")
            value = robot.ultrasonic.getVoltage() * 4.8
            if value >= 2:
                robot.DrivetrainController.left.set(.01)
                robot.DrivetrainController.right.set(.01)
            else:
                robot.drivetrainController.left.set(0)
                robot.drivetrainController.right.set(0)

    #Auto Functions
    def robocheckFeet(robot, distance):
        print("robocheckFeet")
        value = robot.ultrasonic.getVoltage() * 4.8
        value /= 12
        if value > distance:
            robot.moveSafe = "safe"
        elif value <= distance:
            robot.moveSafe = "not safe"

    #trevor's first gyro code:
    '''run gyro.com
    enter
    return True
if gyro = false:
    dont' run gyro(self) init'''