Alitra

import Adafruit_BBIO.PWM as PWM
import Adafruit_BBIO.GPIO as GPIO
import cv2.cv as cv
import os

from time import sleep
import time


in1="P8_7"
in2="P8_8"
in3="P8_9"
in4="P8_10"
enA="P8_13"
enB="P8_19"

servo1="P9_14"
servo2="P9_16"
servo3="P9_21"
servo4="P9_22"

duty_min=2

#test systems
print "setting up wheel pins"
GPIO.setup(in1,GPIO.OUT)
GPIO.setup(in2,GPIO.OUT)
GPIO.setup(in3,GPIO.OUT)
GPIO.setup(in4,GPIO.OUT)

def stop():
    GPIO.output(in1,GPIO.HIGH)
    GPIO.output(in2,GPIO.HIGH)
    GPIO.output(in3,GPIO.HIGH)
    GPIO.output(in4,GPIO.HIGH)
    sleep(1)

def forward():
    print"forward"
    GPIO.output(in1,GPIO.LOW)
    GPIO.output(in2,GPIO.HIGH)
    GPIO.output(in3,GPIO.HIGH)
    GPIO.output(in4,GPIO.LOW)

def backward():
    print "Backward"
    GPIO.output(in1,GPIO.HIGH)
    GPIO.output(in2,GPIO.LOW)
    GPIO.output(in3,GPIO.LOW)
    GPIO.output(in4,GPIO.HIGH)

def turn90left():
    print"90left"
    GPIO.output(in1,GPIO.LOW)
    GPIO.output(in2,GPIO.HIGH)
    GPIO.output(in3,GPIO.LOW)
    GPIO.output(in4,GPIO.HIGH)
    sleep(0.38)
def turn90right():
    print"90right"
    GPIO.output(in1,GPIO.HIGH)
    GPIO.output(in2,GPIO.LOW)
    GPIO.output(in3,GPIO.HIGH)
    GPIO.output(in4,GPIO.LOW)
    sleep(0.38)

PWM.start(enA,80)
PWM.start(enB,80)
PWM.start(servo1,0,50)
PWM.start(servo2,0,50)
PWM.start(servo3,0,50)
PWM.start(servo4,0,50)

def setAngle(angle,servo):

    for i in range(0,angle):
        PWM.set_duty_cycle(servo, (i*0.064) + duty_min)
        sleep(0.01)

def moveBack(angle,servo):

    for i in range(0,angle):
        ##move shaft from 0 to 180 degree
        PWM.set_duty_cycle(servo, (angle - i)*0.064 + duty_min)
        sleep(0.01)

#moves from start forward
forward()
sleep(1.4)
stop()
#turns 90 to the right
turn90right()
stop()
#move towards obstacle x1
forward()
sleep(1.2)
stop()
#open gripper carrier
setAngle(150,servo2)
#open gripper
setAngle(70,servo1)
#lower elbow and shoulder
print"picking x1"
setAngle(80,servo4)
moveBack(70,servo1)
setAngle(40,servo1)
sleep(2)
#raise elbow and shoulder
moveBack(80,servo4)
#raise elbow
setAngle(30,servo3)
#move gripper carrier to zero
moveBack(150,servo2)
#move gripper carrier to placing angle
setAngle(60,servo2)
sleep(2)
forward()
sleep(0.5)
stop()
#release object
print"releasing x1"
moveBack(40,servo1)
setAngle(70,servo1)
sleep(2)
print "folding arm"
#fold arm
moveBack(30,servo3)
moveBack(60,servo2)
moveBack(70,servo1)

backward()
sleep(0.5)
stop()
turn90right()
stop()
#move towards obstacle x2


print"starting sequence to pick x2"
forward()
sleep(0.6)
stop()
#open gripper carrier
setAngle(150,servo2)
#open gripper
setAngle(70,servo1)
sleep(0.5)
#lower elbow and shoulder
print"picking x2"
setAngle(80,servo4)
moveBack(70,servo1)
setAngle(40,servo1)
sleep(2)
#raise elbow and shoulder
moveBack(80,servo4)
#raise elbow
setAngle(30,servo3)
#move gripper carrier to zero
moveBack(150,servo2)
#move gripper carrier to placing angle
setAngle(60,servo2)
sleep(2)
forward()
sleep(0.5)
stop()
#release object
print"placing x2"
moveBack(40,servo1)
setAngle(70,servo1)
sleep(2)
#fold arm
moveBack(30,servo3)
moveBack(60,servo2)
moveBack(70,servo1)


backward()
sleep(0.5)
stop()
turn90left()
stop()
sleep(1)
forward()
sleep(0.8)
stop()
turn90left()
stop()
sleep(1)
#move towards tennis ball
forward()
sleep(0.5)
stop()

print"tennis ball pickup sequence"

#open gripper carrier
setAngle(150,servo2)
#open gripper
setAngle(70,servo1)
sleep(0.5)
#lower elbow and shoulder
print"picking up tennis ball"
setAngle(80,servo4)
moveBack(70,servo1)
setAngle(40,servo1)
sleep(2)
#raise elbow and shoulder
moveBack(80,servo4)
#raise elbow
setAngle(30,servo3)
#move gripper carrier to zero
moveBack(150,servo2)
#move gripper carrier to placing angle
setAngle(60,servo2)
sleep(2)

forward()
sleep(0.4)
stop()
turn90left()
stop()
sleep(1)
forward()
sleep(0.5)
stop()
turn90right()
stop()
sleep(1)
forward()
sleep(0.5)
stop()

#release object
print"dropping tennis ball"
moveBack(40,servo1)
setAngle(70,servo1)
sleep(2)
#fold arm
moveBack(30,servo3)
moveBack(60,servo2)
moveBack(70,servo1)
sleep(1)
backward()
sleep(0.5)
for i in range(0,3):
    PWM.stop(servo1)
    PWM.stop(servo2)
    PWM.stop(servo3)
    PWM.stop(servo4)
    PWM.stop(enA)
    PWM.stop(enB)
print "end stop"
stop()