bot_utilitySensor.py.save

# -*- coding: utf-8 -*-
"""
Created on Thu Jan 28 16:34:01 2016

@author:
"""

import brickpi
import time

def initInterface(interface, motors, touchPort):
    interface.initialize()

    interface.motorEnable(motors[0])
    interface.motorEnable(motors[1])
    
    touch_port = touchPort[0]
    touch_port2 = touchPort[1]

    interface.sensorEnable(touch_port, brickpi.SensorType.SENSOR_TOUCH)
    interface.sensorEnable(touch_port2, brickpi.SensorType.SENSOR_TOUCH)
    
    
    motorParams = interface.MotorAngleControllerParameters()
    motorParams.maxRotationAcceleration = 8.0
    motorParams.maxRotationSpeed = 16.0
    motorParams.feedForwardGain = 255/20.0
    motorParams.minPWM = 18.0
    motorParams.pidParameters.minOutput = -255.0
    motorParams.pidParameters.maxOutput = 255.0
    motorParams.pidParameters.k_p = 240.0
    motorParams.pidParameters.k_i = 1309.0909
    motorParams.pidParameters.k_d = 11.0

    motorParams2 = interface.MotorAngleControllerParameters()
    motorParams2.maxRotationAcceleration = 8.0
    motorParams2.maxRotationSpeed = 16.0
    motorParams2.feedForwardGain = 255/20.0
    motorParams2.minPWM = 18.0
    motorParams2.pidParameters.minOutput = -255.0
    motorParams2.pidParameters.maxOutput = 255.0
    motorParams2.pidParameters.k_p = 240.0
    motorParams2.pidParameters.k_i = 1252.1739
    motorParams2.pidParameters.k_d = 11.5

    interface.setMotorAngleControllerParameters(motors[0],motorParams)
    interface.setMotorAngleControllerParameters(motors[1],motorParams2)


def goStraight(distCm, interface, motors):
    radianPerCm = 0.3625
    angle = distCm * radianPerCm 
    interface.increaseMotorAngleReferences(motors, [angle, angle])

    while not interface.motorAngleReferencesReached(motors) :
        motorAngles = interface.getMotorAngles(motors)
        time.sleep(0.1)
        ## Maybe add a break after x seconds

        
        
## NOT YET TESTED   
def goStraightBump(distCm, interface, motors, touchPort):
    radianPerCm = 0.3625
    backward_dist=5
    angle = distCm * radianPerCm 
    interface.increaseMotorAngleReferences(motors, [angle, angle])
    
    initmotorangles= interface.getMotorAngles(motors)

    while not interface.motorAngleReferences:
        motorAngles = interface.getMotorAngles(motors)
        resultBump = interface.getSensorValue(touchPort[0]) 
        resultBump2 = interface.getSensorValue(touchPort[1]) 
        
        
        bump1 = angle + initmotorangles[0][0]-motorAngles[0][0]
        bump2 = angle + initmotorangles[1][0]-motorAngles[1][0]

        if (resultBump[0]==1 and resultBump2[0]==1):
            # Only the front bumber no need to take care of the sign
            
            goStraight(-backward_dist, interface, motors)
            Right90deg(interface, motors)
            interface.increaseMotorAngleReferences(motors, [bump1, bump2])
            # goStraight(+5, interface, motors)
            
            
        elif (resultBump[0]==1):
            # Only the front bumber no need to take care of the sign
            goStraight(-backward_dist, interface, motors)
            Left90deg(interface, motors)
            interface.increaseMotorAngleReferences(motors, [bump1, bump2])
            # goStraight(+20, interface, motors)
              
        elif (resultBump2[0]==1):
            goStraight(-backward_dist, interface, motors)
            Right90deg(interface, motors)
            interface.increaseMotorAngleReferences(motors, [bump1, bump2])            
        # goStraight(+20, interface, motors)
        time.sleep(0.001)
            
        ## Maybe add a break after x seconds



    
## NOT YET TESTED    
def Right90deg(interface, motors):    
    turn(-90, interface, motors)
    
## NOT YET TESTED    
def Left90deg(interface, motors):    
    turn(90, interface, motors)
        

    
def turn(angleDeg, interface, motors):
    angle = degtorad(angleDeg)
   
    interface.increaseMotorAngleReferences(motors, [angle, -angle])

    while not interface.motorAngleReferencesReached(motors) :
        motorAngles = interface.getMotorAngles(motors)
        time.sleep(0.001)
        
        
def degtorad(deg):
    radianPerDegree = 0.0562222222
    angle = deg * radianPerDegree
    return angle