def motorControl():
global leftMotorSpeed
global initialMotorSpeed
global PD_value
global rightMotorSpeed
leftMotorSpeed = initialMotorSpeed - PD_value;
rightMotorSpeed = initialMotorSpeed + PD_value;
#leftMotorSpeed = leftMotorSpeed + 10
leftMotorSpeed = np.clip(leftMotorSpeed, 0, maxSpeed)
rightMotorSpeed = np.clip(rightMotorSpeed, 0, maxSpeed)
motor.setLeftMotorSpeed(leftMotorSpeed)
motor.setRightMotorSpeed(rightMotorSpeed)
motor.move()
pwmRight = maxSpeed - abs(power)
pwmLeft = maxSpeed + abs(power)
else:
pwmRight = maxSpeed + power
pwmLeft = maxSpeed - power
pwmRight = pwmRight + 3
if (pwmRight > 100):
pwmRight = 100
elif (pwmRight < -100):
pwmRight = -100
if (pwmLeft > 100):
pwmLeft = 100
elif (pwmLeft < -100):
pwmLeft = -100
print str(pwmLeft) + "\t" + str(pwmRight) + "\t" + str(
power) + "\t" + str(lost)
motor.setLeftMotorSpeed(pwmLeft)
motor.setRightMotorSpeed(pwmRight)
motor.move()
finally:
print "Goodbye :)"
motor.clear()
#os.system('sudo halt')
time.sleep(0.05)