Final CMP changes

robot/autonomous/LowBar.py

@@ -81,7 +81,9 @@ def lower_arm(self, initial_call):
         if self.intake.on_target(): 
             self.next_state('drive_under_bar')
     @state
-    def drive_under_bar(self):
+    def drive_under_bar(self, state_tm):
+        if state_tm > 1.5:
+            self.intake.set_target_position(1000)
         if self.drive.drive_distance(self.Drive_Bar_Distance*12, max_speed=self.Max_Drive_Speed):
             self.next_state('drive_forward')
 
@@ -167,14 +169,38 @@ def drive_to_ramp(self, initial_call):
     def lower_to_shoot(self):
         self.intake.set_arm_middle()
 
-        if self.intake.on_target():
+        if self.drive.align_to_tower() and self.intake.on_target():
             self.next_state('shoot')
 
 
     @timed_state(duration = 2, next_state='intakeBall')
     def shoot(self):
         self.intake.outtake()
+
+    @timed_state(duration=2, next_state='unrotate')
+    def go_back(self, initial_call):
+        if initial_call:
+            self.drive.reset_drive_encoders()
+
+        if self.drive.drive_distance(self.Ramp_Distance*-12, max_speed=self.Max_Drive_Speed):
+            self.next_state('unrotate')
+
+    @state
+    def unrotate(self):
+        if self.drive.angle_rotation(0):
+            self.next_state('go_back_more')
+
+    @state
+    def go_back_more(self,initial_call):
+        if initial_call:
+            self.drive.reset_drive_encoders()
+        if self.drive.drive_distance(self.Drive_Distance*-12, max_speed=self.Max_Drive_Speed):
+            self.next_state('end')
 
+    @state
+    def end(self):
+        pass
+
     @timed_state(duration = 2, next_state = 'shoot')
     def intakeBall(self):
         self.intake.intake()

robot/components/drive.py

@@ -34,7 +34,7 @@ def __init__(self):
         self.angle_P = self.sd.getAutoUpdateValue('Drive/Angle_P', .055)
         self.angle_I = self.sd.getAutoUpdateValue('Drive/Angle_I', 0)
         self.drive_constant = self.sd.getAutoUpdateValue('Drive/Drive_Constant', .0001)
-        self.rotate_max = self.sd.getAutoUpdateValue('Drive/Max Gyro Rotate Speed', .4)
+        self.rotate_max = self.sd.getAutoUpdateValue('Drive/Max Gyro Rotate Speed', .37)
 
         self.enabled = False
         self.align_angle = None

robot/components/intake.py

@@ -28,7 +28,7 @@ def __init__(self):
 
         self.positions = [
             self.sd.getAutoUpdateValue('Arm/Bottom', 3000),
-            self.sd.getAutoUpdateValue('Arm/Middle', 2500),
+            self.sd.getAutoUpdateValue('Arm/Middle', 2300),
             self.sd.getAutoUpdateValue('Arm/Top', -20),
           ]
         self.position_threshold = self.sd.getAutoUpdateValue("Arm/On Target Threshold", 25)
@@ -262,7 +262,7 @@ def execute(self):
             self.leftArm.set(0)
 
         if self.leftArm.isFwdLimitSwitchClosed():
-            self.leftArm.setPosition(3000)
+            self.leftArm.setPosition(3600)
 
         if self.leftArm.isRevLimitSwitchClosed():
             self.leftArm.setPosition(0)

robot/physics.py

@@ -94,6 +94,9 @@ def update_sim(self, hal_data, now, tm_diff):
         rr_motor = -hal_data['CAN'][20]['value']/1023
 
         fwd, rcw = four_motor_drivetrain(lr_motor, rr_motor, lf_motor, rf_motor, speed=5)
+        if abs(fwd) > 0.1:
+            rcw += -(0.2*tm_diff)
+
         self.controller.drive(fwd, rcw, tm_diff)
 
         # Simulate the camera approaching the tower