def __init__(self):
"""
"""
# Hyperparameters
self.LAMBDA = 1.047 # Angle of the depth sensor 60*pi/180 but in rad!!!!!
self.N_RASTERPOINTS = 224 # Amount of values in one row of the depth sensor matrix 224
self.MATRIX_SIZE = 100 # 1000
self.SCALE_SENSOR_TO_CM = 100 # 100
self.EPS_DISTANCE = 5 # cells
# robot parameters
self.x_pos = int(round(self.MATRIX_SIZE / 2))
self.y_pos = int(round(self.MATRIX_SIZE / 2))
# matrix
self.ma_room = [[0 for i in range(self.MATRIX_SIZE)]
for j in range(self.MATRIX_SIZE)]
# set the robot into the room
self.ma_room[self.x_pos][self.y_pos] = -1
# sensor input
self.distances = 0
self.angleToNorth = 0
self.picoAngle = 0
self.motors = MotorControl()
self.rangeSens = PicoFlexxSens(self)
self.rangeSens.start()
# starting side methods
self.socketCo = SocketManager(self)
self.socketCo.start()
# Clean up distance sensors
tof.cleanup()
print("Exiting")
exit()
# Attach the Ctrl+C signal interrupt
signal.signal(signal.SIGINT, ctrlC)
# Setup encoder
encoder = Encoder()
encoder.initEncoders()
# Setup motor control
motorControl = MotorControl(encoder)
orientation = Orientation(encoder, motorControl)
tof = TOF()
pid = PID(0.5, -6, 6)
try:
with open('calibratedSpeeds.json') as json_file:
motorControl.speedMap = json.load(json_file)
except IOError:
input("You must calibrate speeds first. Press enter to continue")
motorControl.calibrateSpeeds()
from motorControl import MotorControl as MC
import getch
MovCnt = MC()
while 1:
ctl = getch.getch()
if (ctl == "w"):
MovCnt.MoveForward()
elif (ctl == "s"):
MovCnt.MoveBackward()
elif (ctl == "q"):
MovCnt.Stopper()
elif (ctl == "A"):
MovCnt.TankSteerLeft()
elif (ctl == "D"):
MovCnt.TankSteerRight()
elif (ctl == "R"):
MovCnt.PWMClean()
elif (ctl == "i"):
MovCnt.IncreaseSpeed()
from serialCommunication import SerialCommunication
from motorControl import MotorControl
from servoControl import ServoControl
import time
port = "/dev/ttyAMA0"
baud_rate = 115200
command_terminator = "\x07"
TICKS_PER_REV = 96
TICKS_PER_DEG = TICKS_PER_REV / 360.0
serial_connection = SerialCommunication(port=port, baud_rate=baud_rate)
try:
motor_controller = MotorControl(serialConnection=serial_connection, command_terminator=command_terminator, debug=False)
servo_controller = ServoControl(serialConnection=serial_connection, command_terminator=command_terminator, debug=False)
# motor_controller.reset_encoders_count()
last1Ticks = 0
last2Ticks = 0
print("Hello World")
speed = 0
while True:
try:
motor_controller.move_at_percentage((speed, speed))
except:
print("Failure")
leftTicks = motor_controller.get_encoder_1_count()
rightTicks = motor_controller.get_encoder_2_count()
#if leftTicks != last1Ticks or rightTicks != last2Ticks:
from motorControl import MotorControl as MC
import getch
import time
MovCnt = MC()
for i in range(40, 55):
print(i)
MovCnt.TankSteerLeft(49)
time.sleep(3)
MovCnt.TankSteerLeft(60)
time.sleep(3)
# input()
from motorControl import MotorControl as MC
import getch
MovCnt = MC()
while 1:
ctl = getch.getch()
if (ctl == 'w'):
MovCnt.MoveForward()
elif (ctl == 's'):
MovCnt.MoveBackward()
elif (ctl == 'q'):
MovCnt.Stopper()
elif (ctl == 'a'):
MovCnt.MoveLeft()
elif (ctl == 'd'):
MovCnt.MoveRight()
import RPi.GPIO as GPIO
from motorControl import MotorControl as MC
GPIO.setmode(GPIO.BOARD)
MovCnt = MC()
GPIO.setup(29, GPIO.IN)
GPIO.setup(31, GPIO.IN)
GPIO.setup(33, GPIO.IN)
GPIO.setup(35, GPIO.IN)
GPIO.setup(37, GPIO.IN)
while 1:
s4 = GPIO.input(29)
s3 = GPIO.input(31)
s2 = GPIO.input(33)
s1 = GPIO.input(35)
s0 = GPIO.input(37)
if (s2 == 0):
print('Forward')
MovCnt.MoveForward()
elif ((s1 == 0 or s0 == 0)):
print('Turn Left')
MovCnt.MoveLeft()
elif ((s3 == 0 or s4 == 0)):
print('Turn Right')
MovCnt.MoveRight()
else:
print('Stop')
#MovCnt.Stopper()
#print('| '+str(s1)+','+str(s2)+','+str(s3)+'|')
class Main:
"""
Be carfull!!! every thing is :
angles in rad
distances in cm
"""
def __init__(self):
"""
"""
# Hyperparameters
self.LAMBDA = 1.047 # Angle of the depth sensor 60*pi/180 but in rad!!!!!
self.N_RASTERPOINTS = 224 # Amount of values in one row of the depth sensor matrix 224
self.MATRIX_SIZE = 100 # 1000
self.SCALE_SENSOR_TO_CM = 100 # 100
self.EPS_DISTANCE = 5 # cells
# robot parameters
self.x_pos = int(round(self.MATRIX_SIZE / 2))
self.y_pos = int(round(self.MATRIX_SIZE / 2))
# matrix
self.ma_room = [[0 for i in range(self.MATRIX_SIZE)]
for j in range(self.MATRIX_SIZE)]
# set the robot into the room
self.ma_room[self.x_pos][self.y_pos] = -1
# sensor input
self.distances = 0
self.angleToNorth = 0
self.picoAngle = 0
self.motors = MotorControl()
self.rangeSens = PicoFlexxSens(self)
self.rangeSens.start()
# starting side methods
self.socketCo = SocketManager(self)
self.socketCo.start()
def startMapping(self):
if 0: # Start Main routine
# initialy get 360 view of the first position
self.process360view()
# store current distance
dis = self.distances[round(
len(self.distances) / 2)] * self.SCALE_SENSOR_TO_CM
# move forward approximately 1 meter
self.runMotors(2)
# update the robot position
self.update_position(
self.distances[round(len(self.distances) / 2)] *
self.SCALE_SENSOR_TO_CM - dis)
# get 360 view of the second position
self.process360view()
# turn to left
self.turnRobot(-90)
# store current distance
dis = self.distances[round(
len(self.distances) / 2)] * self.SCALE_SENSOR_TO_CM
# move forward approximately 1 meter
self.runMotors(2)
# update the robot position
self.update_position(
self.distances[round(len(self.distances) / 2)] *
self.SCALE_SENSOR_TO_CM - dis)
# get 360 view of the third position
self.process360view()
else: # Start Debug routine
self.distances = [2, 2, 2]
self.angleToNorth = math.pi
self.update_matrix()
self.update_position(0)
print(np.array(self.ma_room))
print(self.x_pos)
print(self.y_pos)
def update_matrix(self):
for act_pos in range(len(self.distances)):
print("distance: " +
str(self.distances[act_pos] * self.SCALE_SENSOR_TO_CM))
d = self.distances[act_pos] * self.SCALE_SENSOR_TO_CM
n = round(act_pos - math.floor(len(self.distances) / 2))
e1 = int(
round(self.x_pos +
math.sin(self.angleToNorth + self.picoAngle +
n * self.LAMBDA / self.N_RASTERPOINTS) * d))
e2 = int(
round(self.y_pos +
math.cos(self.angleToNorth + self.picoAngle +
n * self.LAMBDA / self.N_RASTERPOINTS) * d))
if e1 >= 0 and e1 < self.MATRIX_SIZE and e2 >= 0 and e2 < self.MATRIX_SIZE:
self.ma_room[e1][e2] = 1
self.socketCo.send(str(e1) + "," + str(e2) + "," + str(1))
print("sending targetted packet:" + str(e1) + "," + str(e2))
#self.socketCo.sendMatrix()
def update_position(self, distance_difference):
self.ma_room[int(round(self.x_pos))][int(round(self.y_pos))] = 0
self.x_pos = self.x_pos + math.sin(
self.angleToNorth + self.picoAngle) * distance_difference
self.y_pos = self.y_pos + math.cos(
self.angleToNorth + self.picoAngle) * distance_difference
self.ma_room[int(round(self.x_pos))][int(round(self.y_pos))] = -1
def turnPico(self, angle):
# extern
return 0
def runMotors(self, sec): # Run Motors for a defined amount of time
self.motors.driveForward(sec)
return 0
def runMotorsForward(self): # Run Motors until stopped
# extern
return 0
def stopMotors(self): # Stop Motors
self.motors.stop()
return 0
def runMotorsBackwards(self):
self.motors.driveBackward(2)
return 0
def turnRobot(self, angle):
# extern
return 0
def turnRobotToAbsoluteAngle(self, absolute_angle):
#extern
return 0
def process360view(self):
for i in range(6):
self.update_matrix()
self.turnPico(60)
def moveToPosition(self, x, y):
# Need to distuingish different cases
phi = -1
if x - self.x_pos >= 0 and y - self.y_pos >= 0:
phi = math.atan((x - self.x_pos) / (y - self.y_pos))
elif x - self.x_pos >= 0 and y - self.y_pos <= 0:
phi = math.atan((self.y_pos - y) / (x - self.x_pos)) + math.pi / 2
elif x - self.x_pos <= 0 and y - self.y_pos <= 0:
phi = math.atan((self.x_pos - x) / (self.y_pos - y)) + math.pi
elif x - self.x_pos <= 0 and y - self.y_pos >= 0:
phi = math.atan(
(y - self.y_pos) / (self.x_pos - x)) + math.pi * 270 / 360
phi = phi * 180 / math.pi
self.turnRobotToAbsoluteAngle(phi)
last_position = np.array([self.x_pos, self.y_pos])
self.runMotorsInf()
while np.linalg.norm(
np.array([x, y]) -
np.array([self.x_pos, self.y_pos])) > self.EPS_DISTANCE:
self.update_position(
np.linalg.norm(
np.array([self.x_pos, self.y_pos]) - last_position))
last_position = np.array([self.x_pos, self.y_pos])
# sleep(100) # Needed?
self.stopMotors()
def first(command):
return command
GAME_TIME = 110
port = "/dev/ttyAMA0"
baud_rate = 115200
command_terminator = "\x07"
# Add a list of PSOC pins below this so we can directly access them
# Add a list of Raspi pins below this so we can directly access them
LED_PIN = 21
raspi = Raspi(led_pin = LED_PIN, debug = True) # Turns on LED as well
serial_connection = SerialCommunication(port=port, baud_rate=baud_rate)
motor_controller = MotorControl(serialConnection=serial_connection, command_terminator=command_terminator, debug=True)
servo_controller = ServoControl(serialConnection=serial_connection, command_terminator=command_terminator, debug=True)
sensor_controller = SensorControl(serialConnection=serial_connection, command_terminator=command_terminator, debug=True)
# # Need to add arguments to the class constructors
#
# activeCommand = first(DriveBackUntilTouch())
# .then(CommandGroup([Turn(90, speed), LowerTrunk()])
# .then(StartConveyorbelt())
# .ifSuccessful(
# Turn(90, speed)
# .then(Drive(-10, speed))
# .then(FollowLineBackwards())
# .then(RaiseBucket())
# .ifFailure(
# DriveBackUntilTouch()
def main():
motors = MotorControl()
motors.disable()
time.sleep(5)
print("starting cw")
for ii in range(100):
throttle = (0.01 * ii) * 0.5 + 0.5
motors.cw(throttle)
motors.disable()
time.sleep(5)
print("starting ccw")
for ii in range(100):
throttle = (0.01 * ii) * 0.5 + 0.5
motors.ccw(throttle)
motors.disable()
time.sleep(5)
print("starting forward")
for ii in range(100):
throttle = (0.01 * ii) * 0.5 + 0.5
motors.forward(throttle)
motors.disable()
time.sleep(5)
print("starting backward")
for ii in range(100):
throttle = (0.01 * ii) * 0.5 + 0.5
motors.backward(throttle)
motors.disable()
time.sleep(5)
print("starting right")
for ii in range(100):
throttle = 0.01 * ii
motors.right(throttle)
motors.disable()
time.sleep(5)
print("starting left")
for ii in range(100):
throttle = 0.01 * ii
motors.left(throttle)
motors.disable()
time.sleep(5)
print("end of script")
throttle = (0.01 * ii) * 0.5 + 0.5
motors.backward(throttle)
motors.disable()
time.sleep(5)
print("starting right")
for ii in range(100):
throttle = 0.01 * ii
motors.right(throttle)
motors.disable()
time.sleep(5)
print("starting left")
for ii in range(100):
throttle = 0.01 * ii
motors.left(throttle)
motors.disable()
time.sleep(5)
print("end of script")
if __name__ == '__main__':
try:
while True:
main()
except KeyboardInterrupt:
motors = MotorControl()
motors.disable()