├── observer_hub.py ├── README.md └── motorcontrol_modified.py /observer_hub.py: -------------------------------------------------------------------------------- 1 | # ----------------------------------------------- 2 | # Observer_Hub 3 | # 4 | # uses https://code.pybricks.com/ , LEGO City hub, LEGO remote control 5 | # connect 1 or 2 motors of any kind to Port A and/or B 6 | # 7 | # Version 1_0 8 | # -----------------------------------------------/ 9 | 10 | # ----------------------------------------------- 11 | # Set user defined values 12 | # ----------------------------------------------- 13 | 14 | observeChannel = 1 # channel number to observe (0 to 255). Needs to match the value the primary hub is broadcasting on. 15 | 16 | # define direction of motors 17 | 18 | dirMotorA = 1 # Direction 1 or -1 19 | dirMotorB = -1 # Direction 1 or -1 20 | 21 | lightValue = 0 # the initial light value, any number between 0 and 100. This will get overridden by the broadcast hub 22 | 23 | # ----------------------------------------------- 24 | # Import classes and functions 25 | # ----------------------------------------------- 26 | 27 | from pybricks.pupdevices import DCMotor, Motor, Light 28 | from pybricks.parameters import Port, Stop, Button, Color 29 | from pybricks.hubs import CityHub 30 | from pybricks.tools import wait, StopWatch 31 | from pybricks.iodevices import PUPDevice 32 | from uerrno import ENODEV 33 | 34 | # Color and brightness of Hub LEDs 35 | LEDreceiving = Color.GREEN*0.3 # if Hub connected, color * brightness 36 | LEDnotreceiving = Color.YELLOW*0.5 # if Hub is not connect, color * brightness 37 | 38 | 39 | # ----observe ----------------------------------------- 40 | 41 | def observe(): 42 | global v 43 | global lightValue 44 | 45 | data = hub.ble.observe(observeChannel) 46 | 47 | if data is None: 48 | # No data has been received in the last 1 second. 49 | hub.light.on(LEDnotreceiving) 50 | print('received nothing') 51 | else: 52 | # Data was received and is less that one second old. 53 | hub.light.on(LEDreceiving) 54 | 55 | speed, light = data 56 | 57 | v = speed 58 | lightValue = light 59 | 60 | drive() 61 | updateLights() 62 | 63 | print(lightValue) 64 | 65 | 66 | # ----------------------------------------------- 67 | # updateLights 68 | # ----------------------------------------------- 69 | 70 | def updateLights(): 71 | global lightValue 72 | 73 | if hasLights: 74 | for x in range(1,3): 75 | if motor[x].getType() == "Light": 76 | if lightValue == min: 77 | motor[x].obj.off() 78 | else: 79 | motor[x].obj.on(lightValue) 80 | 81 | # ----drive ------------------------------------------- 82 | 83 | def drive(): 84 | global vold 85 | global v 86 | 87 | if vold != v: 88 | # for each motor 1,2 89 | for x in range(1,3): 90 | # set speed and direction 91 | s = v*round(motor[x].getDir()) 92 | # real motor commands depending on motor type 93 | if motor[x].getType() == "Motor" : 94 | motor[x].obj.run(s*motor[x].getSpeed()) # in 2.7 95 | if motor[x].getType() == "DCMotor" : 96 | motor[x].obj.dc(s) 97 | if v == 0 and (motor[x].getType() == "Motor" or motor[x].getType() == "DCMotor"): 98 | print("stop",x) 99 | motor[x].obj.stop() 100 | vold = v 101 | 102 | # ----portcheck ------------------------------------------- 103 | 104 | def portcheck(i): 105 | # list of motors, 1 +2 contain string "DC" 106 | devices = { 107 | 1: "Wedo 2.0 DC Motor", 108 | 2: "Train DC Motor", 109 | 8: "Light", 110 | 38: "BOOST Interactive Motor", 111 | 46: "Technic Large Motor", 112 | 47: "Technic Extra Large Motor", 113 | 48: "SPIKE Medium Angular Motor", 114 | 49: "SPIKE Large Angular Motor", 115 | 75: "Technic Medium Angular Motor", 116 | 76: "Technic Large Angular Motor", 117 | } 118 | port = motor[i].getPort() 119 | # Try to get the device, if it is attached. 120 | try: 121 | device = PUPDevice(port) 122 | except OSError as ex: 123 | if ex.args[0] == ENODEV: 124 | # No device found on this port. 125 | motor[i].setType("---") 126 | print(port, ": not connected") 127 | return ("---") 128 | else: 129 | raise 130 | 131 | # Get the device id 132 | id = device.info()['id'] 133 | 134 | # Look up the name. 135 | try: 136 | # get the attributes for tacho motors 137 | if "Motor" in devices[id] and not("DC" in devices[id]): 138 | motor[i].setType("Motor") 139 | motor[i].obj = Motor(port) 140 | 141 | #new in 2.7 142 | # if motor[x].getDir() != 0 and motor[x].getType() == "Motor" : motor[x].obj.run(s*motor[x].getSpeed()) # in 2.7 143 | devs_max_speed = {38:1530,46:1890,47:1980,48:1367,49:1278,75:1367,76:1278 } 144 | dspeed = devs_max_speed.get(PUPDevice(port).info()['id'], 1000) 145 | motor[i].obj.stop() 146 | motor[i].obj.control.limits(speed=dspeed,acceleration=10000) 147 | motor[i].setSpeed(dspeed/100*0.9) 148 | 149 | # and set type for simple DC Motors 150 | if "DC" in devices[id]: 151 | motor[i].setType("DCMotor") 152 | motor[i].obj = DCMotor(port) 153 | 154 | if "Light" in devices[id]: 155 | motor[i].setType("Light") 156 | motor[i].obj = Light(port) 157 | if lightValue > 0: 158 | motor[i].obj.on(lightValue) 159 | 160 | global hasLights 161 | hasLights = True 162 | 163 | wait(100) 164 | print ("--") 165 | print(port, ":", devices[id], motor[i].getType(),motor[i].getSpeed(),motor[i].getAcc()) 166 | except KeyError: 167 | motor[i].setType("unkown") 168 | print(port, ":", "Unknown device with ID", id) 169 | 170 | # ---- device ------------------------------------------- 171 | 172 | class device(): 173 | # store the device infos for each motor 174 | def __init__(self,port,dir): 175 | self.port = port 176 | self.dir = dir 177 | self.type="" 178 | self.speed=99 179 | self.acc=99 180 | self.obj="" 181 | 182 | def setType(self,x) : self.type = x 183 | def setSpeed(self,x): self.speed = x 184 | def setAcc(self,x) : self.acc = x 185 | 186 | def getType(self) : return self.type 187 | def getPort(self) : return self.port 188 | def getDir(self) : return self.dir 189 | def getSpeed(self) : return self.speed 190 | def getAcc(self) : return self.acc 191 | 192 | # ----------------------------------------------- 193 | # globals 194 | # ----------------------------------------------- 195 | 196 | v = 0 197 | vold = 0 198 | 199 | # ----------------------------------------------- 200 | # Ininitialize 201 | # ----------------------------------------------- 202 | 203 | hub = CityHub(observe_channels=[observeChannel]) 204 | 205 | #define motors 206 | motor = [0,0,0] 207 | motor[1] = device(Port.A,dirMotorA) 208 | motor[2] = device(Port.B,dirMotorB) 209 | 210 | hasLights = False 211 | 212 | # get the port properties 213 | portcheck(1) 214 | portcheck(2) 215 | 216 | 217 | # ----------------------------------------------- 218 | # main loop 219 | # ----------------------------------------------- 220 | 221 | while True: 222 | 223 | observe() 224 | 225 | wait(10) 226 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # PyBricks-Train-Motor-Control-Script 2 | 3 | This script allows you to control Lego Trains via PyBricks without using the official Powered Up App. 4 | 5 | The original version of this script was written by Eurobrick user Lok24. It automatically detects what motors your hub is using and provides a number of quality of life improvements over the standard Lego firmware, such as smooth acceleration. 6 | 7 | It also adds better support for technic motors. Using the default firmware and a PoweredUp remote, holding the +/- buttons causes technic motors to run at full speed, and releasing the button causes the motor to stop. This script makes those motors work like the regular Lego train motor, allowing you to accelerate or decelerate these motors freely and maintaining the speed when you release the button. 8 | 9 | I have added a few additional features to the script, specifically support for the official Powered Up lights as well as Hub to Hub communication, which allows you to control multiple locomotives on one train simultaneously. 10 | 11 | More information about the original script from Lok24: https://www.eurobricks.com/forum/index.php?/forums/topic/187081-control-your-trains-without-smart-device-with-pybricks/ 12 | 13 | PyBricks: https://code.pybricks.com/ 14 | 15 | Special thanks to: 16 | 17 | Eurobricks user Lok24 for writing the origional version of this script 18 | 19 | Youtuber BatteryPoweredBricks for help testing 20 | 21 | ## Setup 22 | 23 | I highly recommend this setup tutorial video by BatteryPoweredBricks: https://www.youtube.com/watch?v=sgDMOHEmgL0 24 | 25 | Installation instructions: 26 | 1: Go to https://code.pybricks.com 27 | 2: Install Pybricks on your Hub, instructions here: https://pybricks.com/install/technic-boost-city/ 28 | 3: Import motorcontrol_modified.py into pybricks, and run the code on your hub. 29 | 4: Assuming the code ran, it should be saved to your hub. You can disconnect it from your computer and use it as normal. 30 | 31 | ## Customization 32 | 33 | There are a number of customization options for the script: 34 | 35 | ``` 36 | # define the two profiles 37 | # profil_x = (minimum speed,maximum Speed,accelerate in steps of ..., wait for next acceleration(in ms) 38 | 39 | Profil_A = (20,100,10,100) #min,max,step,acc 40 | Profil_B = (10,500,5,200) #min,max,step,acc 41 | ``` 42 | Defines the minimum speed, maximum speed, how much the speed increases each time it accelerates, and the wait time before accelerating again (in milliseconds). 43 | 44 | The minimum speed is 0 and the maximum speed is 100. Note that Profile_B by default has a maximum speed of 500. The motor's speed will still max out at 100. 45 | 46 | You can swap between modes by pressing the red button on the remote. 47 | 48 | ``` 49 | # define direction of motors 50 | 51 | dirMotorA = 1 # Direction 1 or -1 52 | dirMotorB = -1 # Direction 1 or -1 53 | ``` 54 | 55 | Sets the direction of each motor, for ports A and B. For a locomotive with two standard train motors, they will often be facing opposite directions, hence why you might need to set them to go in opposite directions. 56 | 57 | ``` 58 | autoacc = False # accelerate continuously when holding button 59 | ``` 60 | 61 | If False, you must press and release the Up/Down buttons on the remote for each acceleration step. If True, you can hold the button down to continuously accelerate/decelerate. 62 | 63 | ``` 64 | lightValue = 0 # the initial light value, any number between 0 and 100 65 | ``` 66 | 67 | If you have the Powered Up LIghts on your train, this value will be initial light value, with 0 being off. You can further control the brightness of the lights with the B buttons on the remote. 68 | 69 | ``` 70 | shouldBroadcast = False # whether the hub should broadcast data for a second hub to observe 71 | 72 | broadcastChannel = 1 # channel number to broadcast on (0 to 255). Needs to match the value the second hub is observing. 73 | ``` 74 | 75 | These settings are only used for Hub-to-Hub communication, which allows you to control multiple hubs at the same time. This is useful if you have multiple locomotives on the same train. You will be able to accelerate/decelerate/stop them simultaneously. 76 | 77 | Setting shouldBroadcast to True causes this hub to broadcast the motor speed and light brightness. Broadcast channel is the channel, between 0 and 255, on which to broadcast on. It must match the observeChannel setting on the observer_hub. 78 | 79 | More information about Hub-to-Hub communication, see the Multi-unit section below. 80 | 81 | ``` 82 | mode=1 # start with function number... 83 | ``` 84 | This sets which profile is the default. For more information about profiles, see above. 85 | 86 | ``` 87 | watchdog = False # "True" or "False": Stop motors when loosing remote connection 88 | ``` 89 | 90 | Set this to True if you want the motors to stop when the hub looses connection to the remote. 91 | 92 | ``` 93 | remoteTimeout =10 # hub waits x seconds for remote connect after starting hub 94 | ``` 95 | 96 | Number of seconds the hub waits while trying to connect to the remote at startup. 97 | 98 | ``` 99 | remoteName = "" # connect this remote only 100 | ``` 101 | 102 | You can make the Hub only connect to a specific remote. You can name the remote using the official Lego Powered Up app. 103 | 104 | ## Multi-unit control via Hub-to-Hub communication 105 | 106 | Video Summary by BatteryPoweredBricks: https://www.youtube.com/watch?v=RLTdA9oc_tw 107 | 108 | Maybe you want a train with multiple powered locomotives, all moving in sync. Such as a passenger train with a locomotive at each end (for instance, combining two copies of any of the official Lego passenger trains). Maybe you want multiple locomotives pulling a big freight train, like often happens in real life. Or, you want to add a powered piece of rolling stock to a train to give it some extra pulling power. 109 | 110 | The default Lego firmware can do this by having one remote connect to multiple hubs. However, you can't utilize some of QoL features of this script, like reversing motor direction, smooth acceleration, or better support for technic motors. 111 | 112 | PyBricks by default does not allow one remote to connect to multiple hubs. However, PyBricks allows hubs to broadcast and receive data over bluetooth. This script uses that broadcasting power to to allow you to control multiple hubs at the same time, allowing for simultaneous control of multiple locomotives on one train, accelerating and decelerating them together. 113 | 114 | This is useful if you need a second locomotive (or a powered piece of rolling stock) to allow for longer trains. You can have any number of hubs working simultaneously, allowing for super long and heavy trains. 115 | 116 | Many people buy two of the current passenger train sets to run a full consist with a locomotive at each end, and with this script you can utilize the motors in both locomotives. 117 | 118 | I highly recommend all powered units use the same motors/wheels/gear ratios in order to ensure they all move at the same speed. 119 | 120 | ### Setup 121 | 122 | One of your hubs will be the main hub, referred to as the broadcaster hub. This hub will connect to the remote and regulate the speed for the rest of the train. The rest will be observers, who listen to the broadcaster. 123 | 124 | The broadcaster hub will need `motorcontrol_modified.py` installed on it. Set `shouldBroadcast` to True, and set `broadcastChannel` to any value you want. If you want to have multiple different sets of Multi-unit setups, each train will need it's own channel. 125 | 126 | Each observer hub will need the `observer_hub.py` script installed on it. Set the `observerChannel` to the broadcastChannel. Make sure to verify the `dirMotor` values are correct for whichever direction each locomotive will be facing. 127 | 128 | ### In operation 129 | 130 | When the broadcaster hub starts, it will begin broadcasting it's current speed and lightvalue. 131 | 132 | When you start an observer hub, the led will turn yellow to indicate it is listening but not receiving any data. Once it starts receiving data, the led will return green. 133 | 134 | There is sometimes a slight lag between broadcasting and observing. It is usually unnoticeable, but is most noticeable if one of the hubs is connected to a computer via bluetooth. In testing, this seems to have very little affect on performance while running a train. 135 | 136 | # Troubleshooting 137 | 138 | Multiple users have observed issues using L-Motors with this script, including me. These seem to be erratic, with the script working sometimes but not always. I've attempted to fix it a couple times but being unable to consistently recreate it has made that very difficult. 139 | 140 | The error is that the first time you press a direction on the remote, the script will crash and the hub will start blinking blue. Pressing the green button on the hub will launch the script again, but it will likely keep crashing. 141 | 142 | To fix this issue, I recommend rolling back your PyBricks firmware to 3.2.3, which has worked for me. You'll have to use an older version of this script that doesn't include Broadcasting compatibility. 143 | 144 | ### Step 1: Download PyBricks 3.2.3 145 | Download pybricks-cityhub-v3.2.3.zip from Pybrick's github page here: https://github.com/pybricks/pybricks-micropython/releases/tag/v3.2.3 146 | 147 | ### Step 2: Re-install PyBricks on your Hub with the old firmware 148 | Re-install Pybricks on your hub. But on the first step under 'Select your hub' (https://youtu.be/sgDMOHEmgL0?t=103) you'll want to upload the ZIP file you downloaded under 'Advanced'. After you do this, Pybricks code will keep asking you if you want to update to the latest firmware. You'll want to ignore that. 149 | 150 | ### Step 3: Use an older version of the script 151 | 152 | You will need this older version of the script: https://github.com/and-ampersand-and/PyBricks-Train-Motor-Control-Script/blob/0b5dc0766753b03aacfd82886135d69e5ef04071/motorcontrol_modified.py 153 | The newer version of the script has Broadcasting functionality that isn't compatible with PyBricks 3.2.3 154 | 155 | -------------------------------------------------------------------------------- /motorcontrol_modified.py: -------------------------------------------------------------------------------- 1 | # ----------------------------------------------- 2 | # MotorControl 3 | # 4 | # uses https://code.pybricks.com/ , LEGO City hub, LEGO remote control 5 | # connect 1 or 2 motors of any kind to Port A and/or B 6 | # 7 | # Version 3_0 8 | # -----------------------------------------------/ 9 | from pybricks.parameters import * # Color 10 | 11 | # ----------------------------------------------- 12 | # Set user defined values 13 | # ----------------------------------------------- 14 | 15 | # define the two profiles 16 | # profil_x = (minimun speed,maximum Speed,accelerate in steps of ..., wait for next acceleration(in ms) 17 | 18 | Profil_A = (20,100,10,100) #min,max,step,acc 19 | Profil_B = (10,500,5,200) #min,max,step,acc 20 | 21 | # define direction of motors 22 | 23 | dirMotorA = 1 # Direction 1 or -1 24 | dirMotorB = -1 # Direction 1 or -1 25 | 26 | autoacc = False # accelarate continously when holding butten 27 | 28 | lightValue = 0 # the initial light value, any number between 0 and 100 29 | 30 | shouldBroadcast = False # whether the hub should broadcast data for a second hub to observe 31 | 32 | broadcastChannel = 1 # channel number to broadcast on (0 to 255). Needs to match the value the second hub is observing. 33 | 34 | # ----------------------------------------------- 35 | # Set general values 36 | # ----------------------------------------------- 37 | 38 | # assign buttons to function1 39 | # syntax: function = "name" 40 | # name may be "A+","A-","A0","B+","B-","B0","CENTER" 41 | 42 | UP = "A+" 43 | DOWN = "A-" 44 | STOP = "A0" 45 | SWITCH = "CENTER" 46 | BUP = "B+" 47 | BDOWN = "B-" 48 | BSTOP = "B0" 49 | 50 | mode=1 # start with function number... 51 | watchdog = False # "True" or "False": Stop motors when loosing remote connection 52 | remoteTimeout =10 # hub waits x seconds for remote connect after starting hub 53 | remoteName = "" # connect this remote only 54 | 55 | # Color and brightness of Hub LEDs 56 | LEDconn = Color.GREEN*0.3 # if Hub connected, color * brightness 57 | LEDnotconn = Color.RED*0.5 # if Hub is not connect, color * brightness 58 | 59 | LED_A = Color.GREEN*0.3 # Remote Profil_A, color * brightness 60 | LED_B = Color.RED*0.5 # Remote Profil_B, color * brightness 61 | 62 | # ----------------------------------------------- 63 | # Import classes and functions 64 | # ----------------------------------------------- 65 | 66 | from pybricks.pupdevices import DCMotor, Motor, Remote, Light 67 | from pybricks.parameters import Port, Stop, Button, Color 68 | from pybricks.hubs import CityHub 69 | from pybricks.tools import wait, StopWatch 70 | from pybricks.iodevices import PUPDevice 71 | from uerrno import ENODEV 72 | 73 | # ----------------------------------------------- 74 | # function 1 / drive motors 75 | # ----------------------------------------------- 76 | 77 | def function1(): 78 | 79 | vmax = profile[mode].vmax 80 | vmin = profile[mode].vmin 81 | accdelay = profile[mode].acc 82 | step = profile[mode].step 83 | 84 | global v 85 | 86 | if CheckButton(UP) and not CheckButton(STOP) : 87 | for x in range (1, step + 1): 88 | v = v + 1 89 | if v > vmax : 90 | v = vmax 91 | if v > 0 and v < vmin: 92 | v = vmin 93 | if abs(v) < vmin: 94 | v = 0 95 | drive() 96 | wait (accdelay) 97 | if v==0: 98 | break 99 | # further acceleration if button keeps pressed 100 | while autoacc == False and CheckButton(UP) : 101 | wait (100) 102 | # avoid changing direction when reaching "0" 103 | while v == 0 and CheckButton(UP): 104 | wait (100) 105 | 106 | if CheckButton(DOWN) and not CheckButton(STOP): 107 | for x in range (1, step + 1): 108 | v = v-1 109 | if v < vmax*-1 : 110 | v = vmax*-1 111 | if v < 0 and v > vmin*-1: 112 | v = vmin*-1 113 | if abs(v) < vmin : 114 | v = 0 115 | drive() 116 | wait (accdelay) 117 | if v==0: 118 | break 119 | # further acceleration if button keeps pressed 120 | while autoacc == False and CheckButton(DOWN) : 121 | wait (100) 122 | # avoid changing direction when reaching "0" 123 | while v == 0 and CheckButton(DOWN) : 124 | wait (100) 125 | 126 | if CheckButton(STOP): 127 | v = 0 128 | drive() 129 | wait (100) 130 | 131 | class setprofile(): 132 | def __init__(self,pr): 133 | self.vmin=pr[0] 134 | self.vmax=pr[1] 135 | self.step=pr[2] 136 | self.acc=pr[3] 137 | 138 | profile = [0,0,0] 139 | profile[1] = setprofile(Profil_A) 140 | profile[2] = setprofile(Profil_B) 141 | 142 | # ----------------------------------------------- 143 | # function 2 144 | # ----------------------------------------------- 145 | 146 | ''' 147 | def function2(): 148 | if CheckButton(UP): 149 | timer[1].set(3000) 150 | if timer[1].check(): 151 | print("Do something") 152 | ''' 153 | 154 | # ----------------------------------------------- 155 | # updateLights 156 | # ----------------------------------------------- 157 | 158 | def updateLights(): 159 | global lightValue 160 | max = 100; 161 | min = 0; 162 | step = 10; 163 | 164 | waitBetweenSteps = 0; 165 | 166 | if CheckButton(BUP) and not CheckButton(BSTOP) : 167 | waitBetweenSteps = 100 168 | lightValue += step 169 | 170 | if CheckButton(BDOWN) and not CheckButton(BSTOP) : 171 | waitBetweenSteps = 100 172 | lightValue -= step 173 | 174 | if CheckButton(BSTOP) : 175 | waitBetweenSteps = 300 176 | if lightValue == min : 177 | lightValue = max 178 | else : 179 | lightValue = min 180 | 181 | if lightValue > max: 182 | lightValue = max 183 | if lightValue < min: 184 | lightValue = min 185 | 186 | if CheckButton(BSTOP) or CheckButton(BUP) or CheckButton(BDOWN) : 187 | if shouldBroadcast : broadcastData() 188 | 189 | if hasLights: 190 | for x in range(1,3): 191 | if motor[x].getType() == "Light": 192 | if lightValue == min: 193 | motor[x].obj.off() 194 | else: 195 | motor[x].obj.on(lightValue) 196 | 197 | wait (waitBetweenSteps) 198 | 199 | 200 | # ----------------------------------------------- 201 | # general program routines and classes 202 | # ----------------------------------------------- 203 | 204 | # ----CheckButton ------------------------------------------- 205 | 206 | def CheckButton(x): 207 | try: 208 | button = remote.buttons.pressed() 209 | if x == "A+" : x = Button.LEFT_PLUS 210 | if x == "A-" : x = Button.LEFT_MINUS 211 | if x == "A0" : x = Button.LEFT 212 | 213 | if x == "B+" : x = Button.RIGHT_PLUS 214 | if x == "B-" : x = Button.RIGHT_MINUS 215 | if x == "B0" : x = Button.RIGHT 216 | 217 | if x == "CENTER" : x = Button.CENTER 218 | 219 | if x in button: 220 | return True 221 | else: 222 | return False 223 | except: 224 | return() 225 | 226 | # ----delay ------------------------------------------- 227 | 228 | class delay: 229 | def __init__(self,id,time=0,watch=StopWatch(),busy=False): 230 | self.id=id 231 | self.time=time 232 | self.watch=watch 233 | self.busy=busy 234 | print ("Init Timer",id) 235 | # set a timer 236 | def set(self,x): 237 | if self.busy == False: 238 | self.busy = True 239 | self.watch.reset() 240 | self.time = x 241 | print("Timer",timer[1].id, "set to",x) 242 | #check if timer is reached, then return "True" 243 | def check(self): 244 | if self.busy == True: 245 | if self.watch.time() > self.time: 246 | self.busy = False 247 | self.time=0 248 | print("Timer",timer[1].id, "stopped") 249 | return(True) 250 | else: 251 | return(False) 252 | 253 | # ----drive ------------------------------------------- 254 | 255 | def drive(): 256 | global vold 257 | global v 258 | 259 | if shouldBroadcast : broadcastData() 260 | 261 | if vold != v: 262 | # for each motor 1,2 263 | for x in range(1,3): 264 | # set speed and direction 265 | s = v*round(motor[x].getDir()) 266 | # real motor commands depending on motor type 267 | if motor[x].getType() == "Motor" : 268 | motor[x].obj.run(s*motor[x].getSpeed()) # in 2.7 269 | if motor[x].getType() == "DCMotor" : 270 | motor[x].obj.dc(s) 271 | if v == 0 and (motor[x].getType() == "Motor" or motor[x].getType() == "DCMotor"): 272 | print("stop",x) 273 | motor[x].obj.stop() 274 | #if motor[x].getDir() != 0 and motor[x].getType() == "DCMotor" : motor[x].obj.dc(s) 275 | vold = v 276 | 277 | # ----portcheck ------------------------------------------- 278 | 279 | def portcheck(i): 280 | # list of motors, 1 +2 contain string "DC" 281 | devices = { 282 | 1: "Wedo 2.0 DC Motor", 283 | 2: "Train DC Motor", 284 | 8: "Light", 285 | 38: "BOOST Interactive Motor", 286 | 46: "Technic Large Motor", 287 | 47: "Technic Extra Large Motor", 288 | 48: "SPIKE Medium Angular Motor", 289 | 49: "SPIKE Large Angular Motor", 290 | 75: "Technic Medium Angular Motor", 291 | 76: "Technic Large Angular Motor", 292 | } 293 | port = motor[i].getPort() 294 | # Try to get the device, if it is attached. 295 | try: 296 | device = PUPDevice(port) 297 | except OSError as ex: 298 | if ex.args[0] == ENODEV: 299 | # No device found on this port. 300 | motor[i].setType("---") 301 | print(port, ": not connected") 302 | return ("---") 303 | else: 304 | raise 305 | 306 | # Get the device id 307 | id = device.info()['id'] 308 | 309 | # Look up the name. 310 | try: 311 | # get the attributes for tacho motors 312 | if "Motor" in devices[id] and not("DC" in devices[id]): 313 | motor[i].setType("Motor") 314 | motor[i].obj = Motor(port) 315 | 316 | #new in 2.7 317 | # if motor[x].getDir() != 0 and motor[x].getType() == "Motor" : motor[x].obj.run(s*motor[x].getSpeed()) # in 2.7 318 | devs_max_speed = {38:1530,46:1890,47:1980,48:1367,49:1278,75:1367,76:1278 } 319 | dspeed = devs_max_speed.get(PUPDevice(port).info()['id'], 1000) 320 | motor[i].obj.stop() 321 | motor[i].obj.control.limits(speed=dspeed,acceleration=10000) 322 | motor[i].setSpeed(dspeed/100*0.9) 323 | 324 | # and set type for simple DC Motors 325 | if "DC" in devices[id]: 326 | motor[i].setType("DCMotor") 327 | motor[i].obj = DCMotor(port) 328 | 329 | if "Light" in devices[id]: 330 | motor[i].setType("Light") 331 | motor[i].obj = Light(port) 332 | if lightValue > 0: 333 | motor[i].obj.on(lightValue) 334 | 335 | global hasLights 336 | hasLights = True 337 | 338 | wait(100) 339 | print ("--") 340 | print(port, ":", devices[id], motor[i].getType(),motor[i].getSpeed(),motor[i].getAcc()) 341 | except KeyError: 342 | motor[i].setType("unkown") 343 | print(port, ":", "Unknown device with ID", id) 344 | 345 | 346 | # ---- broadcast ----------------------------------------- 347 | 348 | def broadcastData(): 349 | global v 350 | global lightValue 351 | 352 | speed = v 353 | light = lightValue 354 | 355 | data = ( speed, light ) 356 | hub.ble.broadcast(data) 357 | 358 | 359 | # ---- device ------------------------------------------- 360 | 361 | class device(): 362 | # store the device infos for each motor 363 | def __init__(self,port,dir): 364 | self.port = port 365 | self.dir = dir 366 | self.type="" 367 | self.speed=99 368 | self.acc=99 369 | self.obj="" 370 | 371 | def setType(self,x) : self.type = x 372 | def setSpeed(self,x): self.speed = x 373 | def setAcc(self,x) : self.acc = x 374 | 375 | def getType(self) : return self.type 376 | def getPort(self) : return self.port 377 | def getDir(self) : return self.dir 378 | def getSpeed(self) : return self.speed 379 | def getAcc(self) : return self.acc 380 | 381 | # ----------------------------------------------- 382 | # globals 383 | # ----------------------------------------------- 384 | 385 | v = 0 386 | vold = 0 387 | #remoteConnected = False 388 | 389 | # ----------------------------------------------- 390 | # Ininitialize 391 | # ----------------------------------------------- 392 | 393 | hub = CityHub(broadcast_channel=broadcastChannel) 394 | 395 | #define timers 396 | timer = [0,0,0] 397 | timer[1] = delay(1) 398 | timer[2] = delay(2) 399 | 400 | #define motors 401 | motor = [0,0,0] 402 | motor[1] = device(Port.A,dirMotorA) 403 | motor[2] = device(Port.B,dirMotorB) 404 | 405 | hasLights = False 406 | 407 | # get the port properties 408 | portcheck(1) 409 | portcheck(2) 410 | 411 | # ----------------------------------------------- 412 | # remote connect 413 | # ----------------------------------------------- 414 | 415 | hub.light.on(Color.RED) 416 | print (hub.system.name()) 417 | try: 418 | remote = Remote(name=remoteName,timeout=remoteTimeout*1000) 419 | except OSError as ex: 420 | hub.system.shutdown() 421 | 422 | # ----------------------------------------------- 423 | # main loop 424 | # ----------------------------------------------- 425 | 426 | while True: 427 | 428 | # --check if remote is connected --------------------------------- 429 | 430 | try: 431 | button = remote.buttons.pressed() 432 | hub.light.on(LEDconn) 433 | remoteConnected = True 434 | except OSError as ex: 435 | hub.light.on(LEDnotconn) 436 | print("Remote not connected") 437 | remoteConnected = False 438 | 439 | if watchdog == True: 440 | v=0 441 | drive() 442 | try: 443 | # reconnect remote 444 | remote = Remote(timeout=1000) 445 | wait(100) 446 | print("Remote reconnected") 447 | remoteConnected = True 448 | except OSError as ex: 449 | print("Remote not connected") 450 | 451 | 452 | if CheckButton(SWITCH): 453 | mode = mode+1 454 | if mode > 2: 455 | mode = 1 456 | print (mode) 457 | if mode == 1 : remote.light.on(LED_A) 458 | if mode == 2 : remote.light.on(LED_B) 459 | 460 | while CheckButton(SWITCH): 461 | button = remote.buttons.pressed() 462 | wait (100) 463 | 464 | if mode == 1 : function1() 465 | if mode == 2 : function1() 466 | 467 | if hasLights or shouldBroadcast : updateLights() 468 | 469 | wait(10) 470 | --------------------------------------------------------------------------------