def test_motor(cfg): import pigpio pgio = pigpio.pi() V = dk.vehicle.Vehicle() from parts import PIGPIO_OUT, PIGPIO_PWM left_in1 = PIGPIO_OUT(pin=cfg.LEFT_MOTOR_IN1_GPIO, pgio=pgio) left_in2 = PIGPIO_OUT(pin=cfg.LEFT_MOTOR_IN2_GPIO, pgio=pgio) left_pwm = PIGPIO_PWM(pin=cfg.LEFT_MOTOR_PWM_GPIO, pgio=pgio, freq=cfg.PWM_FREQ, range=cfg.PWM_RANGE, threshold=cfg.PWM_INPUT_THRESHOLD) right_in1 = PIGPIO_OUT(pin=cfg.RIGHT_MOTOR_IN1_GPIO, pgio=pgio) right_in2 = PIGPIO_OUT(pin=cfg.RIGHT_MOTOR_IN2_GPIO, pgio=pgio) right_pwm = PIGPIO_PWM(pin=cfg.RIGHT_MOTOR_PWM_GPIO, pgio=pgio, freq=cfg.PWM_FREQ, range=cfg.PWM_RANGE, threshold=cfg.PWM_INPUT_THRESHOLD) loop = True while loop: try: print('1, 0, 0.4') left_in1.run(1) left_in2.run(0) left_pwm.run(0.4) right_in1.run(1) right_in2.run(0) right_pwm.run(0.4) sleep(5) print('0, 1, 0.4') left_in1.run(0) left_in2.run(1) left_pwm.run(0.4) right_in1.run(0) right_in2.run(1) right_pwm.run(0.4) sleep(5) except KeyboardInterrupt: loop = False left_in1.run(0) left_in2.run(0) left_pwm.run(0) right_in1.run(0) right_in2.run(0) right_pwm.run(0) pgio.stop() print('stop')
def test_motor(use_debug=True): import pigpio pgio = pigpio.pi() import donkeycar as dk cfg = dk.load_config() V = dk.vehicle.Vehicle() class TestDriver: def __init__(self, debug=False): self.values = [ [1, 0, 0.0], [1, 0, 0.1], [1, 0, 0.2], [1, 0, 0.3], [1, 0, 0.4], [1, 0, 0.5], [1, 0, 0.5], [1, 0, 0.4], [1, 0, 0.3], [1, 0, 0.2], [1, 0, 0.1], [1, 0, 0.0], [0, 1, 0.0], [0, 1, 0.1], [0, 1, 0.2], [0, 1, 0.3], [0, 1, 0.4], [0, 1, 0.5], [0, 1, 0.5], [0, 1, 0.4], [0, 1, 0.3], [0, 1, 0.2], [0, 1, 0.1], [0, 1, 0.0], ] self.debug = debug self.count = 0 def run(self): vals = self.values[self.count] self.count = self.count + 1 if self.count >= len(self.values): self.count = 0 if self.debug: print('in1:{}, in2:{}, pwm:{}'.format(str(vals[0]), str(vals[1]), str(vals[2]))) return vals[0], vals[1], vals[2], vals[0], vals[1], vals[2] def shutdown(self): self.count = 0 self.debug = False V.add(TestDriver(debug=use_debug), outputs=[ 'left_motor_in1', 'left_motor_in2', 'left_motor_vref', 'right_motor_in1', 'right_motor_in2', 'right_motor_vref' ]) from parts import PIGPIO_OUT, PIGPIO_PWM # TB6612 STBY ピン初期化 stby = PIGPIO_OUT(pin=cfg.TB6612_STBY_GPIO, pgio=pgio, debug=use_debug) stby.run(1) # 左モータ制御 left_in1 = PIGPIO_OUT(pin=cfg.LEFT_MOTOR_IN1_GPIO, pgio=pgio, debug=use_debug) left_in2 = PIGPIO_OUT(pin=cfg.LEFT_MOTOR_IN2_GPIO, pgio=pgio, debug=use_debug) left_vref = PIGPIO_PWM(pin=cfg.LEFT_MOTOR_PWM_GPIO, pgio=pgio, freq=cfg.PWM_FREQ, range=cfg.PWM_RANGE, debug=use_debug) V.add(left_in1, inputs=['left_motor_in1']) V.add(left_in2, inputs=['left_motor_in2']) V.add(left_vref, inputs=['left_motor_vref']) # 右モータ制御 right_in1 = PIGPIO_OUT(pin=cfg.RIGHT_MOTOR_IN1_GPIO, pgio=pgio, debug=use_debug) right_in2 = PIGPIO_OUT(pin=cfg.RIGHT_MOTOR_IN2_GPIO, pgio=pgio, debug=use_debug) right_vref = PIGPIO_PWM(pin=cfg.RIGHT_MOTOR_PWM_GPIO, pgio=pgio, freq=cfg.PWM_FREQ, range=cfg.PWM_RANGE, debug=use_debug) V.add(right_in1, inputs=['right_motor_in1']) V.add(right_in2, inputs=['right_motor_in2']) V.add(right_vref, inputs=['right_motor_vref']) try: print('Start driving') #run the vehicle for 20 seconds V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS) except KeyboardInterrupt: print('Halt driving') finally: if stby is not None: stby.run(0) if use_debug: print('Stop TB6612 STBY') if pgio is not None: pgio.stop() if use_debug: print('Stop pigpio controll') print('Stop driving')
def test_pad(cfg): from parts import get_js_controller controller = get_js_controller(cfg) V = dk.vehicle.Vehicle() V.add(controller, outputs=['angle', 'throttle', 'lift_throttle', 'mode', 'recording'], threaded=True) class Prt: def run(self, angle, throttle, lift_throttle, mode, recording): print('angle:{}, throttle:{}, lift:{}, mode:{}, rec:{}'.format( str(angle), str(throttle), str(lift_throttle), str(mode), str(recording))) V.add(Prt(), inputs=['angle', 'throttle', 'lift_throttle', 'mode', 'recording']) from parts import ForkliftMotorDriver motor_driver = ForkliftMotorDriver(debug=False) V.add(motor_driver, inputs=['throttle', 'angle', 'lift_throttle'], outputs=[ 'left_motor_vref', 'left_motor_in1', 'left_motor_in2', 'right_motor_vref', 'right_motor_in1', 'right_motor_in2', 'lift_motor_vref', 'lift_motor_in1', 'lift_motor_in2' ]) class Prt2: def run(self, left_verf, left_in1, left_in2, right_verf, right_in1, right_in2, lift_verf, lift_in1, lift_in2): print('ForkliftMD left verf:{}, in1:{}, in2:{}'.format( str(left_verf), str(left_in1), str(left_in2))) print('ForkliftMD right verf:{}, in1:{}, in2:{}'.format( str(right_verf), str(right_in1), str(right_in2))) print('ForkliftMD lift verf:{}, in1:{}, in2:{}'.format( str(lift_verf), str(lift_in1), str(lift_in2))) V.add(Prt2(), inputs=[ 'left_motor_vref', 'left_motor_in1', 'left_motor_in2', 'right_motor_vref', 'right_motor_in1', 'right_motor_in2', 'lift_motor_vref', 'lift_motor_in1', 'lift_motor_in2' ]) import pigpio pgio = pigpio.pi() from parts import PIGPIO_OUT, PIGPIO_PWM V.add(PIGPIO_OUT(pin=cfg.LEFT_MOTOR_IN1_GPIO, pgio=pgio), inputs=['left_motor_in1']) V.add(PIGPIO_OUT(pin=cfg.LEFT_MOTOR_IN2_GPIO, pgio=pgio), inputs=['left_motor_in2']) V.add(PIGPIO_PWM(pin=cfg.LEFT_MOTOR_PWM_GPIO, pgio=pgio, freq=cfg.PWM_FREQ, range=cfg.PWM_RANGE, threshold=cfg.PWM_INPUT_THRESHOLD), inputs=['left_motor_vref']) V.add(PIGPIO_OUT(pin=cfg.RIGHT_MOTOR_IN1_GPIO, pgio=pgio), inputs=['right_motor_in1']) V.add(PIGPIO_OUT(pin=cfg.RIGHT_MOTOR_IN2_GPIO, pgio=pgio), inputs=['right_motor_in2']) V.add(PIGPIO_PWM(pin=cfg.RIGHT_MOTOR_PWM_GPIO, pgio=pgio, freq=cfg.PWM_FREQ, range=cfg.PWM_RANGE, threshold=cfg.PWM_INPUT_THRESHOLD), inputs=['right_motor_vref']) V.add(PIGPIO_OUT(pin=cfg.LIFT_MOTOR_IN1_GPIO, pgio=pgio), inputs=['lift_motor_in1']) V.add(PIGPIO_OUT(pin=cfg.LIFT_MOTOR_IN2_GPIO, pgio=pgio), inputs=['lift_motor_in2']) V.add(PIGPIO_PWM(pin=cfg.LIFT_MOTOR_PWM_GPIO, pgio=pgio, freq=cfg.PWM_FREQ, range=cfg.PWM_RANGE, threshold=cfg.PWM_INPUT_THRESHOLD), inputs=['lift_motor_vref']) try: V.start(rate_hz=20, max_loop_count=10000) except KeyboardInterrupt: print('exit') finally: pgio.stop()
def drive(cfg, model_path=None, use_joystick=False, use_hedge=False, use_aws=False, use_map=False, use_debug=False, model_type=None, camera_type='single', write_both_images=False, meta=[]): ''' Construct a working robotic vehicle from many parts. Each part runs as a job in the Vehicle loop, calling either it's run or run_threaded method depending on the constructor flag `threaded`. All parts are updated one after another at the framerate given in cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner. Parts may have named outputs and inputs. The framework handles passing named outputs to parts requesting the same named input. ''' if cfg.DONKEY_GYM: #the simulator will use cuda and then we usually run out of resources #if we also try to use cuda. so disable for donkey_gym. os.environ["CUDA_VISIBLE_DEVICES"] = "-1" if model_type is None: if cfg.TRAIN_LOCALIZER: model_type = "localizer" elif cfg.TRAIN_BEHAVIORS: model_type = "behavior" else: model_type = cfg.DEFAULT_MODEL_TYPE # pigpio 初期化 try: import pigpio except: raise pgio = pigpio.pi() #Initialize car V = dk.vehicle.Vehicle() ''' カメラ ''' if camera_type == "stereo": if cfg.CAMERA_TYPE == "WEBCAM": from donkeycar.parts.camera import Webcam camA = Webcam(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, image_d=cfg.IMAGE_DEPTH, iCam=0) camB = Webcam(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, image_d=cfg.IMAGE_DEPTH, iCam=1) elif cfg.CAMERA_TYPE == "CVCAM": from donkeycar.parts.cv import CvCam camA = CvCam(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, image_d=cfg.IMAGE_DEPTH, iCam=0) camB = CvCam(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, image_d=cfg.IMAGE_DEPTH, iCam=1) else: raise (Exception("Unsupported camera type: %s" % cfg.CAMERA_TYPE)) V.add(camA, outputs=['cam/image_array_a'], threaded=True) V.add(camB, outputs=['cam/image_array_b'], threaded=True) from donkeycar.parts.image import StereoPair V.add(StereoPair(), inputs=['cam/image_array_a', 'cam/image_array_b'], outputs=['cam/image_array']) elif write_both_images or (cfg.CAMERA_TYPE != "MAP" and use_map == False): print("cfg.CAMERA_TYPE", cfg.CAMERA_TYPE) if cfg.DONKEY_GYM: from donkeycar.parts.dgym import DonkeyGymEnv inputs = [] threaded = True #print("cfg.CAMERA_TYPE", cfg.CAMERA_TYPE) if cfg.DONKEY_GYM: from donkeycar.parts.dgym import DonkeyGymEnv cam = DonkeyGymEnv(cfg.DONKEY_SIM_PATH, env_name=cfg.DONKEY_GYM_ENV_NAME) threaded = True inputs = ['angle', 'throttle'] elif cfg.CAMERA_TYPE == "PICAM": from donkeycar.parts.camera import PiCamera cam = PiCamera(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, image_d=cfg.IMAGE_DEPTH) elif cfg.CAMERA_TYPE == "WEBCAM": from donkeycar.parts.camera import Webcam cam = Webcam(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, image_d=cfg.IMAGE_DEPTH) elif cfg.CAMERA_TYPE == "CVCAM": from donkeycar.parts.cv import CvCam cam = CvCam(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, image_d=cfg.IMAGE_DEPTH) elif cfg.CAMERA_TYPE == "CSIC": from donkeycar.parts.camera import CSICamera cam = CSICamera(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, image_d=cfg.IMAGE_DEPTH, framerate=cfg.CAMERA_FRAMERATE, gstreamer_flip=cfg.CSIC_CAM_GSTREAMER_FLIP_PARM) elif cfg.CAMERA_TYPE == "V4L": from donkeycar.parts.camera import V4LCamera cam = V4LCamera(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, image_d=cfg.IMAGE_DEPTH, framerate=cfg.CAMERA_FRAMERATE) elif cfg.CAMERA_TYPE == "MOCK": from donkeycar.parts.camera import MockCamera cam = MockCamera(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, image_d=cfg.IMAGE_DEPTH) else: raise (Exception("Unkown camera type: %s" % cfg.CAMERA_TYPE)) # 前方画像を保管するV.memキー名 if write_both_images: from parts.datastore import FWD_CAMERA_KEY outputs = [FWD_CAMERA_KEY] else: outputs = ['cam/image_array'] V.add(cam, inputs=inputs, outputs=outputs, threaded=threaded) ''' Marvelmind 位置情報システム ''' # Marvelmind USNav データ hedge_items = [] hedge_types = [] usnav_items = [ 'usnav/id', 'usnav/x', 'usnav/y', 'usnav/z', 'usnav/angle', 'usnav/timestamp', ] usnav_types = [ 'str', 'float', 'float', 'float', 'float', 'float', ] hedge_items += usnav_items hedge_types += usnav_types # Marvelmind USNav Raw データ usnav_raw_items = [ 'dist/id', 'dist/b1', 'dist/b1d', 'dist/b2', 'dist/b2d', 'dist/b3', 'dist/b3d', 'dist/b4', 'dist/b4d', 'dist/timestamp' ] usnav_raw_types = [ 'str', 'str', 'float', 'str', 'float', 'str', 'float', 'str', 'float', 'float', ] hedge_items += usnav_raw_items hedge_types += usnav_raw_types # Marvelmind IMU データ imu_items = [ 'imu/x', 'imu/y', 'imu/z', 'imu/qw', 'imu/qx', 'imu/qy', 'imu/qz', 'imu/vx', 'imu/vy', 'imu/vz', 'imu/ax', 'imu/ay', 'imu/az', 'imu/gx', 'imu/gy', 'imu/gz', 'imu/mx', 'imu/my', 'imu/mz', 'imu/timestamp', ] imu_types = [ 'float', 'float', 'float', 'float', 'float', 'float', 'float', 'float', 'float', 'float', 'float', 'float', 'float', 'float', 'float', 'float', 'float', 'float', 'float', 'float' ] # Marvelmind 全データ hedge_items += imu_items hedge_types += imu_types # Veichle上の Marvelmind 全データ初期化 for i in range(len(hedge_items)): _item = hedge_items[i] _type = hedge_types[i] if _type == 'str': V.mem[_item] = '0' elif _type == 'float': V.mem[_item] = 0.0 else: raise ValueError('unknown type:{}'.format(_type)) if cfg.HAVE_HEDGE and (use_hedge or cfg.USE_HEDGE_AS_DEFAULT): ''' Marvelmind システムを使用する場合 ''' print('Using Marvelmind Mobile Beacon') # Marvelmind モバイルビーコンパーツ追加 from parts import HedgehogController hedge = HedgehogController(tty=cfg.HEDGE_SERIAL_TTY, adr=cfg.HEDGE_ID) V.add(hedge, outputs=hedge_items) ''' IMU(MPU9250/MPU6050) ''' mpu6050_items = [ 'imu/acl_x', 'imu/acl_y', 'imu/acl_z', 'imu/gyr_x', 'imu/gyr_y', 'imu/gyr_z', 'imu/recent', 'imu/mpu_timestamp', ] mpu6050_types = [ 'float', 'float', 'float', 'float', 'float', 'float', 'str', 'float', ] mpu9250_items = [ 'imu/acl_x', 'imu/acl_y', 'imu/acl_z', 'imu/gyr_x', 'imu/gyr_y', 'imu/gyr_z', 'imu/mgt_x', 'imu/mgt_y', 'imu/mgt_z', 'imu/temp', 'imu/recent', 'imu/mpu_timestamp', ] mpu9250_types = [ 'float', 'float', 'float', 'float', 'float', 'float', 'float', 'float', 'float', 'float', 'str', 'float', ] if cfg.HAVE_IMU: mpu_items = [] mpu_types = [] if cfg.IMU_TYPE == 'mpu6050': ''' MPU6050を使用する場合 ''' mpu_items += mpu6050_items mpu_types += mpu6050_types from parts.sensors.imu import Mpu6050 imu = Mpu6050(pgio=pgio, bus=cfg.MPU6050_I2C_BUS, address=cfg.MPU6050_I2C_ADDRESS, depth=cfg.MPU6050_DEPTH, debug=use_debug) elif cfg.IMU_TYPE == 'mpu9250': ''' MPU9250を使用する場合 ''' mpu_items += mpu9250_items mpu_types += mpu9250_types from parts.sensors.imu import Mpu9250 imu = Mpu9250(pgio=pgio, bus=cfg.MPU9250_I2C_BUS, mpu9250_address=cfg.MPU9250_I2C_ADDRESS, ak8963_address=cfg.AK8963_I2C_ADDRESS, depth=cfg.MPU9250_DEPTH, debug=use_debug) else: raise ValueError('unknown IMU_TYPE = {}'.format(str(cfg.IMU_TYPE))) # Veihcle上のIMUデータを初期化 for i in range(len(mpu_items)): _item = mpu_items[i] _type = mpu_types[i] if _type == 'str': V.mem[_item] = '{}' elif _type == 'float': V.mem[_item] = 0.0 else: raise ValueError('unknown type:{}'.format(_type)) # IMUパーツの追加 V.add(imu, outputs=mpu_items) ''' 2D マップ画像 (usnav/x, usnav/y, imu/recent を使用する) ''' map_items = [ 'usnav/x', 'usnav/y', 'imu/recent', ] map_types = [ 'float', 'float', 'str', ] if use_map or cfg.CAMERA_TYPE == "MAP": ''' 2D マップ画像でカメラの代替とする場合 ''' if cfg.HAVE_HEDGE and (use_hedge or cfg.USE_HEDGE_AS_DEFAULT): ''' Marvelmindが有効である場合 ''' if (cfg.HAVE_IMU and cfg.IMU_TYPE == 'mpu9250'): ''' MPU9250を使用する場合 ''' # 2D マップ生成パーツを追加 from parts import MapImageCreator creator = MapImageCreator( base_image_path=cfg.MAP_BASE_IMAGE_PATH, debug=use_debug) V.add(creator, inputs=map_items, outputs=['cam/image_array']) else: raise ValueError('2D map needs mpu9250 data') else: raise ValueError('2D map needs Marvelmind USNav data') ''' ジョイスティック ''' user_items = [ 'user/angle', 'user/throttle', 'user/lift_throttle', 'user/mode', ] user_types = [ 'float', 'float', 'float', 'str', ] joystick_items = [] joystick_types = [] joystick_items += user_items joystick_types += user_types joystick_items += ['recording'] joystick_types += ['boolean'] # ジョイスティックパーツのアウトプット値なのでここでは初期化せず if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT: ''' ジョイスティックを使用する場合 ''' # フォークリフト用ジョイスティックパーツ取得ファクトリ関数を呼び出す from parts import get_js_controller ctr = get_js_controller(cfg) if cfg.USE_NETWORKED_JS: ''' ネットワーク経由で操作する場合(Donkeycar標準) ''' from donkeycar.parts.controller import JoyStickSub netwkJs = JoyStickSub(cfg.NETWORK_JS_SERVER_IP) V.add(netwkJs, threaded=True) ctr.js = netwkJs # リフト値の追加 V.add(ctr, inputs=['cam/image_array'], outputs=joystick_items, threaded=True) else: ''' Webコントローラを使用する ''' # user/lift_throttle 値が常に0となるWebコントローラパーツを生成 from parts import LocalWebForkliftController ctr = LocalWebForkliftController() V.add(ctr, inputs=['cam/image_array'], outputs=joystick_items, threaded=True) # 変数ctrはこの後でも使用する ''' スロットルフィルタ(ワンタップESC後進) ''' # 入力値はジョイスティックパーツがアウトプット #this throttle filter will allow one tap back for esc reverse th_filter = ThrottleFilter() V.add(th_filter, inputs=['user/throttle'], outputs=['user/throttle']) ''' 手動運転・自動運転判定フラグ設定 ''' #See if we should even run the pilot module. #This is only needed because the part run_condition only accepts boolean class PilotCondition: def run(self, mode): if mode == 'user': return False else: return True V.add(PilotCondition(), inputs=['user/mode'], outputs=['run_pilot']) # run_pilot 値はboolean(偽:手動運転、真:一部もしくは全て自動運転) ''' 3色LED表示 ''' class LedConditionLogic: def __init__(self, cfg): self.cfg = cfg def run(self, mode, recording, recording_alert, behavior_state, model_file_changed, track_loc): #returns a blink rate. 0 for off. -1 for on. positive for rate. if track_loc is not None: led.set_rgb(*self.cfg.LOC_COLORS[track_loc]) return -1 if model_file_changed: led.set_rgb(self.cfg.MODEL_RELOADED_LED_R, self.cfg.MODEL_RELOADED_LED_G, self.cfg.MODEL_RELOADED_LED_B) return 0.1 else: led.set_rgb(self.cfg.LED_R, self.cfg.LED_G, self.cfg.LED_B) if recording_alert: led.set_rgb(*recording_alert) return self.cfg.REC_COUNT_ALERT_BLINK_RATE else: led.set_rgb(self.cfg.LED_R, self.cfg.LED_G, self.cfg.LED_B) if behavior_state is not None and model_type == 'behavior': r, g, b = self.cfg.BEHAVIOR_LED_COLORS[behavior_state] led.set_rgb(r, g, b) return -1 #solid on if recording: return -1 #solid on elif mode == 'user': return 1 elif mode == 'local_angle': return 0.5 elif mode == 'local': return 0.1 return 0 if cfg.HAVE_RGB_LED and not cfg.DONKEY_GYM: #from donkeycar.parts.led_status import RGB_LED #led = RGB_LED(cfg.LED_PIN_R, cfg.LED_PIN_G, cfg.LED_PIN_B, cfg.LED_INVERT) from parts import RGB_LED led = RGB_LED(pgio, cfg.LED_PIN_R, cfg.LED_PIN_G, cfg.LED_PIN_B, cfg.LED_INVERT) led.set_rgb(cfg.LED_R, cfg.LED_G, cfg.LED_B) V.add(LedConditionLogic(cfg), inputs=[ 'user/mode', 'recording', "records/alert", 'behavior/state', 'modelfile/modified', "pilot/loc" ], outputs=['led/blink_rate']) V.add(led, inputs=['led/blink_rate']) def get_record_alert_color(num_records): """ Tubデータ件数を色PWMタプル(r, g, b):各0-100に変換する。 件数範囲、PWMタプルはconfig.py上のRECORD_ALERT_COLOR_ARR を参照している。 引数: num_records Tubデータ件数 戻り値 色PWMタプル (r, g, b)形式(0-100) """ col = (0, 0, 0) for count, color in cfg.RECORD_ALERT_COLOR_ARR: if num_records >= count: col = color return col ''' レコードトラッカ ''' class RecordTracker: def __init__(self): self.last_num_rec_print = 0 self.dur_alert = 0 self.force_alert = 0 def run(self, num_records): if num_records is None: return 0 if self.last_num_rec_print != num_records or self.force_alert: self.last_num_rec_print = num_records if num_records % 10 == 0: print("recorded", num_records, "records") if num_records % cfg.REC_COUNT_ALERT == 0 or self.force_alert: self.dur_alert = num_records // cfg.REC_COUNT_ALERT * cfg.REC_COUNT_ALERT_CYC self.force_alert = 0 if self.dur_alert > 0: self.dur_alert -= 1 if self.dur_alert != 0: return get_record_alert_color(num_records) return 0 rec_tracker_part = RecordTracker() V.add(rec_tracker_part, inputs=["tub/num_records"], outputs=['records/alert']) ''' 自動記録 ''' if cfg.AUTO_RECORD_ON_THROTTLE and isinstance(ctr, JoystickController): """ 自動記録指定されておりかつジョイスティックを使用する場合 """ #then we are not using the circle button. hijack that to force a record count indication def show_record_acount_status(): rec_tracker_part.last_num_rec_print = 0 rec_tracker_part.force_alert = 1 # ジョイスティックのボタンにレコード件数表示用のボタン割当 # F710 if cfg.CONTROLLER_TYPE == 'F710' or cfg.CONTROLLER_TYPE == 'F710_Forklift': ctr.set_button_down_trigger('back', show_record_acount_status) # JC-U3912T elif cfg.CONTROLLER_TYPE == 'JC-U3912T': ctr.set_button_down_trigger('7', show_record_acount_status) # default else: ctr.set_button_down_trigger('circle', show_record_acount_status) ''' Sombero HAT ''' #Sombrero if cfg.HAVE_SOMBRERO: from donkeycar.parts.sombrero import Sombrero _ = Sombrero() # この段階でカメラ or 2Dマップ画像が cam/image_array に格納されている ''' 画像前処理 ''' class ImgPreProcess(): ''' preprocess camera image for inference. normalize and crop if needed. ''' def __init__(self, cfg): self.cfg = cfg def run(self, img_arr): return normalize_and_crop(img_arr, self.cfg) if "coral" in model_type: ''' coral を使用する場合は画像をそのまま使用 ''' inf_input = 'cam/image_array' else: ''' 自動運転時のみ正規化・CROP処理する ''' inf_input = 'cam/normalized/cropped' V.add(ImgPreProcess(cfg), inputs=['cam/image_array'], outputs=[inf_input], run_condition='run_pilot') #Behavioral state if cfg.TRAIN_BEHAVIORS: ''' behaviorモデルを使用する場合 ''' bh = BehaviorPart(cfg.BEHAVIOR_LIST) V.add(bh, outputs=[ 'behavior/state', 'behavior/label', "behavior/one_hot_state_array" ]) try: # L1ボタンにbehavior状態更新を割当 ctr.set_button_down_trigger('L1', bh.increment_state) except: pass inputs = [inf_input, "behavior/one_hot_state_array"] #IMU elif model_type == "imu": ''' IMU(MPU9250/MPU6050)モデル ''' #assert(cfg.HAVE_IMU) #Run the pilot if the mode is not user. if cfg.HAVE_IMU: # 機械学習モデルのインプットにMPU9250/6050の加速度、角速度を追加 inputs = [ 'cam/image_array', 'imu/acl_x', 'imu/acl_y', 'imu/acl_z', 'imu/gyr_x', 'imu/gyr_y', 'imu/gyr_z', ] elif cfg.HAVE_HEDGE and ( use_hedge or cfg.USE_HEDGE_AS_DEFAULT) and cfg.USE_HEDGE_IMU: # 機械学習モデルのインプットにMarvelmindの加速度、角速度を追加 inputs = [ 'cam/image_array', 'imu/ax', 'imu/ay', 'imu/az', 'imu/vx', 'imu/vy', 'imu/vz' ] else: raise ValueError('can not use imu model without imu data') else: # 機械学習モデルのインプットは画像のみ inputs = [inf_input] ''' 機械学習モデル ''' def load_model(kl, model_path): """ 機械学習モデルに学習済みパラメータをロードする。 引数: kl 機械学習モデルオブジェクト model_path 学習済みパラーメータファイルのパス 戻り値: なし """ start = time.time() print('loading model', model_path) kl.load(model_path) print('finished loading in %s sec.' % (str(time.time() - start))) def load_weights(kl, weights_path): """ 機械学習モデルに学習済みパラメータをロードする。 引数: kl 機械学習モデルオブジェクト model_path 学習済みパラーメータファイルのパス 戻り値: なし """ start = time.time() try: print('loading model weights', weights_path) kl.model.load_weights(weights_path) print('finished loading in %s sec.' % (str(time.time() - start))) except Exception as e: print(e) print('ERR>> problems loading weights', weights_path) def load_model_json(kl, json_fnm): start = time.time() print('loading model json', json_fnm) try: from tensorflow.python import keras except: raise try: with open(json_fnm, 'r') as handle: contents = handle.read() kl.model = keras.models.model_from_json(contents) print('finished loading json in %s sec.' % (str(time.time() - start))) except Exception as e: print(e) print("ERR>> problems loading model json", json_fnm) if model_path: #When we have a model, first create an appropriate Keras part kl = dk.utils.get_model_by_type(model_type, cfg) model_reload_cb = None if '.h5' in model_path or '.uff' in model_path or 'tflite' in model_path or '.pkl' in model_path: #when we have a .h5 extension #load everything from the model file load_model(kl, model_path) def reload_model(filename): load_model(kl, filename) model_reload_cb = reload_model elif '.json' in model_path: #when we have a .json extension #load the model from there and look for a matching #.wts file with just weights load_model_json(kl, model_path) weights_path = model_path.replace('.json', '.weights') load_weights(kl, weights_path) def reload_weights(filename): weights_path = filename.replace('.json', '.weights') load_weights(kl, weights_path) model_reload_cb = reload_weights else: print("ERR>> Unknown extension type on model file!!") return ''' モデルファイル更新監視 ''' #this part will signal visual LED, if connected V.add(FileWatcher(model_path, verbose=True), outputs=['modelfile/modified']) #these parts will reload the model file, but only when ai is running so we don't interrupt user driving V.add(FileWatcher(model_path), outputs=['modelfile/dirty'], run_condition="ai_running") V.add(DelayedTrigger(100), inputs=['modelfile/dirty'], outputs=['modelfile/reload'], run_condition="ai_running") V.add(TriggeredCallback(model_path, model_reload_cb), inputs=["modelfile/reload"], run_condition="ai_running") outputs = ['pilot/angle', 'pilot/throttle'] ''' ローカライザモデルの場合の出力編集 ''' if cfg.TRAIN_LOCALIZER: outputs.append("pilot/loc") V.add(kl, inputs=inputs, outputs=outputs, run_condition='run_pilot') ''' モデルがない場合の自動運転結果初期値設定 ''' V.mem['pilot/throttle'] = 0.0 V.mem['pilot/angle'] = 0.0 V.mem['pilot/lift_throttle'] = 0.0 ''' 運転モードから、モータ入力値決定 ''' #Choose what inputs should change the car. class DriveMode: def run(self, mode, user_angle, user_throttle, user_lift_throttle, pilot_angle, pilot_throttle, pilot_lift_throttle): if mode == 'user': return user_angle, user_throttle, user_lift_throttle elif mode == 'local_angle': return pilot_angle, user_throttle, user_lift_throttle else: return pilot_angle, (pilot_throttle * cfg.AI_THROTTLE_MULT), ( pilot_lift_throttle * cfg.AI_THROTTLE_MULT) V.add(DriveMode(), inputs=[ 'user/mode', 'user/angle', 'user/throttle', 'user/lift_throttle', 'pilot/angle', 'pilot/throttle', 'pilot/lift_throttle' ], outputs=['angle', 'throttle', 'lift_throttle']) # Donkeycarへの最終的な指示が angle, throttle, lift_throttle 値となる ''' AIランチャ ''' #to give the car a boost when starting ai mode in a race. aiLauncher = AiLaunch(cfg.AI_LAUNCH_DURATION, cfg.AI_LAUNCH_THROTTLE, cfg.AI_LAUNCH_KEEP_ENABLED) V.add(aiLauncher, inputs=['user/mode', 'throttle'], outputs=['throttle']) if isinstance(ctr, JoystickController): ctr.set_button_down_trigger(cfg.AI_LAUNCH_ENABLE_BUTTON, aiLauncher.enable_ai_launch) ''' AIによる自動運転かどうかの判定 ''' class AiRunCondition: ''' A bool part to let us know when ai is running. ''' def run(self, mode): if mode == "user": return False return True V.add(AiRunCondition(), inputs=['user/mode'], outputs=['ai_running']) ''' AI運転時の記録モード ''' #Ai Recording class AiRecordingCondition: ''' return True when ai mode, otherwize respect user mode recording flag ''' def run(self, mode, recording): if mode == 'user': return recording return True if cfg.RECORD_DURING_AI: V.add(AiRecordingCondition(), inputs=['user/mode', 'recording'], outputs=['recording']) ''' モータ駆動 ''' #Drive train setup if cfg.DONKEY_GYM: pass elif cfg.DRIVE_TRAIN_TYPE == "SERVO_ESC": from donkeycar.parts.actuator import PCA9685, PWMSteering, PWMThrottle steering_controller = PCA9685(cfg.STEERING_CHANNEL, cfg.PCA9685_I2C_ADDR, busnum=cfg.PCA9685_I2C_BUSNUM) steering = PWMSteering(controller=steering_controller, left_pulse=cfg.STEERING_LEFT_PWM, right_pulse=cfg.STEERING_RIGHT_PWM) throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL, cfg.PCA9685_I2C_ADDR, busnum=cfg.PCA9685_I2C_BUSNUM) throttle = PWMThrottle(controller=throttle_controller, max_pulse=cfg.THROTTLE_FORWARD_PWM, zero_pulse=cfg.THROTTLE_STOPPED_PWM, min_pulse=cfg.THROTTLE_REVERSE_PWM) V.add(steering, inputs=['angle']) V.add(throttle, inputs=['throttle']) V.mem['lift_throttle'] = 0 elif cfg.DRIVE_TRAIN_TYPE == "DC_STEER_THROTTLE": from donkeycar.parts.actuator import Mini_HBridge_DC_Motor_PWM steering = Mini_HBridge_DC_Motor_PWM(cfg.HBRIDGE_PIN_LEFT, cfg.HBRIDGE_PIN_RIGHT) throttle = Mini_HBridge_DC_Motor_PWM(cfg.HBRIDGE_PIN_FWD, cfg.HBRIDGE_PIN_BWD) V.add(steering, inputs=['angle']) V.add(throttle, inputs=['throttle']) V.mem['lift_throttle'] = 0 elif cfg.DRIVE_TRAIN_TYPE == "DC_TWO_WHEEL": from donkeycar.parts.actuator import TwoWheelSteeringThrottle, Mini_HBridge_DC_Motor_PWM left_motor = Mini_HBridge_DC_Motor_PWM(cfg.HBRIDGE_PIN_LEFT_FWD, cfg.HBRIDGE_PIN_LEFT_BWD) right_motor = Mini_HBridge_DC_Motor_PWM(cfg.HBRIDGE_PIN_RIGHT_FWD, cfg.HBRIDGE_PIN_RIGHT_BWD) two_wheel_control = TwoWheelSteeringThrottle() V.add(two_wheel_control, inputs=['throttle', 'angle'], outputs=['left_motor_speed', 'right_motor_speed']) V.add(left_motor, inputs=['left_motor_speed']) V.add(right_motor, inputs=['right_motor_speed']) V.mem['lift_motor_throttle'] = 0 # Forklift 駆動モータ操作 elif cfg.DRIVE_TRAIN_TYPE == "THREE_MOTORS_PIGPIO": ''' フォークリフト3モータ駆動 ''' from parts import PIGPIO_OUT, PIGPIO_PWM, ForkliftMotorDriver motor_driver = ForkliftMotorDriver(left_balance=cfg.LEFT_PWM_BALANCE, right_balance=cfg.RIGHT_PWM_BALANCE, debug=False) V.add(motor_driver, inputs=['throttle', 'angle', 'lift_throttle'], outputs=[ 'left_motor_vref', 'left_motor_in1', 'left_motor_in2', 'right_motor_vref', 'right_motor_in1', 'right_motor_in2', 'lift_motor_vref', 'lift_motor_in1', 'lift_motor_in2' ]) if use_debug: class Prt: def run(self, left_vref, left_in1, left_in2, right_vref, right_in1, right_in2, lift_vref, lift_in1, lift_in2): print('ForkliftMD left vref:{}, in1:{}, in2:{}'.format( str(left_vref), str(left_in1), str(left_in2))) print('ForkliftMD right vref:{}, in1:{}, in2:{}'.format( str(right_vref), str(right_in1), str(right_in2))) print('ForkliftMD lift vref:{}, in1:{}, in2:{}'.format( str(lift_vref), str(lift_in1), str(lift_in2))) V.add(Prt(), inputs=[ 'left_motor_vref', 'left_motor_in1', 'left_motor_in2', 'right_motor_vref', 'right_motor_in1', 'right_motor_in2', 'lift_motor_vref', 'lift_motor_in1', 'lift_motor_in2' ]) # TB6612#1 A系(右モータ、左折時駆動する) V.add(PIGPIO_OUT(pin=cfg.LEFT_MOTOR_IN1_GPIO, pgio=pgio), inputs=['left_motor_in1']) V.add(PIGPIO_OUT(pin=cfg.LEFT_MOTOR_IN2_GPIO, pgio=pgio), inputs=['left_motor_in2']) V.add(PIGPIO_PWM(pin=cfg.LEFT_MOTOR_PWM_GPIO, pgio=pgio, freq=cfg.PWM_FREQ, range=cfg.PWM_RANGE, threshold=cfg.PWM_INPUT_THRESHOLD), inputs=['left_motor_vref']) # TB6612#1 B系(左モータ、右折時駆動する) V.add(PIGPIO_OUT(pin=cfg.RIGHT_MOTOR_IN1_GPIO, pgio=pgio), inputs=['right_motor_in1']) V.add(PIGPIO_OUT(pin=cfg.RIGHT_MOTOR_IN2_GPIO, pgio=pgio), inputs=['right_motor_in2']) V.add(PIGPIO_PWM(pin=cfg.RIGHT_MOTOR_PWM_GPIO, pgio=pgio, freq=cfg.PWM_FREQ, range=cfg.PWM_RANGE, threshold=cfg.PWM_INPUT_THRESHOLD), inputs=['right_motor_vref']) # TB6612#2 A系(リフトモータ) V.add(PIGPIO_OUT(pin=cfg.LIFT_MOTOR_IN1_GPIO, pgio=pgio), inputs=['lift_motor_in1']) V.add(PIGPIO_OUT(pin=cfg.LIFT_MOTOR_IN2_GPIO, pgio=pgio), inputs=['lift_motor_in2']) V.add(PIGPIO_PWM(pin=cfg.LIFT_MOTOR_PWM_GPIO, pgio=pgio, freq=cfg.PWM_FREQ, range=cfg.PWM_RANGE, threshold=cfg.PWM_INPUT_THRESHOLD), inputs=['lift_motor_vref']) elif cfg.DRIVE_TRAIN_TYPE == "SERVO_HBRIDGE_PWM": from donkeycar.parts.actuator import ServoBlaster, PWMSteering steering_controller = ServoBlaster(cfg.STEERING_CHANNEL) #really pin #PWM pulse values should be in the range of 100 to 200 assert (cfg.STEERING_LEFT_PWM <= 200) assert (cfg.STEERING_RIGHT_PWM <= 200) steering = PWMSteering(controller=steering_controller, left_pulse=cfg.STEERING_LEFT_PWM, right_pulse=cfg.STEERING_RIGHT_PWM) from donkeycar.parts.actuator import Mini_HBridge_DC_Motor_PWM motor = Mini_HBridge_DC_Motor_PWM(cfg.HBRIDGE_PIN_FWD, cfg.HBRIDGE_PIN_BWD) V.add(steering, inputs=['angle']) V.add(motor, inputs=["throttle"]) V.mem['lift_throttle'] = 0 ''' Tubデータ ''' # ベースとなるTubデータ if write_both_images: from parts.datastore import FWD_CAMERA_KEY inputs = ['cam/image_array', FWD_CAMERA_KEY] types = ['image_array', 'image_array'] else: inputs = ['cam/image_array'] types = ['image_array'] inputs += user_items types += user_types if cfg.TRAIN_BEHAVIORS: ''' behaviorモデルを使用する場合の入力データを追加 ''' inputs += [ 'behavior/state', 'behavior/label', 'behavior/one_hot_state_array' ] types += ['int', 'str', 'vector'] ''' IMU(MPU6050)データ追加 ''' # Tubへ格納する加速度、角速度 tub_imu_inputs = [ 'imu/acl_x', 'imu/acl_y', 'imu/acl_z', 'imu/gyr_x', 'imu/gyr_y', 'imu/gyr_z', ] tub_imu_input_types = [ 'float', 'float', 'float', 'float', 'float', 'float', ] if not cfg.HAVE_IMU: ''' MPU9250/MPU6050を持っていない場合 ''' if cfg.HAVE_HEDGE and (use_hedge or cfg.USE_HEDGE_AS_DEFAULT): ''' Marvelmind が有効な場合 ''' if cfg.USE_HEDGE_IMU: ''' Marvelmind IMUが使用可能な場合 ''' # 必要な Marvelmind IMUデータを移動させる class MoveIMU: def run(self, ax, ay, az, gx, gy, gz): return ax, ay, az, gx, gy, gz move = MoveIMU() V.add(move, inputs=[ 'imu/ax', 'imu/ay', 'imu/az', 'imu/gx', 'imu/gy', 'imu/gz', ], outputs=tub_imu_inputs) inputs += tub_imu_inputs types += tub_imu_input_types else: inputs += tub_imu_inputs types += tub_imu_input_types if cfg.RECORD_DURING_AI: ''' 自動運転中であっても記録しておくモードの場合 ''' # pilot/* を Tub データに加える inputs += ['pilot/angle', 'pilot/throttle', 'pilot/lift_throttle'] types += ['float', 'float', 'float'] ''' Tubデータ書き込み ''' th = TubHandler(path=cfg.DATA_PATH) if write_both_images: from parts.datastore import TubHandler as NewTubHandler th = NewTubHandler(path=cfg.DATA_PATH) tub = th.new_tub_writer(inputs=inputs, types=types, user_meta=meta) V.add(tub, inputs=inputs, outputs=["tub/num_records"], run_condition='recording') if cfg.PUB_CAMERA_IMAGES: ''' カメライメージを通信して貰う場合(Donkeycar標準) ''' from donkeycar.parts.network import TCPServeValue from donkeycar.parts.image import ImgArrToJpg pub = TCPServeValue("camera") V.add(ImgArrToJpg(), inputs=['cam/image_array'], outputs=['jpg/bin']) V.add(pub, inputs=['jpg/bin']) ''' 操縦系統別 ''' if type(ctr) is LocalWebController: ''' LocalWebController を使用している場合の usage を表示 ''' print("You can now go to <your pi ip address>:8887 to drive your car.") elif isinstance(ctr, JoystickController): ''' ジョイスティックを使用している場合の usage を表示 ''' print("You can now move your joystick to drive your car.") #tell the controller about the tub ctr.set_tub(tub) if cfg.BUTTON_PRESS_NEW_TUB: ''' cfg.BUTTON_PRESS_NEW_TUB が True の場合、ボタンを押すごとに別のTubディレクトリに データが格納されるようになる(デフォルト:False)。 ''' def new_tub_dir(): V.parts.pop() tub = th.new_tub_writer(inputs=inputs, types=types, user_meta=meta) V.add(tub, inputs=inputs, outputs=["tub/num_records"], run_condition='recording') ctr.set_tub(tub) # ボタン割当 # F710 if cfg.CONTROLLER_TYPE == 'F710' or cfg.CONTROLLER_TYPE == 'F710_Forklift': ctr.set_button_down_trigger('A', new_tub_dir) # JC-U3912T elif cfg.CONTROLLER_TYPE == 'JC-U3912T' or cfg.CONTROLLER_TYPE == 'JC-U3912T_Forklift': ctr.set_button_down_trigger('12', new_tub_dir) # default else: ctr.set_button_down_trigger('cross', new_tub_dir) # 現時点のボタン割当状態を表示 ctr.print_controls() """ AWS IoT Core """ if use_aws or cfg.USE_AWS_AS_DEFAULT: print('Start aws configuration') from parts.broker import AWSShadowClientFactory, PowerReporter factory = AWSShadowClientFactory(cfg.AWS_CONFIG_PATH, cfg.AWS_THING_NAME) # Power ON 情報の送信 power = PowerReporter(factory, debug=use_debug) power.on() # Tubデータ(json)送信 from parts.broker.pub import Publisher pub_tub = Publisher(factory, debug=use_debug) V.add(pub_tub, inputs=[ 'user/angle', 'user/throttle', 'user/lift_throttle', 'pilot/angle', 'pilot/throttle', 'pilot/lift_throttle', 'user/mode' ]) # Tubデータ(イメージ)送信 from parts.broker.pub import ImagePublisher pub_img = ImagePublisher(factory, debug=use_debug) V.add(pub_img, inputs=[ 'cam/image_array', ]) # fwdイメージデータ送信 if write_both_images: from parts.broker.pub import FwdImagePublisher pub_fwd_img = FwdImagePublisher(factory, debug=use_debug) from parts.datastore import FWD_CAMERA_KEY V.add(pub_fwd_img, inputs=[ FWD_CAMERA_KEY, ]) ''' ジョイスティックデータの送信 ''' if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT: ''' ジョイスティックを使用している場合 ''' from parts.broker.pub import JoystickPublisher pub_joy = JoystickPublisher(factory, debug=use_debug) V.add(pub_joy, inputs=joystick_items) ''' Marvelmind システムデータの送信 ''' if cfg.HAVE_HEDGE and (use_hedge or cfg.USE_HEDGE_AS_DEFAULT): ''' Marvelmind システムを使用している場合 ''' if cfg.USE_HEDGE_USNAV: ''' Marvelmind 位置情報を使用している場合 ''' from parts.broker.pub import USNavPublisher pub_usn = USNavPublisher(factory, debug=use_debug) V.add(pub_usn, inputs=usnav_items) if cfg.USE_HEDGE_USNAV_RAW: ''' Marvelmind 距離情報を使用している場合 ''' from parts.broker.pub import USNavRawPublisher pub_raw = USNavRawPublisher(factory, debug=use_debug) V.add(pub_raw, inputs=usnav_raw_items) if cfg.USE_HEDGE_IMU: ''' Marvelmind IMU情報を使用している場合 ''' from parts.broker.pub import IMUPublisher pub_imu = IMUPublisher(factory, debug=use_debug) V.add(pub_imu, inputs=imu_items) ''' MPU9250/MPU6050 データの送信 ''' if cfg.HAVE_IMU: ''' MPU9250/MPU6050 を使用している場合 ''' if cfg.IMU_TYPE == 'mpu6050': ''' MPU6050を使用している場合 ''' from parts.broker.pub import Mpu6050Publisher pub_mpu = Mpu6050Publisher(factory, debug=use_debug) V.add(pub_mpu, inputs=mpu_items) elif cfg.IMU_TYPE == 'mpu9250': ''' MPU6050を使用している場合 ''' from parts.broker.pub import Mpu9250Publisher pub_mpu = Mpu9250Publisher(factory, debug=use_debug) V.add(pub_mpu, inputs=mpu_items) ''' 運転ループ ''' try: print('Start running') #run the vehicle for 20 seconds V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS) except KeyboardInterrupt: # Ctrl+C押下時 pass finally: # デバッグ if use_debug: print('Stop running') # pigpio 利用停止 pgio.stop() if use_aws or cfg.USE_AWS_AS_DEFAULT: if power is not None: # Power Off 情報の送信 if use_debug: print('Sending power off') power.off() # 送信するまで待機 time.sleep(1) print('Stopped')
def drive_straight(cfg, pgio, left_balance=1.0, right_balance=1.0): from parts import PIGPIO_OUT, PIGPIO_PWM left_in1 = PIGPIO_OUT(pin=cfg.LEFT_MOTOR_IN1_GPIO, pgio=pgio) left_in2 = PIGPIO_OUT(pin=cfg.LEFT_MOTOR_IN2_GPIO, pgio=pgio) left_pwm = PIGPIO_PWM(pin=cfg.LEFT_MOTOR_PWM_GPIO, pgio=pgio, freq=cfg.PWM_FREQ, range=cfg.PWM_RANGE, threshold=cfg.PWM_INPUT_THRESHOLD) right_in1 = PIGPIO_OUT(pin=cfg.RIGHT_MOTOR_IN1_GPIO, pgio=pgio) right_in2 = PIGPIO_OUT(pin=cfg.RIGHT_MOTOR_IN2_GPIO, pgio=pgio) right_pwm = PIGPIO_PWM(pin=cfg.RIGHT_MOTOR_PWM_GPIO, pgio=pgio, freq=cfg.PWM_FREQ, range=cfg.PWM_RANGE, threshold=cfg.PWM_INPUT_THRESHOLD) from parts import ForkliftMotorDriver driver = ForkliftMotorDriver(left_balance=left_balance, right_balance=right_balance) left_pwm_v, left_in1_v, left_in2_v, right_pwm_v, right_in1_v, right_in2_v, _, _, _ = driver.run( 0.5, 0.0, 0.0) print('drive start') left_in1.run(left_in1_v) left_in2.run(left_in2_v) right_in1.run(right_in1_v) right_in2.run(right_in2_v) left_pwm.run(left_pwm_v) right_pwm.run(right_pwm_v) sleep(3) left_pwm.run(0) right_pwm.run(0) print('drive end')
def test_motor(): import pigpio pgio = pigpio.pi() from parts import PIGPIO_OUT, PIGPIO_PWM right_in1 = PIGPIO_OUT(pin=RIGHT_MOTOR_IN1_GPIO, pgio=pgio) right_in2 = PIGPIO_OUT(pin=RIGHT_MOTOR_IN2_GPIO, pgio=pgio) right_vref = PIGPIO_PWM(pin=RIGHT_MOTOR_PWM_GPIO, pgio=pgio, freq=PWM_FREQ, range=PWM_RANGE) right_in1.run(1.0) right_in2.run(0.0) right_vref.run(0.5) print('right fwd') sleep(1) right_in1.run(0.0) right_in2.run(0.0) right_vref.run(0.0) print('right stop') sleep(3) left_in1 = PIGPIO_OUT(pin=LEFT_MOTOR_IN1_GPIO, pgio=pgio) left_in2 = PIGPIO_OUT(pin=LEFT_MOTOR_IN2_GPIO, pgio=pgio) left_vref = PIGPIO_PWM(pin=LEFT_MOTOR_PWM_GPIO, pgio=pgio, freq=PWM_FREQ, range=PWM_RANGE) left_in1.run(1.0) left_in2.run(0.0) left_vref.run(1.0) print('left fwd') sleep(1) left_in1.run(0.0) left_in2.run(0.0) left_vref.run(0.0) print('left stop') sleep(3) lift_in1 = PIGPIO_OUT(pin=LIFT_MOTOR_IN1_GPIO, pgio=pgio) lift_in2 = PIGPIO_OUT(pin=LIFT_MOTOR_IN2_GPIO, pgio=pgio) lift_vref = PIGPIO_PWM(pin=LIFT_MOTOR_PWM_GPIO, pgio=pgio, freq=PWM_FREQ, range=PWM_RANGE) lift_in1.run(1.0) lift_in2.run(0.0) lift_vref.run(1.0) print('lift fwd') sleep(2) lift_in1.run(0.0) lift_in2.run(0.0) lift_vref.run(0.0) print('lift stop') #sleep(3) pgio.stop()
def test_lift(): import donkeycar as dk V = dk.vehicle.Vehicle() import pigpio pgio = pigpio.pi() from parts import PIGPIO_OUT, PIGPIO_PWM, ForkliftMotorDriver #, CalibrateAngle left_in1 = PIGPIO_OUT(pin=LEFT_MOTOR_IN1_GPIO, pgio=pgio) left_in2 = PIGPIO_OUT(pin=LEFT_MOTOR_IN2_GPIO, pgio=pgio) left_vref = PIGPIO_PWM(pin=LEFT_MOTOR_PWM_GPIO, pgio=pgio, freq=PWM_FREQ, range=PWM_RANGE) right_in1 = PIGPIO_OUT(pin=RIGHT_MOTOR_IN1_GPIO, pgio=pgio) right_in2 = PIGPIO_OUT(pin=RIGHT_MOTOR_IN2_GPIO, pgio=pgio) right_vref = PIGPIO_PWM(pin=RIGHT_MOTOR_PWM_GPIO, pgio=pgio, freq=PWM_FREQ, range=PWM_RANGE) lift_in1 = PIGPIO_OUT(pin=LIFT_MOTOR_IN1_GPIO, pgio=pgio) lift_in2 = PIGPIO_OUT(pin=LIFT_MOTOR_IN2_GPIO, pgio=pgio) lift_vref = PIGPIO_PWM(pin=LIFT_MOTOR_PWM_GPIO, pgio=pgio, freq=PWM_FREQ, range=PWM_RANGE) V.add(ThrottleTest(), outputs=['throttle', 'steering', 'lift_throttle']) #V.add(CalibrateAngle(diff=0.00, debug=False), inputs=['steering'], outputs=['steering']) #V.mem['dummy_gpio'] = 0 driver = ForkliftMotorDriver(debug=True) V.add(driver, inputs=['throttle', 'steering', 'lift_throttle'], outputs=[ 'left_motor_vref', 'left_motor_in1', 'left_motor_in2', 'right_motor_vref', 'right_motor_in1', 'right_motor_in2', 'lift_motor_vref', 'lift_motor_in1', 'lift_motor_in2' ]) V.add(lift_in1, inputs=['lift_motor_in1']) V.add(lift_in2, inputs=['lift_motor_in2']) V.add(lift_vref, inputs=['lift_motor_vref']) V.add(left_in1, inputs=['left_motor_in1']) V.add(left_in2, inputs=['left_motor_in2']) V.add(left_vref, inputs=['left_motor_vref']) V.add(right_in1, inputs=['right_motor_in1']) V.add(right_in2, inputs=['right_motor_in2']) V.add(right_vref, inputs=['right_motor_vref']) try: #V.mem['lift_throttle'] = 0.0 V.start(rate_hz=20, max_loop_count=2000) finally: V.mem['throttle'] = 0.0 V.mem['lift_throttle'] = 0.0 sleep(1) pgio.stop() print('test stop')
def drive(cfg, model_path=None, use_joystick=False, model_type=None, camera_type='single', use_debug=False, meta=[] ): ''' Construct a working robotic vehicle from many parts. Each part runs as a job in the Vehicle loop, calling either it's run or run_threaded method depending on the constructor flag `threaded`. All parts are updated one after another at the framerate given in cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner. Parts may have named outputs and inputs. The framework handles passing named outputs to parts requesting the same named input. ''' if cfg.DONKEY_GYM: #the simulator will use cuda and then we usually run out of resources #if we also try to use cuda. so disable for donkey_gym. os.environ["CUDA_VISIBLE_DEVICES"]="-1" if model_type is None: if cfg.TRAIN_LOCALIZER: model_type = "localizer" elif cfg.TRAIN_BEHAVIORS: model_type = "behavior" else: model_type = cfg.DEFAULT_MODEL_TYPE #Initialize car V = dk.vehicle.Vehicle() if camera_type == "stereo": if cfg.CAMERA_TYPE == "WEBCAM": from donkeycar.parts.camera import Webcam camA = Webcam(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, image_d=cfg.IMAGE_DEPTH, iCam = 0) camB = Webcam(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, image_d=cfg.IMAGE_DEPTH, iCam = 1) elif cfg.CAMERA_TYPE == "CVCAM": from donkeycar.parts.cv import CvCam camA = CvCam(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, image_d=cfg.IMAGE_DEPTH, iCam = 0) camB = CvCam(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, image_d=cfg.IMAGE_DEPTH, iCam = 1) else: raise(Exception("Unsupported camera type: %s" % cfg.CAMERA_TYPE)) V.add(camA, outputs=['cam/image_array_a'], threaded=True) V.add(camB, outputs=['cam/image_array_b'], threaded=True) from donkeycar.parts.image import StereoPair V.add(StereoPair(), inputs=['cam/image_array_a', 'cam/image_array_b'], outputs=['cam/image_array']) else: print("cfg.CAMERA_TYPE", cfg.CAMERA_TYPE) if cfg.DONKEY_GYM: from donkeycar.parts.dgym import DonkeyGymEnv inputs = [] threaded = True print("cfg.CAMERA_TYPE", cfg.CAMERA_TYPE) if cfg.DONKEY_GYM: from donkeycar.parts.dgym import DonkeyGymEnv cam = DonkeyGymEnv(cfg.DONKEY_SIM_PATH, env_name=cfg.DONKEY_GYM_ENV_NAME) threaded = True inputs = ['angle', 'throttle'] elif cfg.CAMERA_TYPE == "PICAM": from donkeycar.parts.camera import PiCamera cam = PiCamera(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, image_d=cfg.IMAGE_DEPTH) elif cfg.CAMERA_TYPE == "WEBCAM": from donkeycar.parts.camera import Webcam cam = Webcam(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, image_d=cfg.IMAGE_DEPTH) elif cfg.CAMERA_TYPE == "CVCAM": from donkeycar.parts.cv import CvCam cam = CvCam(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, image_d=cfg.IMAGE_DEPTH) elif cfg.CAMERA_TYPE == "CSIC": from donkeycar.parts.camera import CSICamera cam = CSICamera(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, image_d=cfg.IMAGE_DEPTH, framerate=cfg.CAMERA_FRAMERATE, gstreamer_flip=cfg.CSIC_CAM_GSTREAMER_FLIP_PARM) elif cfg.CAMERA_TYPE == "V4L": from donkeycar.parts.camera import V4LCamera cam = V4LCamera(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, image_d=cfg.IMAGE_DEPTH, framerate=cfg.CAMERA_FRAMERATE) elif cfg.CAMERA_TYPE == "MOCK": from donkeycar.parts.camera import MockCamera cam = MockCamera(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, image_d=cfg.IMAGE_DEPTH) else: raise(Exception("Unkown camera type: %s" % cfg.CAMERA_TYPE)) V.add(cam, inputs=inputs, outputs=['cam/image_array'], threaded=threaded) if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT: #modify max_throttle closer to 1.0 to have more power #modify steering_scale lower than 1.0 to have less responsive steering # 独自のファクトリ関数に変更 #from donkeycar.parts.controller import get_js_controller from parts import get_js_controller ctr = get_js_controller(cfg) if cfg.USE_NETWORKED_JS: from donkeycar.parts.controller import JoyStickSub netwkJs = JoyStickSub(cfg.NETWORK_JS_SERVER_IP) V.add(netwkJs, threaded=True) ctr.js = netwkJs else: #This web controller will create a web server that is capable #of managing steering, throttle, and modes, and more. ctr = LocalWebController() V.add(ctr, inputs=['cam/image_array'], outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'], threaded=True) #this throttle filter will allow one tap back for esc reverse th_filter = ThrottleFilter() V.add(th_filter, inputs=['user/throttle'], outputs=['user/throttle']) #See if we should even run the pilot module. #This is only needed because the part run_condition only accepts boolean class PilotCondition: def run(self, mode): if mode == 'user': return False else: return True V.add(PilotCondition(), inputs=['user/mode'], outputs=['run_pilot']) class LedConditionLogic: def __init__(self, cfg): self.cfg = cfg def run(self, mode, recording, recording_alert, behavior_state, model_file_changed, track_loc): #returns a blink rate. 0 for off. -1 for on. positive for rate. if track_loc is not None: led.set_rgb(*self.cfg.LOC_COLORS[track_loc]) return -1 if model_file_changed: led.set_rgb(self.cfg.MODEL_RELOADED_LED_R, self.cfg.MODEL_RELOADED_LED_G, self.cfg.MODEL_RELOADED_LED_B) return 0.1 else: led.set_rgb(self.cfg.LED_R, self.cfg.LED_G, self.cfg.LED_B) if recording_alert: led.set_rgb(*recording_alert) return self.cfg.REC_COUNT_ALERT_BLINK_RATE else: led.set_rgb(self.cfg.LED_R, self.cfg.LED_G, self.cfg.LED_B) if behavior_state is not None and model_type == 'behavior': r, g, b = self.cfg.BEHAVIOR_LED_COLORS[behavior_state] led.set_rgb(r, g, b) return -1 #solid on if recording: return -1 #solid on elif mode == 'user': return 1 elif mode == 'local_angle': return 0.5 elif mode == 'local': return 0.1 return 0 if cfg.HAVE_RGB_LED and not cfg.DONKEY_GYM: from donkeycar.parts.led_status import RGB_LED led = RGB_LED(cfg.LED_PIN_R, cfg.LED_PIN_G, cfg.LED_PIN_B, cfg.LED_INVERT) led.set_rgb(cfg.LED_R, cfg.LED_G, cfg.LED_B) V.add(LedConditionLogic(cfg), inputs=['user/mode', 'recording', "records/alert", 'behavior/state', 'modelfile/modified', "pilot/loc"], outputs=['led/blink_rate']) V.add(led, inputs=['led/blink_rate']) def get_record_alert_color(num_records): col = (0, 0, 0) for count, color in cfg.RECORD_ALERT_COLOR_ARR: if num_records >= count: col = color return col class RecordTracker: def __init__(self): self.last_num_rec_print = 0 self.dur_alert = 0 self.force_alert = 0 def run(self, num_records): if num_records is None: return 0 if self.last_num_rec_print != num_records or self.force_alert: self.last_num_rec_print = num_records if num_records % 10 == 0: print("recorded", num_records, "records") if num_records % cfg.REC_COUNT_ALERT == 0 or self.force_alert: self.dur_alert = num_records // cfg.REC_COUNT_ALERT * cfg.REC_COUNT_ALERT_CYC self.force_alert = 0 if self.dur_alert > 0: self.dur_alert -= 1 if self.dur_alert != 0: return get_record_alert_color(num_records) return 0 rec_tracker_part = RecordTracker() V.add(rec_tracker_part, inputs=["tub/num_records"], outputs=['records/alert']) if cfg.AUTO_RECORD_ON_THROTTLE and isinstance(ctr, JoystickController): #then we are not using the circle button. hijack that to force a record count indication def show_record_acount_status(): rec_tracker_part.last_num_rec_print = 0 rec_tracker_part.force_alert = 1 ctr.set_button_down_trigger('circle', show_record_acount_status) #Sombrero if cfg.HAVE_SOMBRERO: from donkeycar.parts.sombrero import Sombrero s = Sombrero() #IMU if cfg.HAVE_IMU: from donkeycar.parts.imu import Mpu6050 imu = Mpu6050() V.add(imu, outputs=['imu/acl_x', 'imu/acl_y', 'imu/acl_z', 'imu/gyr_x', 'imu/gyr_y', 'imu/gyr_z'], threaded=True) class ImgPreProcess(): ''' preprocess camera image for inference. normalize and crop if needed. ''' def __init__(self, cfg): self.cfg = cfg def run(self, img_arr): return normalize_and_crop(img_arr, self.cfg) if "coral" in model_type: inf_input = 'cam/image_array' else: inf_input = 'cam/normalized/cropped' V.add(ImgPreProcess(cfg), inputs=['cam/image_array'], outputs=[inf_input], run_condition='run_pilot') #Behavioral state if cfg.TRAIN_BEHAVIORS: bh = BehaviorPart(cfg.BEHAVIOR_LIST) V.add(bh, outputs=['behavior/state', 'behavior/label', "behavior/one_hot_state_array"]) try: ctr.set_button_down_trigger('L1', bh.increment_state) except: pass inputs = [inf_input, "behavior/one_hot_state_array"] #IMU elif model_type == "imu": assert(cfg.HAVE_IMU) #Run the pilot if the mode is not user. inputs=[inf_input, 'imu/acl_x', 'imu/acl_y', 'imu/acl_z', 'imu/gyr_x', 'imu/gyr_y', 'imu/gyr_z'] else: inputs=[inf_input] def load_model(kl, model_path): start = time.time() print('loading model', model_path) kl.load(model_path) print('finished loading in %s sec.' % (str(time.time() - start)) ) def load_weights(kl, weights_path): start = time.time() try: print('loading model weights', weights_path) kl.model.load_weights(weights_path) print('finished loading in %s sec.' % (str(time.time() - start)) ) except Exception as e: print(e) print('ERR>> problems loading weights', weights_path) def load_model_json(kl, json_fnm): start = time.time() print('loading model json', json_fnm) try: from tensorflow.python import keras except: raise try: with open(json_fnm, 'r') as handle: contents = handle.read() kl.model = keras.models.model_from_json(contents) print('finished loading json in %s sec.' % (str(time.time() - start)) ) except Exception as e: print(e) print("ERR>> problems loading model json", json_fnm) if model_path: #When we have a model, first create an appropriate Keras part kl = dk.utils.get_model_by_type(model_type, cfg) model_reload_cb = None if '.h5' in model_path or '.uff' in model_path or 'tflite' in model_path or '.pkl' in model_path: #when we have a .h5 extension #load everything from the model file load_model(kl, model_path) def reload_model(filename): load_model(kl, filename) model_reload_cb = reload_model elif '.json' in model_path: #when we have a .json extension #load the model from there and look for a matching #.wts file with just weights load_model_json(kl, model_path) weights_path = model_path.replace('.json', '.weights') load_weights(kl, weights_path) def reload_weights(filename): weights_path = filename.replace('.json', '.weights') load_weights(kl, weights_path) model_reload_cb = reload_weights else: print("ERR>> Unknown extension type on model file!!") return #this part will signal visual LED, if connected V.add(FileWatcher(model_path, verbose=True), outputs=['modelfile/modified']) #these parts will reload the model file, but only when ai is running so we don't interrupt user driving V.add(FileWatcher(model_path), outputs=['modelfile/dirty'], run_condition="ai_running") V.add(DelayedTrigger(100), inputs=['modelfile/dirty'], outputs=['modelfile/reload'], run_condition="ai_running") V.add(TriggeredCallback(model_path, model_reload_cb), inputs=["modelfile/reload"], run_condition="ai_running") outputs=['pilot/angle', 'pilot/throttle'] if cfg.TRAIN_LOCALIZER: outputs.append("pilot/loc") V.add(kl, inputs=inputs, outputs=outputs, run_condition='run_pilot') #Choose what inputs should change the car. class DriveMode: def run(self, mode, user_angle, user_throttle, pilot_angle, pilot_throttle): if mode == 'user': return user_angle, user_throttle elif mode == 'local_angle': return pilot_angle, user_throttle else: return pilot_angle, pilot_throttle * cfg.AI_THROTTLE_MULT V.add(DriveMode(), inputs=['user/mode', 'user/angle', 'user/throttle', 'pilot/angle', 'pilot/throttle'], outputs=['angle', 'throttle']) #to give the car a boost when starting ai mode in a race. aiLauncher = AiLaunch(cfg.AI_LAUNCH_DURATION, cfg.AI_LAUNCH_THROTTLE, cfg.AI_LAUNCH_KEEP_ENABLED) V.add(aiLauncher, inputs=['user/mode', 'throttle'], outputs=['throttle']) if isinstance(ctr, JoystickController): ctr.set_button_down_trigger(cfg.AI_LAUNCH_ENABLE_BUTTON, aiLauncher.enable_ai_launch) class AiRunCondition: ''' A bool part to let us know when ai is running. ''' def run(self, mode): if mode == "user": return False return True V.add(AiRunCondition(), inputs=['user/mode'], outputs=['ai_running']) #Ai Recording class AiRecordingCondition: ''' return True when ai mode, otherwize respect user mode recording flag ''' def run(self, mode, recording): if mode == 'user': return recording return True if cfg.RECORD_DURING_AI: V.add(AiRecordingCondition(), inputs=['user/mode', 'recording'], outputs=['recording']) #Drive train setup if cfg.DONKEY_GYM: pass elif cfg.DRIVE_TRAIN_TYPE == "SERVO_ESC": from donkeycar.parts.actuator import PCA9685, PWMSteering, PWMThrottle steering_controller = PCA9685(cfg.STEERING_CHANNEL, cfg.PCA9685_I2C_ADDR, busnum=cfg.PCA9685_I2C_BUSNUM) steering = PWMSteering(controller=steering_controller, left_pulse=cfg.STEERING_LEFT_PWM, right_pulse=cfg.STEERING_RIGHT_PWM) throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL, cfg.PCA9685_I2C_ADDR, busnum=cfg.PCA9685_I2C_BUSNUM) throttle = PWMThrottle(controller=throttle_controller, max_pulse=cfg.THROTTLE_FORWARD_PWM, zero_pulse=cfg.THROTTLE_STOPPED_PWM, min_pulse=cfg.THROTTLE_REVERSE_PWM) V.add(steering, inputs=['angle']) V.add(throttle, inputs=['throttle']) elif cfg.DRIVE_TRAIN_TYPE == "DC_STEER_THROTTLE": from donkeycar.parts.actuator import Mini_HBridge_DC_Motor_PWM steering = Mini_HBridge_DC_Motor_PWM(cfg.HBRIDGE_PIN_LEFT, cfg.HBRIDGE_PIN_RIGHT) throttle = Mini_HBridge_DC_Motor_PWM(cfg.HBRIDGE_PIN_FWD, cfg.HBRIDGE_PIN_BWD) V.add(steering, inputs=['angle']) V.add(throttle, inputs=['throttle']) elif cfg.DRIVE_TRAIN_TYPE == "DC_TWO_WHEEL": from donkeycar.parts.actuator import TwoWheelSteeringThrottle, Mini_HBridge_DC_Motor_PWM left_motor = Mini_HBridge_DC_Motor_PWM(cfg.HBRIDGE_PIN_LEFT_FWD, cfg.HBRIDGE_PIN_LEFT_BWD) right_motor = Mini_HBridge_DC_Motor_PWM(cfg.HBRIDGE_PIN_RIGHT_FWD, cfg.HBRIDGE_PIN_RIGHT_BWD) two_wheel_control = TwoWheelSteeringThrottle() V.add(two_wheel_control, inputs=['throttle', 'angle'], outputs=['left_motor_speed', 'right_motor_speed']) V.add(left_motor, inputs=['left_motor_speed']) V.add(right_motor, inputs=['right_motor_speed']) elif cfg.DRIVE_TRAIN_TYPE == "SERVO_HBRIDGE_PWM": from donkeycar.parts.actuator import ServoBlaster, PWMSteering steering_controller = ServoBlaster(cfg.STEERING_CHANNEL) #really pin #PWM pulse values should be in the range of 100 to 200 assert(cfg.STEERING_LEFT_PWM <= 200) assert(cfg.STEERING_RIGHT_PWM <= 200) steering = PWMSteering(controller=steering_controller, left_pulse=cfg.STEERING_LEFT_PWM, right_pulse=cfg.STEERING_RIGHT_PWM) from donkeycar.parts.actuator import Mini_HBridge_DC_Motor_PWM motor = Mini_HBridge_DC_Motor_PWM(cfg.HBRIDGE_PIN_FWD, cfg.HBRIDGE_PIN_BWD) V.add(steering, inputs=['angle']) V.add(motor, inputs=["throttle"]) # DC2モータ pigpio制御、TB6612 ジャンパ設定無し elif cfg.DRIVE_TRAIN_TYPE == "DC_TWO_WHEEL_PIGPIO": try: import pigpio except: raise # pigpio 制御開始 pgio = pigpio.pi() from parts import PIGPIO_OUT, PIGPIO_PWM, CaterpillerMotorDriver # TB6612 STBY ピン初期化 stby = PIGPIO_OUT(pin=cfg.TB6612_STBY_GPIO, pgio=pgio, debug=use_debug) stby.run(1) # ジョイスティック出力値をDCモータ入力値に変換 driver = CaterpillerMotorDriver( left_balance=cfg.LEFT_PWM_BALANCE, right_balance=cfg.RIGHT_PWM_BALANCE, debug=use_debug) V.add(driver, inputs=['throttle', 'angle'], outputs=['left_motor_vref', 'left_motor_in1', 'left_motor_in2', 'right_motor_vref', 'right_motor_in1', 'right_motor_in2']) # 左モータ制御 left_in1 = PIGPIO_OUT(pin=cfg.LEFT_MOTOR_IN1_GPIO, pgio=pgio, debug=use_debug) left_in2 = PIGPIO_OUT(pin=cfg.LEFT_MOTOR_IN2_GPIO, pgio=pgio, debug=use_debug) left_vref = PIGPIO_PWM(pin=cfg.LEFT_MOTOR_PWM_GPIO, pgio=pgio, freq=cfg.PWM_FREQ, range=cfg.PWM_RANGE, debug=use_debug) V.add(left_in1, inputs=['left_motor_in1']) V.add(left_in2, inputs=['left_motor_in2']) V.add(left_vref, inputs=['left_motor_vref']) # 右モータ制御 right_in1 = PIGPIO_OUT(pin=cfg.RIGHT_MOTOR_IN1_GPIO, pgio=pgio, debug=use_debug) right_in2 = PIGPIO_OUT(pin=cfg.RIGHT_MOTOR_IN2_GPIO, pgio=pgio, debug=use_debug) right_vref = PIGPIO_PWM(pin=cfg.RIGHT_MOTOR_PWM_GPIO, pgio=pgio, freq=cfg.PWM_FREQ, range=cfg.PWM_RANGE, debug=use_debug) V.add(right_in1, inputs=['right_motor_in1']) V.add(right_in2, inputs=['right_motor_in2']) V.add(right_vref, inputs=['right_motor_vref']) #add tub to save data inputs=['cam/image_array', 'user/angle', 'user/throttle', 'user/mode'] types=['image_array', 'float', 'float', 'str'] if cfg.TRAIN_BEHAVIORS: inputs += ['behavior/state', 'behavior/label', "behavior/one_hot_state_array"] types += ['int', 'str', 'vector'] if cfg.HAVE_IMU: inputs += ['imu/acl_x', 'imu/acl_y', 'imu/acl_z', 'imu/gyr_x', 'imu/gyr_y', 'imu/gyr_z'] types +=['float', 'float', 'float', 'float', 'float', 'float'] if cfg.RECORD_DURING_AI: inputs += ['pilot/angle', 'pilot/throttle'] types += ['float', 'float'] th = TubHandler(path=cfg.DATA_PATH) tub = th.new_tub_writer(inputs=inputs, types=types, user_meta=meta) V.add(tub, inputs=inputs, outputs=["tub/num_records"], run_condition='recording') if cfg.PUB_CAMERA_IMAGES: from donkeycar.parts.network import TCPServeValue from donkeycar.parts.image import ImgArrToJpg pub = TCPServeValue("camera") V.add(ImgArrToJpg(), inputs=['cam/image_array'], outputs=['jpg/bin']) V.add(pub, inputs=['jpg/bin']) if type(ctr) is LocalWebController: print("You can now go to <your pis hostname.local>:8887 to drive your car.") elif isinstance(ctr, JoystickController): print("You can now move your joystick to drive your car.") #tell the controller about the tub ctr.set_tub(tub) if cfg.BUTTON_PRESS_NEW_TUB: def new_tub_dir(): V.parts.pop() tub = th.new_tub_writer(inputs=inputs, types=types, user_meta=meta) V.add(tub, inputs=inputs, outputs=["tub/num_records"], run_condition='recording') ctr.set_tub(tub) ctr.set_button_down_trigger('cross', new_tub_dir) ctr.print_controls() try: print('Start driving') #run the vehicle for 20 seconds V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS) except KeyboardInterrupt: print('Halt driving') finally: if stby is not None: stby.run(0) if use_debug: print('Stop TB6612 STBY') if pgio is not None: pgio.stop() if use_debug: print('Stop pigpio controll') print('Stop driving')