def initialize(self):
        """Figure out the file location and play it back."""

        try:
            #attempt to access the files required
            if self.robot.isReal():
                self.f = open("/home/lvuser/py/macros/"+self.name)
            else:
                self.f = open(self.name)
            self.reader_iterator = csv.DictReader(self.f)
        except FileNotFoundError:
            #This bit runs if the file isn't there
            self.reader_iterator = []

        #length of time to play the macro.
        self.setTimeout(Settings.num_macro_timeout)

        #start time is important for making sure everything plays at the right time
        start_time = Timer.getFPGATimestamp()

        #do the actual playback bit
        for line in self.reader_iterator:
            t_delta = float(line["Time"]) - (Timer.getFPGATimestamp()-start_time)
            if t_delta > 0:
                Timer.delay(t_delta)
            #Add subsystems in the following manner:
            #self.robot.subsystem.manualCommand(float(line["Row_Name"]))

            self.robot.drivetrain.driveManual(float(line["Drive_X"]),
                                            float(line["Drive_Y"]),
                                            float(line["Drive_Z"]))

            if self.isTimedOut() or self.done_yet:
                break
 def initialize(self):
     try:
         if self.robot.isReal():
             self.f = open("/home/lvuser/py/"+self.name)
         else:
             self.f = open(self.name)
         self.reader_iterator = csv.DictReader(self.f)
     except FileNotFoundError:
         self.reader_iterator = []
     self.setTimeout(15)
     start_time = Timer.getFPGATimestamp()
     for line in self.reader_iterator:
         t_delta = float(line["Time"]) - (Timer.getFPGATimestamp()-start_time)
         if t_delta > 0:
             Timer.delay(t_delta)
         self.robot.drivetrain.driveManual(float(line["Drive_X"]),
                                         float(line["Drive_Y"]),
                                         float(line["Drive_Rotation"]))
         self.robot.lift.manualSet(float(line["Lift"]))
         self.robot.mast.manualSet(float(line["Mast"]))
         self.robot.claw.manualSet(float(line["Claw"]))
        # self.robot.lock.spike.set(int(line["Lock"]))
         self.robot.winch.manualSet(float(line["Winch"]))
         if self.isTimedOut() or self.done_yet:
             break
Beispiel #3
0
 def initialize(self):
     try:
         if self.robot.isReal():
             self.f = open("/home/lvuser/py/" + self.name)
         else:
             self.f = open(self.name)
         self.reader_iterator = csv.DictReader(self.f)
     except FileNotFoundError:
         self.reader_iterator = []
     self.setTimeout(15)
     start_time = Timer.getFPGATimestamp()
     for line in self.reader_iterator:
         t_delta = float(
             line["Time"]) - (Timer.getFPGATimestamp() - start_time)
         if t_delta > 0:
             Timer.delay(t_delta)
         self.robot.drivetrain.driveManual(float(line["Drive_X"]),
                                           float(line["Drive_Y"]),
                                           float(line["Drive_Rotation"]))
         self.robot.lift.manualSet(float(line["Lift"]))
         self.robot.mast.manualSet(float(line["Mast"]))
         self.robot.claw.manualSet(float(line["Claw"]))
         # self.robot.lock.spike.set(int(line["Lock"]))
         self.robot.winch.manualSet(float(line["Winch"]))
         if self.isTimedOut() or self.done_yet:
             break
 def initialize(self):
     try:
         #attempt to access the files required
         if self.robot.isReal():
             self.f = open("/home/lvuser/py/"+self.name)
         else:
             self.f = open(self.name)
         self.reader_iterator = csv.DictReader(self.f)
     except FileNotFoundError:
         #This bit runs if the file isn't there
         self.reader_iterator = []
     self.setTimeout(15)
     start_time = Timer.getFPGATimestamp()
     for line in self.reader_iterator:
         t_delta = float(line["Time"]) - (Timer.getFPGATimestamp()-start_time)
         if t_delta > 0:
             Timer.delay(t_delta)
         self.robot.subsystem.manualSet(float(line["Subsystem"]))
         if self.isTimedOut() or self.done_yet:
             break
