from sw import pins from tamproxy.devices import Odometer from tamproxy.devices import Gyro from tamproxy.devices import Encoder from tamproxy import TAMProxy import time if __name__ == "__main__": tamp = TAMProxy() gyro = Gyro(tamp, pins.gyro_cs, integrate=False) lEnc = Encoder(tamp, pins.l_encoder_a, pins.l_encoder_b, continuous = False) rEnc = Encoder(tamp, pins.r_encoder_a, pins.r_encoder_b, continuous = False) odo = Odometer(tamp, lEnc, rEnc, gyro, 0.0) while True: odo.update() lEnc.update() rEnc.update() print odo.val print lEnc.val print rEnc.val time.sleep(0.05)
class SortBlocks(SyncedSketch): pins = 31, 32 def setup(self): self.LEFT_TOWER = 85 self.RIGHT_TOWER = 24 self.CENTER = 53 self.color = Color(self.tamp, integrationTime=Color.INTEGRATION_TIME_101MS, gain=Color.GAIN_1X) self.servo = Servo(self.tamp, 23) #pin TBD self.red_detected = False self.green_detected = False self.servo.write(self.CENTER) self.counter = 0 self.servo_bottom = Servo(self.tamp,22) self.servo_bottom.write(34) self.motor = Motor(self.tamp, 24, 25) self.motor.write(True, 250) self.jammed = False self.conveyor_encoder = Encoder(self.tamp, *self.pins, continuous=True) self.prev_encoder_value = 0 self.conveyor_encoder.update() self.timer = Timer() self.sorter_delta = 50 def loop(self): if self.timer.millis() > 300: self.timer.reset() #self.conveyor_encoder.update() encoder_value = self.conveyor_encoder.val #print self.prev_encoder_value #print encoder_value if(encoder_value == self.prev_encoder_value): self.jammed = True else: self.jammed = False self.prev_encoder_value = encoder_value self.motor.write(not self.jammed,250) # print self.color.r, self.color.g, self.color.b, self.color.c # print self.color.colorTemp, self.color.lux #if not self.red_detected and not self.green_detected and self.color.r > 1.3 * self.color.g and self.color.r > 1.3 * self.color.b: detect_red = self.color.r > 1.3*self.color.g detect_green = self.color.g > 1.3*self.color.r sum_val = self.color.r+self.color.g+self.color.b if detect_red and sum_val > 300: self.servo.write(self.LEFT_TOWER) self.counter = 0 #elif not self.red_detected and not self.green_detected and self.color.g > 1.3 * self.color.r and self.color.g > 1.3 * self.color.b: elif detect_green and sum_val > 300: self.servo.write(self.RIGHT_TOWER) self.counter = 0 elif self.counter > 400: self.servo.write(self.CENTER) # else: # self.servo.write(self.CENTER + self.sorter_delta) # self.sorter_delta *= -1 self.counter += 100