SHOW:
|
|
- or go back to the newest paste.
| 1 | from microbit import * | |
| 2 | ||
| 3 | class KitronikRoboticsBoard: | |
| 4 | PRESCALE_REG = 0xFE | |
| 5 | MODE_1_REG = 0x00 | |
| 6 | SRV_REG_BASE = 0x08 | |
| 7 | MOT_REG_BASE = 0x28 | |
| 8 | REG_OFFSET = 4 | |
| 9 | SERVO_MULTIPLIER = 226 | |
| 10 | SERVO_ZERO_OFFSET = 0x66 | |
| 11 | ||
| 12 | chipAddress = 0x6C | |
| 13 | initialised = False | |
| 14 | stepInit = False | |
| 15 | stepStage = 0 | |
| 16 | stepper1Steps = 200 | |
| 17 | stepper2Steps = 200 | |
| 18 | ||
| 19 | def __init(self): | |
| 20 | ||
| 21 | buf = bytearray(2) | |
| 22 | ||
| 23 | buf[0] = self.PRESCALE_REG | |
| 24 | buf[1] = 0x85 #50Hz | |
| 25 | i2c.write(self.chipAddress, buf, False) | |
| 26 | ||
| 27 | for blockReg in range(0xFA, 0xFE, 1): | |
| 28 | buf[0] = blockReg | |
| 29 | buf[1] = 0x00 | |
| 30 | i2c.write(self.chipAddress, buf, False) | |
| 31 | ||
| 32 | buf[0] = self.MODE_1_REG | |
| 33 | buf[1] = 0x01 | |
| 34 | i2c.write(self.chipAddress, buf, False) | |
| 35 | self.initialised = True | |
| 36 | ||
| 37 | def servoWrite(self, servo, degrees): | |
| 38 | if self.initialised is False: | |
| 39 | self.__init(self) | |
| 40 | buf = bytearray(2) | |
| 41 | calcServo = self.SRV_REG_BASE + ((servo - 1) * self.REG_OFFSET) | |
| 42 | HighByte = False | |
| 43 | PWMVal = (degrees * 100 * self.SERVO_MULTIPLIER) / (10000 + self.SERVO_ZERO_OFFSET) | |
| 44 | ||
| 45 | if (PWMVal > 0xFF): | |
| 46 | HighByte = True | |
| 47 | buf[0] = calcServo | |
| 48 | buf[1] = int(PWMVal) | |
| 49 | i2c.write(self.chipAddress, buf, False) | |
| 50 | buf[0] = calcServo + 1 | |
| 51 | if (HighByte): | |
| 52 | buf[1] = 0x01 | |
| 53 | else: | |
| 54 | buf[1] = 0x00 | |
| 55 | i2c.write(self.chipAddress, buf, False) | |
| 56 | ||
| 57 | def motorOn(self, motor, direction, speed): | |
| 58 | if self.initialised is False: | |
| 59 | self.__init(self) | |
| 60 | buf = bytearray(2) | |
| 61 | motorReg = self.MOT_REG_BASE + (2 * (motor - 1) * self.REG_OFFSET) | |
| 62 | HighByte = False | |
| 63 | OutputVal = speed * 40 | |
| 64 | ||
| 65 | if direction == "forward": | |
| 66 | if OutputVal > 0xFF: | |
| 67 | HighByte = True | |
| 68 | HighOutputVal = int(OutputVal/256) | |
| 69 | buf[0] = motorReg | |
| 70 | buf[1] = int(OutputVal) | |
| 71 | i2c.write(self.chipAddress, buf, False) | |
| 72 | buf[0] = motorReg + 1 | |
| 73 | if HighByte: | |
| 74 | buf[1] = HighOutputVal | |
| 75 | else: | |
| 76 | buf[1] = 0x00 | |
| 77 | i2c.write(self.chipAddress, buf, False) | |
| 78 | ||
| 79 | for offset in range(4, 6, 1): | |
| 80 | buf[0] = motorReg + offset | |
| 81 | buf[1] = 0x00 | |
| 82 | i2c.write(self.chipAddress, buf, False) | |
| 83 | ||
| 84 | elif direction == "reverse": | |
| 85 | if OutputVal > 0xFF: | |
| 86 | HighByte = True | |
| 87 | HighOutputVal = int(OutputVal/256) | |
| 88 | buf[0] = motorReg + 4 | |
| 89 | buf[1] = int(OutputVal) | |
| 90 | i2c.write(self.chipAddress, buf, False) | |
| 91 | buf[0] = motorReg + 5 | |
| 92 | if HighByte: | |
| 93 | buf[1] = HighOutputVal | |
| 94 | else: | |
| 95 | buf[1] = 0x00 | |
| 96 | i2c.write(self.chipAddress, buf, False) | |
| 97 | ||
| 98 | for offset2 in range(0, 2, 1): | |
| 99 | buf[0] = motorReg + offset2 | |
| 100 | buf[1] = 0x00 | |
| 101 | i2c.write(self.chipAddress, buf, False) | |
| 102 | ||
| 103 | def motorOff(self, motor): | |
| 104 | buf = bytearray(2) | |
| 105 | motorReg = self.MOT_REG_BASE + (2 * (motor - 1) * self.REG_OFFSET) | |
| 106 | ||
| 107 | for offset3 in range(0, 2, 1): | |
| 108 | buf[0] = motorReg + offset3 | |
| 109 | buf[1] = 0x00 | |
| 110 | i2c.write(self.chipAddress, buf, False) | |
| 111 | ||
| 112 | for offset4 in range(4, 6, 1): | |
| 113 | buf[0] = motorReg + offset4 | |
| 114 | buf[1] = 0x00 | |
| 115 | i2c.write(self.chipAddress, buf, False) | |
| 116 | ||
| 117 | def allOff(self): | |
| 118 | buf = bytearray(2) | |
| 119 | servoOffCount = 0 | |
| 120 | servoRegCount = 0 | |
| 121 | ||
| 122 | for motors in range(1, 5, 1): | |
| 123 | self.motorOff(self, motors) | |
| 124 | ||
| 125 | while servoOffCount < 8: | |
| 126 | for offset5 in range(0, 2, 1): | |
| 127 | buf[0] = self.SRV_REG_BASE + servoRegCount + offset5 | |
| 128 | buf[1] = 0x00 | |
| 129 | i2c.write(self.chipAddress, buf, False) | |
| 130 | ||
| 131 | servoRegCount += 4 | |
| 132 | servoOffCount += 1 | |
| 133 | ||
| 134 | def stepperMotorTurnAngle(self, stepper, direction, angle): | |
| 135 | angleToSteps = 0 | |
| 136 | ||
| 137 | if self.initialised is False: | |
| 138 | self.__init(self) | |
| 139 | ||
| 140 | if stepper == "Stepper1": | |
| 141 | angleToSteps = ((angle - 1) * (self.stepper1Steps - 1)) / (360 - 1) + 1 | |
| 142 | else: | |
| 143 | angleToSteps = ((angle - 1) * (self.stepper2Steps - 1)) / (360 - 1) + 1 | |
| 144 | ||
| 145 | angleToSteps = int(angleToSteps) | |
| 146 | self._turnStepperMotor(self, stepper, direction, angleToSteps) | |
| 147 | ||
| 148 | def stepperMotorTurnSteps(self, stepper, direction, stepperSteps): | |
| 149 | if self.initialised is False: | |
| 150 | self.__init(self) | |
| 151 | ||
| 152 | self._turnStepperMotor(self, stepper, direction, stepperSteps) | |
| 153 | ||
| 154 | def _turnStepperMotor(self, stepper, direction, steps): | |
| 155 | stepCounter = 0 | |
| 156 | ||
| 157 | if self.stepInit is False: | |
| 158 | self.stepStage = 1 | |
| 159 | self.stepInit = True | |
| 160 | ||
| 161 | while stepCounter < steps: | |
| 162 | if stepStage == 1 or stepStage == 3: | |
| 163 | if stepper == "Stepper1": | |
| 164 | currentMotor = 1 | |
| 165 | else: | |
| 166 | currentMotor = 3 | |
| 167 | else: | |
| 168 | if stepper == "Stepper1": | |
| 169 | currentMotor = 2 | |
| 170 | else: | |
| 171 | currentMotor = 4 | |
| 172 | ||
| 173 | if stepStage == 1 or stepStage == 4: | |
| 174 | currentDirection = "forward" | |
| 175 | else: | |
| 176 | currentDirection = "reverse" | |
| 177 | ||
| 178 | self.motorOn(self, currentMotor, currentDirection, 100) | |
| 179 | sleep(20) | |
| 180 | ||
| 181 | if direction == "forward": | |
| 182 | if stepStage == 4: | |
| 183 | stepStage = 1 | |
| 184 | else: | |
| 185 | stepStage += 1 | |
| 186 | elif direction == "reverse": | |
| 187 | if stepStage == 1: | |
| 188 | stepStage = 4 | |
| 189 | else: | |
| 190 | stepStage -= 1 | |
| 191 | ||
| 192 | stepCounter += 1 | |
| 193 | ||
| 194 | while True: | |
| 195 | theBoard = KitronikRoboticsBoard | |
| 196 | if button_a.is_pressed(): | |
| 197 | theBoard.stepperMotorTurnAngle(theBoard, "Stepper1", "forward", 180) | |
| 198 | theBoard.motorOn(theBoard, 3, "forward", 10) | |
| 199 | theBoard.motorOn(theBoard, 4, "reverse", 100) | |
| 200 | theBoard.servoWrite(theBoard, 1, 180) | |
| 201 | theBoard.servoWrite(theBoard, 2, 180) | |
| 202 | theBoard.servoWrite(theBoard, 3, 180) | |
| 203 | theBoard.servoWrite(theBoard, 4, 180) | |
| 204 | theBoard.servoWrite(theBoard, 5, 0) | |
| 205 | theBoard.servoWrite(theBoard, 6, 0) | |
| 206 | theBoard.servoWrite(theBoard, 7, 0) | |
| 207 | theBoard.servoWrite(theBoard, 8, 0) | |
| 208 | if button_b.is_pressed(): | |
| 209 | theBoard.stepperMotorTurnSteps(theBoard, "Stepper1", "reverse", 100) | |
| 210 | theBoard.motorOff(theBoard, 3) | |
| 211 | theBoard.motorOff(theBoard, 4) | |
| 212 | theBoard.servoWrite(theBoard, 1, 90) | |
| 213 | theBoard.servoWrite(theBoard, 2, 90) | |
| 214 | theBoard.servoWrite(theBoard, 3, 90) | |
| 215 | theBoard.servoWrite(theBoard, 4, 90) | |
| 216 | theBoard.servoWrite(theBoard, 5, 90) | |
| 217 | theBoard.servoWrite(theBoard, 6, 90) | |
| 218 | theBoard.servoWrite(theBoard, 7, 90) | |
| 219 | theBoard.servoWrite(theBoard, 8, 90) | |
| 220 | if button_a.is_pressed() and button_b.is_pressed(): | |
| 221 | theBoard.allOff(theBoard) |