Beispiel #5
0
    def initialize(self):
        """Figure out the file location and play it back."""
        print("Initializing macro " + self.name + "...")

        try:
            #attempt to access the files required
            if self.robot.isReal():
                self.f = open("/home/lvuser/py/macros/" + self.name)
            else:
                self.f = open(self.name)
            self.reader_iterator = csv.DictReader(self.f)
        except FileNotFoundError:
            #This bit runs if the file isn't there
            self.reader_iterator = []

        #length of time to play the macro.
        self.setTimeout(Settings.num_macro_timeout)

        #start time is important for making sure everything plays at the right time
        start_time = Timer.getFPGATimestamp()
        #do the actual playback bit
        for line in self.reader_iterator:
            t_delta = float(
                line["Time"]) - (Timer.getFPGATimestamp() - start_time)
            if t_delta > 0:
                Timer.delay(t_delta)
            #Add subsystems in the following manner:
            #self.robot.subsystem.manualCommand(float(line["Row_Name"]))

            self.robot.drivetrain.driveManual(float(line["Drive_Y"]),
                                              float(line["Drive_Twist"]))
            self.robot.ears.manualSet(float(line["Ears"]))
            self.robot.hat.manualSet(float(line["Hat"]))
            self.robot.tilt.manualSet(float(line["Tilt"]))

            if self.isTimedOut() or self.done_yet:
                break
Beispiel #6
0
 def isConnected(self):
     time_since_last_update = Timer.getFPGATimestamp(
     ) - self.last_update_time
     return time_since_last_update <= IO_TIMEOUT_SECONDS
