#!/usr/bin/python # Post 1010 Pom Collector import os, sys import ctypes KIPR=ctypes.CDLL("/usr/lib/libkipr.so") # CONSTANTS --------------------------------------------------------- R_MOTOR = 0 L_MOTOR = 1 LIFT_MOTOR = 2 ARM_MOTOR = 3 TOPH_RIGHT = 0 TOPH_LEFT = 1 TOPH_BRIGHT = 2 TOPH_BLEFT = 3 LS = 5 ARM_SERVO = 1 UP = 950 GROUND = 1860 BLACK = 2500 BACK_BLACK = 3800 # FUNCTIONS --------------------------------------------------------------------------- def move(l_power, r_power, sleep_time=5): KIPR.motor(L_MOTOR, l_power) KIPR.motor(R_MOTOR, r_power) KIPR.msleep(sleep_time) def stop(s_time): move(0, 0, s_time) def go_to_black(l_power=50, r_power=50): while(KIPR.analog(TOPH_LEFT) < BLACK or KIPR.analog(TOPH_RIGHT) < BLACK): move(l_power, r_power) def go_to_white(l_power=50, r_power=50): while(KIPR.analog(TOPH_LEFT) > BLACK or KIPR.analog(TOPH_RIGHT) > BLACK): move(l_power, r_power) def arm_control(power, sleep_time = 5): KIPR.motor(ARM_MOTOR, power) KIPR.msleep(sleep_time) def lift_control(power, sleep_time = 5): KIPR.motor(LIFT_MOTOR, power) KIPR.msleep(sleep_time) def line_follow(time, sensor=TOPH_LEFT): end_time = KIPR.seconds() + time while(KIPR.seconds() < end_time): if(KIPR.analog(sensor) > BLACK): if(sensor == TOPH_LEFT): move(37, 50) else: move(50, 37) else: if(sensor == TOPH_LEFT): move(50, 37) else: move(37, 50) def hold_on(): KIPR.clear_motor_position_counter(3) while(KIPR.get_motor_position_counter(3) > -18): KIPR.motor(ARM_MOTOR, -10) KIPR.off(3) # Backward Line Follower def blf(time, power): end_time = KIPR.seconds() + time while(KIPR.seconds() < end_time): if(KIPR.analog(TOPH_BRIGHT) < BACK_BLACK and KIPR.analog(TOPH_BLEFT) < BACK_BLACK): move(-power, -power) elif(KIPR.analog(TOPH_BRIGHT) < BACK_BLACK and KIPR.analog(TOPH_BLEFT) > BACK_BLACK): move(int(-power * 0.67), -power) elif(KIPR.analog(TOPH_BRIGHT) > BACK_BLACK and KIPR.analog(TOPH_BLEFT) < BACK_BLACK): move(-power, int(-power * 0.67)) # Knock Down Pom Pile def jitter(): arm_control(-25, 450) arm_control(25, 300) # Wait for Start Light def wfl(): START = KIPR.analog(LS) # Get ambiant light while(KIPR.analog(LS) > START/2): stop(200) # Initialization ----------------------------------------------------- def setup(): print "Waiting for start light" arm_control(-15, 200) arm_control(15, 190) KIPR.off(3) wfl() # Main Code -------------------------------------------------------- def main(): # Wait for other bot print "Waiting for other bot" stop(4500) # Put lift forward lift_control(50, 3700) KIPR.off(2) # Go forward to black line go_to_black(100, 100) # Align on black line go_to_white(100, 100) move(100, 0, 1400) move(100, 100, 400) # Forward line follow to Transporter line_follow(20300) stop(300) move(-10, -10, 1100) stop(300) # Put poms into Transporter arm_control(33, 120) move(30, 30, 790) jitter() stop(500) # Grab transporter arm_control(-25, 1200) KIPR.off(3) # Backward line follow blf(3100, 40) # Move forwards a bit move(50, 50, 800) # Backward line follow blf(3200, 40) # Move forwards a bit move(50, 50, 700) # Backward line follow blf(1600, 40) # Move forwards a bit move(50, 50, 700) # Backward line follow blf(1800, 40) # Move forwards a bit move(50, 50, 700) # Backward line follow blf(2200, 40) # Move forwards a bit move(50, 50, 700) # Backward line follow blf(2000, 40) # Move forwards a bit move(50, 50, 700) # Backward line follow blf(2000, 40) # Rotate 90d to the left hold_on() move(-25, 25, 1600) move(25, -25, 600) hold_on() move(-25, 25, 2000) move(25, -25, 600) move(-25, 25, 1100) stop(100) # Lower lift lift_control(-50, 3000) KIPR.off(2) # Move forward a bit move(50, 50, 1000) stop(1800) # Elevate lift lift_control(50, 2700) KIPR.off(2) # Lower lift lift_control(-50, 3500) KIPR.off(2) # Shift to the right slightly move(20, -20, 550) # Move backwards a bit move(-30, -30, 1800) stop(300) # Elevate lift lift_control(50, 2500) KIPR.off(2) # Rotate 90d to the right move(25, -25, 1600) while(KIPR.analog(TOPH_LEFT) < BLACK): move(25, -25) stop(300) # Back line follow blf(1000, 30) hold_on() blf(1000, 30) hold_on() blf(1000, 30) # Back line follow blf(3700, 33) # Move forwards a bit move(55, 55, 700) # Back line follow blf(3500, 33) # Move forwards a bit move(50, 50, 700) # Back line follow blf(3400, 33) # Move forwards a bit move(50, 50, 700) # Back line follow blf(3500, 33) # Move forwards a bit move(50, 50, 700) # Back line follow blf(3400, 33) # Move forwards a bit move(50, 50, 700) # Back line follow blf(3700, 33) stop(400) # Rotate 90d right move(0, -30, 6000) stop(100) # Forward to pipe move(50, 50, 3400) stop(500) # Backward a little arm_control(20, 100) jitter() move(-50, -50, 1200) # All done KIPR.ao() KIPR.disable_servos() print "Game over" stop(2000) # -------------------------------------------------------------- if __name__== "__main__": sys.stdout = os.fdopen(sys.stdout.fileno(),"w",0) setup() main()