Self balanced robot V0.0

sbr.py

#!/usr/bin/python
global currentDirMotorA, currentDirMotorB

from sense_hat import SenseHat
import time
import RPi.GPIO as GPIO

GPIO.setmode(GPIO.BCM)

sense = SenseHat()
sense.clear()
sense.set_imu_config(False, True, True)  # compass_enabled, gyro_enabled, accel_enabled

X = [255, 0, 0]  # Red
O = [0, 0, 0]  # Bclak

forward = [
O, O, X, X, O, O, O, O,
O, O, X, X, O, O, O, O,
O, O, X, X, O, O, O, O,
O, O, X, X, O, O, O, O,
O, O, X, X, O, O, O, O,
O, O, X, X, O, O, O, O,
O, O, X, X, O, O, O, O,
O, O, X, X, O, O, O, O
]
center = [
O, O, O, X, X, O, O, O,
O, O, O, X, X, O, O, O,
O, O, O, X, X, O, O, O,
O, O, O, X, X, O, O, O,
O, O, O, X, X, O, O, O,
O, O, O, X, X, O, O, O,
O, O, O, X, X, O, O, O,
O, O, O, X, X, O, O, O
]
backward = [
O, O, O, O, X, X, O, O,
O, O, O, O, X, X, O, O,
O, O, O, O, X, X, O, O,
O, O, O, O, X, X, O, O,
O, O, O, O, X, X, O, O,
O, O, O, O, X, X, O, O,
O, O, O, O, X, X, O, O,
O, O, O, O, X, X, O, O
]

PIN_PWM_A = 18 #ENA
PIN_PWM_B = 19 #ENB
PIN_1_A = 20 #IN1
PIN_2_A = 21 #IN2
PIN_1_B = 16 #IN3
PIN_2_B = 26 #IN4
FREQ = 50
PIN_BTN = 13

GPIO.setup(PIN_PWM_A, GPIO.OUT)
GPIO.setup(PIN_PWM_B, GPIO.OUT)
GPIO.setup(PIN_1_A, GPIO.OUT)
GPIO.setup(PIN_2_A, GPIO.OUT)
GPIO.setup(PIN_1_B, GPIO.OUT)
GPIO.setup(PIN_2_B, GPIO.OUT)
motorA = GPIO.PWM(PIN_PWM_A, FREQ)
motorA.start(0.0) #ici, rapport_cyclique vaut entre 0.0 et 100.0
motorB = GPIO.PWM(PIN_PWM_B, FREQ)
motorB.start(0.0) #ici, rapport_cyclique vaut entre 0.0 et 100.0

GPIO.setup(PIN_BTN, GPIO.IN, pull_up_down=GPIO.PUD_UP)

DIR_FORWARD = 1
DIR_BACKWARD = 2
DIR_STOP = 3

currentDirMotorA = DIR_STOP
currentDirMotorB = DIR_STOP

doDriveMotors = False

# MOTOR A

def stopMotorA():
	global currentDirMotorA
	motorA.ChangeDutyCycle( 0.0 )
	GPIO.output( PIN_1_A, GPIO.LOW )
	GPIO.output( PIN_2_A, GPIO.LOW )
	motorA.ChangeDutyCycle( 100.0 )
	currentDirMotorA = DIR_STOP

def moveForwardMotorA(speed):
	global currentDirMotorA
	if currentDirMotorA == DIR_BACKWARD or currentDirMotorA == DIR_STOP:
		motorA.ChangeDutyCycle( 0.0 )
		GPIO.output( PIN_1_A, GPIO.LOW )
		GPIO.output( PIN_2_A, GPIO.HIGH )

	motorA.ChangeDutyCycle( speed )
	currentDirMotorA = DIR_FORWARD

def moveBackwardMotorA(speed):
	global currentDirMotorA
	if currentDirMotorA == DIR_FORWARD or currentDirMotorA == DIR_STOP:
		motorA.ChangeDutyCycle( 0.0 )
		GPIO.output( PIN_1_A, GPIO.HIGH )
		GPIO.output( PIN_2_A, GPIO.LOW )

	motorA.ChangeDutyCycle( speed )
	currentDirMotorA = DIR_BACKWARD