Beispiel #7
0
    def getCurrentData(self):
        first_address = IMURegisters.NAVX_REG_UPDATE_RATE_HZ
        displacement_registers = self.board_capabilities._isDisplacementSupported(
        )

        # If firmware supports displacement data, acquire it - otherwise implement
        # similar (but potentially less accurate) calculations on this processor.
        if displacement_registers:
            read_count = IMURegisters.NAVX_REG_LAST + 1 - first_address
        else:
            read_count = IMURegisters.NAVX_REG_QUAT_OFFSET_Z_H + 1 - first_address

        curr_data = self.io_provider.read(first_address, read_count)

        #timestamp_low = AHRSProtocol.decodeBinaryUint16(curr_data, IMURegisters.NAVX_REG_TIMESTAMP_L_L-first_address)
        #timestamp_high = AHRSProtocol.decodeBinaryUint16(curr_data, IMURegisters.NAVX_REG_TIMESTAMP_H_L-first_address)
        #sensor_timestamp            = (timestamp_high << 16) + timestamp_low
        ahrspos_update = self.ahrspos_update
        ahrspos_update.op_status = curr_data[IMURegisters.NAVX_REG_OP_STATUS -
                                             first_address]
        ahrspos_update.selftest_status = curr_data[
            IMURegisters.NAVX_REG_SELFTEST_STATUS - first_address]
        ahrspos_update.cal_status = curr_data[IMURegisters.NAVX_REG_CAL_STATUS]
        ahrspos_update.sensor_status = curr_data[
            IMURegisters.NAVX_REG_SENSOR_STATUS_L - first_address]
        ahrspos_update.yaw = AHRSProtocol.decodeProtocolSignedHundredthsFloat(
            curr_data, IMURegisters.NAVX_REG_YAW_L - first_address)
        ahrspos_update.pitch = AHRSProtocol.decodeProtocolSignedHundredthsFloat(
            curr_data, IMURegisters.NAVX_REG_PITCH_L - first_address)
        ahrspos_update.roll = AHRSProtocol.decodeProtocolSignedHundredthsFloat(
            curr_data, IMURegisters.NAVX_REG_ROLL_L - first_address)
        ahrspos_update.compass_heading = AHRSProtocol.decodeProtocolUnsignedHundredthsFloat(
            curr_data, IMURegisters.NAVX_REG_HEADING_L - first_address)
        ahrspos_update.mpu_temp_c = AHRSProtocol.decodeProtocolSignedHundredthsFloat(
            curr_data, IMURegisters.NAVX_REG_MPU_TEMP_C_L - first_address)
        ahrspos_update.world_linear_accel_x = AHRSProtocol.decodeProtocolSignedThousandthsFloat(
            curr_data, IMURegisters.NAVX_REG_LINEAR_ACC_X_L - first_address)
        ahrspos_update.world_linear_accel_y = AHRSProtocol.decodeProtocolSignedThousandthsFloat(
            curr_data, IMURegisters.NAVX_REG_LINEAR_ACC_Y_L - first_address)
        ahrspos_update.world_linear_accel_z = AHRSProtocol.decodeProtocolSignedThousandthsFloat(
            curr_data, IMURegisters.NAVX_REG_LINEAR_ACC_Z_L - first_address)
        ahrspos_update.altitude = AHRSProtocol.decodeProtocol1616Float(
            curr_data, IMURegisters.NAVX_REG_ALTITUDE_D_L - first_address)
        ahrspos_update.baro_pressure = AHRSProtocol.decodeProtocol1616Float(
            curr_data, IMURegisters.NAVX_REG_PRESSURE_DL - first_address)
        ahrspos_update.fused_heading = AHRSProtocol.decodeProtocolUnsignedHundredthsFloat(
            curr_data, IMURegisters.NAVX_REG_FUSED_HEADING_L - first_address)
        ahrspos_update.quaternionW = AHRSProtocol.decodeBinaryInt16(
            curr_data, IMURegisters.NAVX_REG_QUAT_W_L - first_address)
        ahrspos_update.quaternionX = AHRSProtocol.decodeBinaryInt16(
            curr_data, IMURegisters.NAVX_REG_QUAT_X_L - first_address)
        ahrspos_update.quaternionY = AHRSProtocol.decodeBinaryInt16(
            curr_data, IMURegisters.NAVX_REG_QUAT_Y_L - first_address)
        ahrspos_update.quaternionZ = AHRSProtocol.decodeBinaryInt16(
            curr_data, IMURegisters.NAVX_REG_QUAT_Z_L - first_address)
        if displacement_registers:
            ahrspos_update.vel_x = AHRSProtocol.decodeProtocol1616Float(
                curr_data, IMURegisters.NAVX_REG_VEL_X_I_L - first_address)
            ahrspos_update.vel_y = AHRSProtocol.decodeProtocol1616Float(
                curr_data, IMURegisters.NAVX_REG_VEL_Y_I_L - first_address)
            ahrspos_update.vel_z = AHRSProtocol.decodeProtocol1616Float(
                curr_data, IMURegisters.NAVX_REG_VEL_Z_I_L - first_address)
            ahrspos_update.disp_x = AHRSProtocol.decodeProtocol1616Float(
                curr_data, IMURegisters.NAVX_REG_DISP_X_I_L - first_address)
            ahrspos_update.disp_y = AHRSProtocol.decodeProtocol1616Float(
                curr_data, IMURegisters.NAVX_REG_DISP_Y_I_L - first_address)
            ahrspos_update.disp_z = AHRSProtocol.decodeProtocol1616Float(
                curr_data, IMURegisters.NAVX_REG_DISP_Z_I_L - first_address)

            self.notify_sink._setAHRSPosData(ahrspos_update)
        else:
            self.notify_sink._setAHRSData(ahrspos_update)

        board_state = self.board_state
        board_state.cal_status = curr_data[IMURegisters.NAVX_REG_CAL_STATUS -
                                           first_address]
        board_state.op_status = curr_data[IMURegisters.NAVX_REG_OP_STATUS -
                                          first_address]
        board_state.selftest_status = curr_data[
            IMURegisters.NAVX_REG_SELFTEST_STATUS - first_address]
        board_state.sensor_status = AHRSProtocol.decodeBinaryUint16(
            curr_data, IMURegisters.NAVX_REG_SENSOR_STATUS_L - first_address)
        board_state.update_rate_hz = curr_data[
            IMURegisters.NAVX_REG_UPDATE_RATE_HZ - first_address]
        board_state.gyro_fsr_dps = AHRSProtocol.decodeBinaryUint16(
            curr_data, IMURegisters.NAVX_REG_GYRO_FSR_DPS_L)
        board_state.accel_fsr_g = curr_data[IMURegisters.NAVX_REG_ACCEL_FSR_G]
        board_state.capability_flags = AHRSProtocol.decodeBinaryUint16(
            curr_data,
            IMURegisters.NAVX_REG_CAPABILITY_FLAGS_L - first_address)
        self.notify_sink._setBoardState(board_state)

        raw_data_update = self.raw_data_update
        raw_data_update.raw_gyro_x = AHRSProtocol.decodeBinaryInt16(
            curr_data, IMURegisters.NAVX_REG_GYRO_X_L - first_address)
        raw_data_update.raw_gyro_y = AHRSProtocol.decodeBinaryInt16(
            curr_data, IMURegisters.NAVX_REG_GYRO_Y_L - first_address)
        raw_data_update.raw_gyro_z = AHRSProtocol.decodeBinaryInt16(
            curr_data, IMURegisters.NAVX_REG_GYRO_Z_L - first_address)
        raw_data_update.raw_accel_x = AHRSProtocol.decodeBinaryInt16(
            curr_data, IMURegisters.NAVX_REG_ACC_X_L - first_address)
        raw_data_update.raw_accel_y = AHRSProtocol.decodeBinaryInt16(
            curr_data, IMURegisters.NAVX_REG_ACC_Y_L - first_address)
        raw_data_update.raw_accel_z = AHRSProtocol.decodeBinaryInt16(
            curr_data, IMURegisters.NAVX_REG_ACC_Z_L - first_address)
        raw_data_update.cal_mag_x = AHRSProtocol.decodeBinaryInt16(
            curr_data, IMURegisters.NAVX_REG_MAG_X_L - first_address)
        raw_data_update.cal_mag_y = AHRSProtocol.decodeBinaryInt16(
            curr_data, IMURegisters.NAVX_REG_MAG_Y_L - first_address)
        raw_data_update.cal_mag_z = AHRSProtocol.decodeBinaryInt16(
            curr_data, IMURegisters.NAVX_REG_MAG_Z_L - first_address)
        raw_data_update.mpu_temp_c = ahrspos_update.mpu_temp
        self.notify_sink._setRawData(raw_data_update)

        self.last_update_time = Timer.getFPGATimestamp()
        self.byte_count += len(curr_data)
        self.update_count += 1
 def isConnected(self):
     time_since_last_update = Timer.getFPGATimestamp() - self.last_update_time
     return time_since_last_update <= IO_TIMEOUT_SECONDS
 def getCurrentData(self):
     first_address = IMURegisters.NAVX_REG_UPDATE_RATE_HZ
     displacement_registers = self.board_capabilities._isDisplacementSupported()
     
     # If firmware supports displacement data, acquire it - otherwise implement
     # similar (but potentially less accurate) calculations on this processor.
     if displacement_registers:
         read_count = IMURegisters.NAVX_REG_LAST + 1 - first_address
     else:
         read_count = IMURegisters.NAVX_REG_QUAT_OFFSET_Z_H + 1 - first_address
     
     curr_data = self.io_provider.read(first_address, read_count)
 
     sensor_timestamp               = AHRSProtocol.decodeBinaryUint32(curr_data, IMURegisters.NAVX_REG_TIMESTAMP_L_L-first_address)
     if sensor_timestamp == self.last_sensor_timestamp:
         return
     
     self.last_sensor_timestamp = sensor_timestamp
     
     ahrspos_update = self.ahrspos_update
     ahrspos_update.op_status       = curr_data[IMURegisters.NAVX_REG_OP_STATUS - first_address]
     ahrspos_update.selftest_status = curr_data[IMURegisters.NAVX_REG_SELFTEST_STATUS - first_address]
     ahrspos_update.cal_status      = curr_data[IMURegisters.NAVX_REG_CAL_STATUS]
     ahrspos_update.sensor_status   = curr_data[IMURegisters.NAVX_REG_SENSOR_STATUS_L - first_address]
     ahrspos_update.yaw             = AHRSProtocol.decodeProtocolSignedHundredthsFloat(curr_data, IMURegisters.NAVX_REG_YAW_L-first_address)
     ahrspos_update.pitch           = AHRSProtocol.decodeProtocolSignedHundredthsFloat(curr_data, IMURegisters.NAVX_REG_PITCH_L-first_address)
     ahrspos_update.roll            = AHRSProtocol.decodeProtocolSignedHundredthsFloat(curr_data, IMURegisters.NAVX_REG_ROLL_L-first_address)
     ahrspos_update.compass_heading = AHRSProtocol.decodeProtocolUnsignedHundredthsFloat(curr_data, IMURegisters.NAVX_REG_HEADING_L-first_address)
     ahrspos_update.mpu_temp_c      = AHRSProtocol.decodeProtocolSignedHundredthsFloat(curr_data, IMURegisters.NAVX_REG_MPU_TEMP_C_L - first_address)
     ahrspos_update.world_linear_accel_x  = AHRSProtocol.decodeProtocolSignedThousandthsFloat(curr_data, IMURegisters.NAVX_REG_LINEAR_ACC_X_L-first_address)
     ahrspos_update.world_linear_accel_y  = AHRSProtocol.decodeProtocolSignedThousandthsFloat(curr_data, IMURegisters.NAVX_REG_LINEAR_ACC_Y_L-first_address)
     ahrspos_update.world_linear_accel_z  = AHRSProtocol.decodeProtocolSignedThousandthsFloat(curr_data, IMURegisters.NAVX_REG_LINEAR_ACC_Z_L-first_address)
     ahrspos_update.altitude        = AHRSProtocol.decodeProtocol1616Float(curr_data, IMURegisters.NAVX_REG_ALTITUDE_D_L - first_address)
     ahrspos_update.baro_pressure   = AHRSProtocol.decodeProtocol1616Float(curr_data, IMURegisters.NAVX_REG_PRESSURE_DL - first_address)
     ahrspos_update.fused_heading   = AHRSProtocol.decodeProtocolUnsignedHundredthsFloat(curr_data, IMURegisters.NAVX_REG_FUSED_HEADING_L-first_address)
     ahrspos_update.quaternionW     = AHRSProtocol.decodeBinaryInt16(curr_data, IMURegisters.NAVX_REG_QUAT_W_L-first_address)/ 32768.0
     ahrspos_update.quaternionX     = AHRSProtocol.decodeBinaryInt16(curr_data, IMURegisters.NAVX_REG_QUAT_X_L-first_address)/ 32768.0
     ahrspos_update.quaternionY     = AHRSProtocol.decodeBinaryInt16(curr_data, IMURegisters.NAVX_REG_QUAT_Y_L-first_address)/ 32768.0
     ahrspos_update.quaternionZ     = AHRSProtocol.decodeBinaryInt16(curr_data, IMURegisters.NAVX_REG_QUAT_Z_L-first_address)/ 32768.0
     if displacement_registers:
         ahrspos_update.vel_x       = AHRSProtocol.decodeProtocol1616Float(curr_data, IMURegisters.NAVX_REG_VEL_X_I_L-first_address)
         ahrspos_update.vel_y       = AHRSProtocol.decodeProtocol1616Float(curr_data, IMURegisters.NAVX_REG_VEL_Y_I_L-first_address)
         ahrspos_update.vel_z       = AHRSProtocol.decodeProtocol1616Float(curr_data, IMURegisters.NAVX_REG_VEL_Z_I_L-first_address)
         ahrspos_update.disp_x      = AHRSProtocol.decodeProtocol1616Float(curr_data, IMURegisters.NAVX_REG_DISP_X_I_L-first_address)
         ahrspos_update.disp_y      = AHRSProtocol.decodeProtocol1616Float(curr_data, IMURegisters.NAVX_REG_DISP_Y_I_L-first_address)
         ahrspos_update.disp_z      = AHRSProtocol.decodeProtocol1616Float(curr_data, IMURegisters.NAVX_REG_DISP_Z_I_L-first_address)            
          
         self.notify_sink._setAHRSPosData(ahrspos_update, sensor_timestamp)
     else:
         self.notify_sink._setAHRSData(ahrspos_update, sensor_timestamp)
     
     board_state = self.board_state
     board_state.cal_status      = curr_data[IMURegisters.NAVX_REG_CAL_STATUS-first_address]
     board_state.op_status       = curr_data[IMURegisters.NAVX_REG_OP_STATUS-first_address]
     board_state.selftest_status = curr_data[IMURegisters.NAVX_REG_SELFTEST_STATUS-first_address]
     board_state.sensor_status   = AHRSProtocol.decodeBinaryUint16(curr_data,IMURegisters.NAVX_REG_SENSOR_STATUS_L-first_address)
     board_state.update_rate_hz  = curr_data[IMURegisters.NAVX_REG_UPDATE_RATE_HZ-first_address]
     board_state.gyro_fsr_dps    = AHRSProtocol.decodeBinaryUint16(curr_data,IMURegisters.NAVX_REG_GYRO_FSR_DPS_L)
     board_state.accel_fsr_g     = curr_data[IMURegisters.NAVX_REG_ACCEL_FSR_G]
     board_state.capability_flags= AHRSProtocol.decodeBinaryUint16(curr_data,IMURegisters.NAVX_REG_CAPABILITY_FLAGS_L-first_address)
     self.notify_sink._setBoardState(board_state)
     
     raw_data_update = self.raw_data_update
     raw_data_update.raw_gyro_x      = AHRSProtocol.decodeBinaryInt16(curr_data,  IMURegisters.NAVX_REG_GYRO_X_L-first_address)
     raw_data_update.raw_gyro_y      = AHRSProtocol.decodeBinaryInt16(curr_data,  IMURegisters.NAVX_REG_GYRO_Y_L-first_address)
     raw_data_update.raw_gyro_z      = AHRSProtocol.decodeBinaryInt16(curr_data,  IMURegisters.NAVX_REG_GYRO_Z_L-first_address)
     raw_data_update.raw_accel_x     = AHRSProtocol.decodeBinaryInt16(curr_data,  IMURegisters.NAVX_REG_ACC_X_L-first_address)
     raw_data_update.raw_accel_y     = AHRSProtocol.decodeBinaryInt16(curr_data,  IMURegisters.NAVX_REG_ACC_Y_L-first_address)
     raw_data_update.raw_accel_z     = AHRSProtocol.decodeBinaryInt16(curr_data,  IMURegisters.NAVX_REG_ACC_Z_L-first_address)
     raw_data_update.cal_mag_x       = AHRSProtocol.decodeBinaryInt16(curr_data,  IMURegisters.NAVX_REG_MAG_X_L-first_address)
     raw_data_update.cal_mag_y       = AHRSProtocol.decodeBinaryInt16(curr_data,  IMURegisters.NAVX_REG_MAG_Y_L-first_address)
     raw_data_update.cal_mag_z       = AHRSProtocol.decodeBinaryInt16(curr_data,  IMURegisters.NAVX_REG_MAG_Z_L-first_address)
     raw_data_update.mpu_temp_c      = ahrspos_update.mpu_temp
     self.notify_sink._setRawData(raw_data_update, sensor_timestamp)
     
     self.last_update_time = Timer.getFPGATimestamp()
     self.byte_count += len(curr_data)
     self.update_count += 1