Beispiel #1
0
    def update(self):
        current_time = time.time()
        dt = current_time - self.last_time
        updated = False

        if (dt >= self.time_period):
            self.speed = accelerationRestrictor(dt, self.last_speed,
                                                self.desired_speed,
                                                MAX_SPEED_ACCEL)
            self.steering = accelerationRestrictor(dt, self.last_steering,
                                                   self.desired_steering,
                                                   MAX_STEERING_ACCEL)

            left_power = powerDeadzoneHandler(self.speed - self.steering)
            right_power = powerDeadzoneHandler(self.speed + self.steering)

            self.LeftMotor.power = int(
                mapToLimits(left_power, MAX_OUTPUT, MIN_OUTPUT))
            self.RightMotor.power = int(
                mapToLimits(right_power, MAX_OUTPUT, MIN_OUTPUT))

            self.last_speed = self.speed
            self.last_steering = self.steering
            self.last_time = current_time
            updated = True
        return updated
Beispiel #2
0
 def setGrab(self, grab):
     self.position['grab'] = grab
     self.servos[ARMS_SERVO_BOARD][LEFT_GRAB_PIN] = int(
         mapToLimits((self.position['grab'] + LEFT_GRAB_OFFSET) *
                     LEFT_GRAB_DIRECTION))
     self.servos[ARMS_SERVO_BOARD][RIGHT_GRAB_PIN] = int(
         mapToLimits((self.position['grab'] + RIGHT_GRAB_OFFSET) *
                     RIGHT_GRAB_DIRECTION))
Beispiel #3
0
 def setLift(self, lift):
     self.position['lift'] = lift
     self.servos[ARMS_SERVO_BOARD][LEFT_LIFT_PIN] = int(
         mapToLimits((self.position['lift'] + LEFT_LIFT_OFFSET) *
                     LEFT_LIFT_DIRECTION))
     self.servos[ARMS_SERVO_BOARD][RIGHT_LIFT_PIN] = int(
         mapToLimits((self.position['lift'] + RIGHT_LIFT_OFFSET) *
                     RIGHT_LIFT_DIRECTION))
Beispiel #4
0
 def setRotate(self, rotate):
     self.position['rotate'] = rotate
     self.servos[ARMS_SERVO_BOARD][LEFT_ROTATE_PIN] = int(
         mapToLimits((self.position['rotate'] + LEFT_ROTATE_OFFSET) *
                     LEFT_ROTATE_DIRECTION))
     self.servos[ARMS_SERVO_BOARD][RIGHT_ROTATE_PIN] = int(
         mapToLimits((self.position['rotate'] + RIGHT_ROTATE_OFFSET) *
                     RIGHT_ROTATE_DIRECTION))
Beispiel #5
0
    def run(self, value):

        if (self.stopped == True):
            self.output = 0
            output_calculated = True

        else:  #self.stopped == False
            current_time = time.time()
            dt = current_time - self.last_time
            output_calculated = False

            if (self.first_run == False):

                if (dt >= self.time_period):
                    error = self.setpoint - value

                    self.p = self.kp * error
                    self.i += self.ki * error * dt
                    self.d = (error - self.last_error) / dt

                    if (self.i_limit != 0):
                        self.i = mapToLimits(self.i, self.i_limit,
                                             -self.i_limit)

                    if ((self.max_output != 0) or (self.min_output != 0)):
                        self.output = mapToLimits((self.p + self.i + self.d),
                                                  self.max_output,
                                                  self.min_output)

                    else:
                        self.output = self.p + self.i + self.d

                    output_calculated = True

                    self.last_time = current_time
                    self.last_error = error

            else:  #self.first_run == True
                error = value - self.setpoint

                self.p = self.kp * error
                self.i = self.starting_i
                self.d = 0

                if ((self.max_output != 0) or (self.min_output != 0)):
                    self.output = mapToLimits((self.p + self.i + self.d),
                                              self.max_output, self.min_output)

                else:
                    self.output = self.p + self.i + self.d

                output_calculated = True

                self.last_time = current_time
                self.last_error = error
                self.first_run = False

        return output_calculated
Beispiel #6
0
def debugYawDrift():

    if (DEBUG_YAW_DRIFT == True):
        print "debugging yaw drift"
        MotionThread.setAction(STILL)
        wake_up_time = time.time()

        while (True):
            wake_up_time += DEBUG_TIMEPERIOD
            print str(MotionThread.yaw)
            sleep_time = mapToLimits(wake_up_time - time.time(),
                                     DEBUG_TIMEPERIOD, 0)
            time.sleep(DEBUG_TIMEPERIOD)