# MOTOR B

def stopMotorB():
	global currentDirMotorB
	motorB.ChangeDutyCycle( 0.0 )
	GPIO.output( PIN_1_B, GPIO.LOW )
	GPIO.output( PIN_2_B, GPIO.LOW )
	motorB.ChangeDutyCycle( 100.0 )
	currentDirMotorB = DIR_STOP

def moveForwardMotorB(speed):
	global currentDirMotorB
	if currentDirMotorB == DIR_BACKWARD or currentDirMotorB == DIR_STOP:
		motorB.ChangeDutyCycle( 0.0 )
		GPIO.output( PIN_1_B, GPIO.LOW)
		GPIO.output( PIN_2_B, GPIO.HIGH )

	motorB.ChangeDutyCycle( speed )
	currentDirMotorB = DIR_FORWARD

def moveBackwardMotorB(speed):
	global currentDirMotorB
	if currentDirMotorB == DIR_FORWARD or currentDirMotorB == DIR_STOP:
		motorB.ChangeDutyCycle( 0.0 )
		GPIO.output( PIN_1_B, GPIO.HIGH)
		GPIO.output( PIN_2_B, GPIO.LOW )

	motorB.ChangeDutyCycle( speed )
	currentDirMotorB = DIR_BACKWARD

# BTN START / STOP MOTORS

def btnClickCallback(PIN_BTN):
	global doDriveMotors
	doDriveMotors = not doDriveMotors
	if not doDriveMotors:
		stopMotorA()
		stopMotorB()

GPIO.add_event_detect(PIN_BTN, GPIO.FALLING, callback=btnClickCallback, bouncetime=1000)

# CLEAN

def clean():
	sense.clear()
	motorA.stop()
	motorB.stop()
	GPIO.remove_event_detect(PIN_BTN)
	GPIO.cleanup()
	print ("\nCleaned")
	exit()

# MAIN LOOP

#PID constantes
KP = 9
KI = 1
KD = 3
iTerm = 0
lastAngle = 0

def loop():
	global iTerm, lastAngle
	while True:
			orientation = sense.get_orientation_degrees()
			pitch = orientation["pitch"]
			pitch = round(pitch, 2) # axe vertical de la cam
			#roll = orientation["roll"]
			#roll = round(roll, 1)
			#yaw = orientation["yaw"]
			#yaw = round(yaw, 1) # axe horizontal de la cam
			#print("{\"pitch\": %s, \"roll\": %s, \"yaw\": %s}" % (pitch, roll, yaw))

			P_BACKWARD_T = 359.9
			P_FORWARD_T = 0.1
			if pitch > P_FORWARD_T and pitch < 180.0:
				sense.set_pixels(forward)
			if pitch <= P_FORWARD_T or pitch >= P_BACKWARD_T:
				sense.set_pixels(center)
			if pitch < P_BACKWARD_T and pitch > 180.0:
				sense.set_pixels(backward)

			angle = 0
			if pitch >=0 and pitch < 180:
				angle = pitch
			if pitch <= 360 and pitch > 180:
				angle = 360 - pitch
				angle = angle * -1
			pTerm = KP * angle
			iTerm += KI * angle
			dTerm = KD * (angle - lastAngle)
			lastAngle = angle
			output = pTerm + iTerm + dTerm

			print output

			direction = DIR_FORWARD
			if output < 0:
				direction = DIR_BACKWARD
			if output == 0:
				direction = DIR_STOP

			speed = output
			if output < 0:
				speed = speed * -1
			speed = round(speed, 0)
			if speed > 100:
				speed = 100

			print("{\"output\": %s, \"direction\": %s, \"speed\": %s}" % (output, direction, speed))

			if doDriveMotors:
				if direction == DIR_FORWARD:
					moveForwardMotorA(speed)
					moveForwardMotorB(speed)

				if direction == DIR_STOP:
					stopMotorA()
					stopMotorB()

				if direction == DIR_BACKWARD:
					moveBackwardMotorA(speed)
					moveBackwardMotorB(speed)

			time.sleep(.01)

stopMotorA()
stopMotorB()

try:
	loop()
except KeyboardInterrupt:
	clean()