Because I'am an ignorant ahole, I just wrote a small generator idea t…

…est based off of my idea of commands to see how it will work. Testing it tonight,

but I am still sticking to magic bot.

benscommands/mockcommand.py

@@ -0,0 +1,12 @@
+from components.colorsensor.colorwheel import ColorWheel
+
+from generator import Generator
+
+import robot
+
+class AutoSpinWheel(Generator):
+    def __init__(self):
+        super(AutoSpinWheel, self).__init__()
+
+    def initialize(self):
+        robot.mockcommand.getColor()

components/colorsensor/colorwheel.py

@@ -20,21 +20,25 @@ def setup(self):
 
         self.colorWheelMotor.setInverted(False) # might need to change
 
-        self.colorWheelMotor.setP(0.01, 0) # Dummy values from the falcon tester
-        self.colorWheelMotor.setI(0, 0)
-        self.colorWheelMotor.setD(0.1, 0)
-        self.colorWheelMotor.setIZone(1, 0)
-        self.colorWheelMotor.setFF(0.1, 0)
+        self.colorWheelController.setP(0.01, 0) # Dummy values from the falcon tester
+        self.colorWheelController.setI(0, 0)
+        self.colorWheelController.setD(0.1, 0)
+        self.colorWheelController.setIZone(1, 0)
+        self.colorWheelController.setFF(0.1, 0)
 
     def getColor(self):
         return self.colorSensor.getColor()
 
-    def spinFour(self):
+    def autoSpinWheel(self):
+        ''' Should be about 3.25 rotations of the wheel '''
         #self.colorWheelMotor.set(ControlMode.Position, (self.colorWheelMotor.getSelectedSensorPosition() + )
-        self.colorWheelController.setReference(50, ControlType.kPosition, 0, 0)
+        self.colorWheelController.setReference(9630, ControlType.kPosition, 0, 0) # Look at photos for calc, Ben.
 
     def spinClockwise(self):
         self.colorWheelMotor.set(0.9)
 
     def spinCClockwise(self):
         self.colorWheelMotor.set(-0.9)
+
+    def execute(self):
+        pass

generator.py

@@ -0,0 +1,24 @@
+class Generator:
+
+    def __init__(self):
+        self.initialize()
+        while not self.isFinished():
+            self.execute()
+            yield
+        self.end()
+
+    def initialize(self):
+        print('No i ran')
+        pass
+
+    def execute(self):
+        pass
+
+    def isFinished(self):
+        return True # change as needed
+
+    def interrupted(self):
+        pass # probably will never use
+
+    def end(self):
+        pass

robot.py

@@ -15,6 +15,8 @@
 
 from components.falcon.falconcomponent import FalconTest
 
+from components.colorsensor.colorwheel import ColorWheel
+
 #from statemachines.drivetrain.movemachine import MoveStateMachine
 
 from controller.logitechdualshock import LogitechDualshock
@@ -29,18 +31,23 @@
 import collections
 
 class KryptonBot(magicbot.MagicRobot):
-    #smartcargointake: SmartIntake
-#cargooutake: CargoOutake
-
     robotdrive: RobotDrive
     velocity: TankDrive
 
     falcon: FalconTest
 
     potent: Potentiometer
 
+    wheelactions: ColorWheel
+
     #movemachine: MoveStateMachine
 
+    @classmethod
+    def createGenerators(cls):
+        try:
+            sys.modules['robot'].ColorWheel = ColorWheel()
+        except KeyError:
+            pass
     def createObjects(self):
 
         #self.compBot =  Config('DriveTrain/Robot', True) # Make this tunable or nt value
@@ -71,9 +78,9 @@ def createObjects(self):
         self.falconTest.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, 0)
 
         self.colorSensor = ColorSensorV3(wpilib.I2C.Port.kOnboard)
-        self.colorwheelMotor = CANSparkMax(ports.ColorWheelPorts.motorID, MotorType.kBrushless)# WPI_TalonSRX(ports.ColorWheelPorts.motorID)
-        self.colorWheelEncoder = self.colorwheelMotor.getEncoder()
-        self.colorWheelController = self.colorwheelMotor.getPIDController()
+        self.colorWheelMotor = CANSparkMax(ports.ColorWheelPorts.motorID, MotorType.kBrushless)# WPI_TalonSRX(ports.ColorWheelPorts.motorID)
+        self.colorWheelEncoder = self.colorWheelMotor.getEncoder()
+        self.colorWheelController = self.colorWheelMotor.getPIDController()
 
         self.velocityCalculator = TankDrive()
 
@@ -91,6 +98,9 @@ def createObjects(self):
 
         self.useActives = []
 
+        self.createGenerators()
+
+
     def teleopInit(self):
         self.robotdrive.prepareToDrive(self.compBot)
 

statemachines/colorwheel/setcolor.py

@@ -0,0 +1,31 @@
+from magicbot import StateMachine, state
+
+from components.colorsensor.colorwheel import ColorWheel
+
+class AutoSetColor(StateMachine):
+    wheelactions: ColorWheel
+
+    def __init__(self):
+        self.colors = ['y', 'r', 'g', 'b']
+        self.direction = True
+        self.colorDistance = 37.56 # 11.8", 37.56 rotations (30:1 + 3" diameter)
+
+    def autoSetColor(self):
+        self.engage()
+
+    @state(first=True)
+    def getColorAndNeeded(self):
+        self.myColor = self.wheelactions.getColor() # Make sure this provides the correct value, not an assumption
+        self.desiredColor = 'r' # Make this come from FMS later. NOTE: If not a string, simply use a dictionary instead.
+
+        # Forward?
+        if self.colors[self.myColor] < self.colors[self.desiredColor]:
+            self.direction = True
+        elif self.colors[self.myColor] > self.colors[self.desiredColor]:
+            self.direction = False
+        else:
+            self.direction = None
+
+        self.next_state_now(spinWheel)
+
+    def spinWheel(self):

statemachines/colorwheel/spinwheel.py

@@ -0,0 +1 @@
+# Probably won't need this, just do it in the component

statemachines/drivetrain/driverobotmachine.py

@@ -7,7 +7,7 @@
 from magicbot import StateMachine, state, timed_state, default_state
 
 class DriveRobotMachine(StateMachine):
-    robotdrive = RobotDrive
+    robotdrive: RobotDrive
 
     def beginDrive(self):
         self.engage()