Beispiel #7
0
    def getOutputFromAngle(self, new_camera_angle):
        appropriate_lower_measurement_index = 0
        appropriate_upper_measurement_index = len(CAMERA_MEASUREMENTS) - 1

        lower_measurement_index = 0
        upper_measurement_index = 1

        while (upper_measurement_index < len(CAMERA_MEASUREMENTS)):

            if (new_camera_angle <=
                    CAMERA_MEASUREMENTS[upper_measurement_index][0]
                    and new_camera_angle >=
                    CAMERA_MEASUREMENTS[lower_measurement_index][0]):
                appropriate_lower_measurement_index = lower_measurement_index
                appropriate_upper_measurement_index = upper_measurement_index

            lower_measurement_index += 1
            upper_measurement_index += 1

        # print str(appropriate_lower_measurement_index)
        # print str(appropriate_upper_measurement_index)

        lower_measurement_angle = CAMERA_MEASUREMENTS[
            appropriate_lower_measurement_index][0]
        upper_measurement_angle = CAMERA_MEASUREMENTS[
            appropriate_upper_measurement_index][0]
        lower_measurement_output = CAMERA_MEASUREMENTS[
            appropriate_lower_measurement_index][1]
        upper_measurement_output = CAMERA_MEASUREMENTS[
            appropriate_upper_measurement_index][1]

        # print "lower_measurement_angle " + str(lower_measurement_angle)
        # print "upper_measurement_angle " + str(upper_measurement_angle)
        # print "lower_measurement_output " + str(lower_measurement_output)
        # print "upper_measurement_output " + str(upper_measurement_output)

        gradient = (upper_measurement_output - lower_measurement_output) / (
            upper_measurement_angle - lower_measurement_angle)
        d_angle = (new_camera_angle - lower_measurement_angle)
        output = lower_measurement_output + (gradient * d_angle)

        # print "gradient " + str(gradient)
        # print "d_angle " + str(d_angle)
        # print "output " + str(output)

        # output = (((new_camera_angle - lower_measurement_angle) / (upper_measurement_angle - lower_measurement_angle)) * (upper_measurement_output - lower_measurement_output)) + lower_measurement_output
        output = int(mapToLimits(output, MAX_CAMERA_OUTPUT, MIN_CAMERA_OUTPUT))

        return output
Beispiel #8
0
    def moveCameraServo(self, new_camera_angle):
        # print "new_camera_angle = " + str(new_camera_angle)
        new_camera_angle = mapToLimits(new_camera_angle, MAX_CAMERA_ANGLE,
                                       MIN_CAMERA_ANGLE)
        #new_output = int(mapToLimits(((((new_camera_angle - MIN_CAMERA_ANGLE) / (MAX_CAMERA_ANGLE - MIN_CAMERA_ANGLE)) * (MAX_CAMERA_OUTPUT - MIN_CAMERA_OUTPUT)) + MIN_CAMERA_OUTPUT), MAX_CAMERA_OUTPUT, MIN_CAMERA_OUTPUT))

        # print "new_camera_angle = " + str(new_camera_angle)
        new_output = self.getOutputFromAngle(new_camera_angle)
        # print "new_output = " + str(new_output)

        change_in_output = abs(new_output - self.last_output)
        self.last_output = new_output
        # print "new_output = " + str(new_output)
        self.ruggeduino.setCameraServoAngle(new_output)
        self.camera_angle = new_camera_angle
        return change_in_output
Beispiel #9
0
    def run(self):
        print "Starting " + self.name
        wake_up_time = time.time() + self.time_period

        while (True):
            self.time_to_sleep = mapToLimits(wake_up_time - time.time(),
                                             self.time_period, 0)
            wake_up_time += self.time_period
            time.sleep(self.time_to_sleep)

            self.updateSensors()
            self.updateRobotLocation()
            self.processActionBundleStack()
            new_steering = self.runYawPid()
            new_speed = self.runDistancePid()
            self.updateMotors(new_steering, new_speed)

        print "Exiting " + self.name
Beispiel #10
0
 def setDesiredSpeed(self, desired_speed):
     self.desired_speed = mapToLimits(desired_speed, MAX_SPEED, MIN_SPEED)
     return self.update()
Beispiel #11
0
def accelerationRestrictor(dt, last_value, desired_value, max_accel):
    max_change = dt * max_accel
    value_upper_limit = last_value + max_change
    value_lower_limit = last_value - max_change
    output = mapToLimits(desired_value, value_upper_limit, value_lower_limit)
    return output
Beispiel #12
0
 def setDesiredSpeedAndSteering(self, desired_speed, desired_steering):
     self.desired_speed = mapToLimits(desired_speed, MAX_SPEED, MIN_SPEED)
     self.desired_steering = mapToLimits(desired_steering, MAX_STEERING,
                                         MIN_STEERING)
     return self.update()
Beispiel #13
0
 def setDesiredSteering(self, desired_steering):
     self.desired_steering = mapToLimits(desired_steering, MAX_STEERING,
                                         MIN_STEERING)
     return self.update()