def main():
#default speed
speed = .1
while 1:
action = get_action()
if action == 'UP':
mlib.goXYOmegaWorld(speed,0)
elif action == 'LEFT':
mlib.goXYOmegaWorld(0,speed)
elif action == 'DOWN':
mlib.goXYOmegaWorld(-speed,0)
elif action == 'RIGHT':
mlib.goXYOmegaWorld(0,-speed)
elif action == 'SPIN_CCW':
mlib.goXYOmegaWorld(0,0,1+speed)
elif action == 'SPIN_CW':
mlib.goXYOmegaWorld(0,0,-1-speed)
elif action == 'STOP':
mlib.stop()
elif action == 'SPEED_DEC':
speed = speed - .1
print("Speed: ",speed)
elif action == 'SPEED_INC':
speed = speed + .1
print("Speed: ",speed)
elif action == 'KILL':
return
elif action == 'KICK':
mlib.kick()
sys.exit(1)
def main():
#default speed
speed = .1
while 1:
action = get_action()
if action == 'UP':
mlib.goXYOmegaWorld(speed, 0)
elif action == 'LEFT':
mlib.goXYOmegaWorld(0, speed)
elif action == 'DOWN':
mlib.goXYOmegaWorld(-speed, 0)
elif action == 'RIGHT':
mlib.goXYOmegaWorld(0, -speed)
elif action == 'SPIN_CCW':
mlib.goXYOmegaWorld(0, 0, 1 + speed)
elif action == 'SPIN_CW':
mlib.goXYOmegaWorld(0, 0, -1 - speed)
elif action == 'STOP':
mlib.stop()
elif action == 'SPEED_DEC':
speed = speed - .1
print("Speed: ", speed)
elif action == 'SPEED_INC':
speed = speed + .1
print("Speed: ", speed)
elif action == 'KILL':
return
elif action == 'KICK':
mlib.kick()
sys.exit(1)
def roboControl(data):
## Decisions ##
if data.cmdType == 'movefast':
globals.xy_limit = globals.fast_limit
if data.x == data.x and data.y == data.y and data.theta == data.theta: # check for NaN
count = 0
vx, vy, omega = pid.robot_ctrl(data)
if vx != vx or vy != vy or omega != omega:
print("ERROR with NaN")
pid.reset()
return
# store last valid values in case of NaN
vx_valid = vx
vy_valid = vy
omega_valid = omega
# Go, baby, go
mlib.goXYOmegaWorld(vx, vy, omega, mlib.deg2rad(data.theta))
else: # we got a NaN
count = count + 1
if count < 50: # Only try and go backwards for a little bit
mlib.goXYOmegaWorld(-vx_valid, -vy_valid, -omega_valid,
mlib.deg2rad(data.theta))
else: # if you get NaNs for a while, just stop moving. There clearly a bigger issue.
mlib.stop()
elif data.cmdType == 'moveslow':
globals.xy_limit = globals.slow_limit
if data.x == data.x and data.y == data.y and data.theta == data.theta: # check for NaN
count = 0
vx, vy, omega = pid.robot_ctrl(data)
if vx != vx or vy != vy or omega != omega:
print("ERROR with NaN")
pid.reset()
return
# store last valid values in case of NaN
vx_valid = vx
vy_valid = vy
omega_valid = omega
# Go, baby, go
mlib.goXYOmegaWorld(vx, vy, omega, mlib.deg2rad(data.theta))
else:
count = count + 1
if count < 50: # Only try and go backwards for a little bit
mlib.goXYOmegaWorld(-vx_valid, -vy_valid, -omega_valid,
mlib.deg2rad(data.theta))
else: # if you get NaNs for a while, just stop moving. There clearly a bigger issue.
mlib.stop()
elif data.cmdType == 'kick':
if data.x == data.x and data.y == data.y and data.theta == data.theta: # check for NaN
mlib.kick()
elif data.cmdType == 'idle':
mlib.stop()
def roboControl(data):
## Decisions ##
if data.cmdType == 'movefast':
globals.xy_limit = globals.fast_limit
if data.x == data.x and data.y == data.y and data.theta == data.theta: # check for NaN
count = 0
vx, vy, omega = pid.robot_ctrl(data)
if vx != vx or vy != vy or omega != omega:
print("ERROR with NaN")
pid.reset()
return
# store last valid values in case of NaN
vx_valid = vx
vy_valid = vy
omega_valid = omega
# Go, baby, go
mlib.goXYOmegaWorld(vx,vy,omega,mlib.deg2rad(data.theta))
else: # we got a NaN
count = count + 1
if count < 50: # Only try and go backwards for a little bit
mlib.goXYOmegaWorld(-vx_valid,-vy_valid,-omega_valid,mlib.deg2rad(data.theta))
else: # if you get NaNs for a while, just stop moving. There clearly a bigger issue.
mlib.stop()
elif data.cmdType == 'moveslow':
globals.xy_limit = globals.slow_limit
if data.x == data.x and data.y == data.y and data.theta == data.theta: # check for NaN
count = 0
vx, vy, omega = pid.robot_ctrl(data)
if vx != vx or vy != vy or omega != omega:
print("ERROR with NaN")
pid.reset()
return
# store last valid values in case of NaN
vx_valid = vx
vy_valid = vy
omega_valid = omega
# Go, baby, go
mlib.goXYOmegaWorld(vx,vy,omega,mlib.deg2rad(data.theta))
else:
count = count + 1
if count < 50: # Only try and go backwards for a little bit
mlib.goXYOmegaWorld(-vx_valid,-vy_valid,-omega_valid,mlib.deg2rad(data.theta))
else: # if you get NaNs for a while, just stop moving. There clearly a bigger issue.
mlib.stop()
elif data.cmdType == 'kick':
if data.x == data.x and data.y == data.y and data.theta == data.theta: # check for NaN
mlib.kick()
elif data.cmdType == 'idle':
mlib.stop()
def calibrate():
print "==Initial Values==\n"
print 'Battery Voltage: ' + str(ReadMainBatteryVoltage(128))
crc, p1, i1, d1, q1 = mlib.readMVelocityPID(1)
print "M1 P= ", p1
print "M1 I= ", i1
print "M1 D= ", d1
print "M1 QPPS=", q1
crc, p2, i2, d2, q2 = mlib.readMVelocityPID(2)
print "M2 P= ", p2
print "M2 I= ", i2
print "M2 D= ", d2
print "M2 QPPS= ", q2
crc, p3, i3, d3, q3 = mlib.readMVelocityPID(3)
print "M3 P= ", p3
print "M3 I= ", i3
print "M3 D= ", d3
print "M3 QPPS= ", q3
# This function takes multiple samples of the motor speed and then returns the average
def read(motor):
samples = 2
result = 0
for k in range(0, samples):
sample = 0
sample = mlib.readSpeedM(motor)[1]
#print sample
result = result + sample
result = result / samples
return result
speedM1Forward = 0
speedM1Backward = 0
speedM2Forward = 0
speedM2Backward = 0
speedM3Forward = 0
speedM3Backward = 0
speed = 50
mlib.stop()
## Forward Back
#Forwards
mlib.BackwardM(1, speed)
#M1 backward sample 1
mlib.ForwardM(3, speed)
#M3 forward sample 1
time.sleep(2)
speedM1Backward = speedM1Backward + read(1)
speedM3Forward = speedM3Forward + read(3)
mlib.stop()
time.sleep(1)
#Backwards
mlib.ForwardM(1, speed)
#M1 forward sample 1
mlib.BackwardM(3, speed)
#M3 backward sample 1
time.sleep(2)
speedM1Forward = speedM1Forward + read(1)
speedM3Backward = speedM3Backward + read(3)
mlib.stop()
time.sleep(5)
## Diagonal 1
#Left back
mlib.BackwardM(2, speed)
#M2 backward sample 1
mlib.ForwardM(1, speed)
#M1 forward sample 2
time.sleep(2)
speedM2Backward = speedM2Backward + read(2)
speedM1Forward = speedM1Forward + read(1)
speedM1Forward = speedM1Forward / 2
mlib.stop()
time.sleep(1)
#Left forward
mlib.ForwardM(2, speed)
#M2 forward sample 1
mlib.BackwardM(1, speed)
#M1 backward sample 2
time.sleep(2)
speedM2Forward = speedM2Forward + read(2)
speedM1Backward = speedM1Backward + read(1)
speedM1Backward = speedM1Backward / 2
mlib.stop()
time.sleep(5)
## Diagonal 2
# RightBack
mlib.ForwardM(2, speed)
#M2 forward sample 2
mlib.BackwardM(3, speed)
#M3 backward sample 2
time.sleep(2)
speedM2Forward = speedM2Forward + read(2)
speedM2Forward = speedM2Forward / 2
speedM3Backward = speedM3Backward + read(3)
speedM3Backward = speedM3Backward / 2
mlib.stop()
time.sleep(1)
# Right Forward
mlib.BackwardM(2, speed)
#M2 backward sample 2
mlib.ForwardM(3, speed)
#M3 forward sample 2
time.sleep(2)
speedM2Backward = speedM2Backward + read(2)
speedM2Backward = speedM2Backward / 2
speedM3Forward = speedM3Forward + read(3)
speedM3Forward = speedM3Forward / 2
mlib.stop()
# Make Calculations
speedM1Forward = (speedM1Forward * 127) / speed
speedM1Backward = (speedM1Backward * 127) / speed
speedM2Forward = (speedM2Forward * 127) / speed
speedM2Backward = (speedM2Backward * 127) / speed
speedM3Forward = (speedM3Forward * 127) / speed
speedM3Backward = (speedM3Backward * 127) / speed
#print speedM1Forward;
#print speedM1Backward;
#print speedM2Forward;
#print speedM2Backward;
#print speedM3Forward;
#print speedM3Backward;
speedM1 = (speedM1Forward - speedM1Backward) / 2
speedM2 = (speedM2Forward - speedM2Backward) / 2
speedM3 = (speedM3Forward - speedM3Backward) / 2
print "qpps values:"
print speedM1
print speedM2
print speedM3
mlib.setMVelocityPID(1, p, i, d, speedM1)
mlib.setMVelocityPID(2, p, i, d, speedM2)
mlib.setMVelocityPID(3, p, i, d, speedM3)
print "==Final Values==\n"
crc, p1, i1, d1, q1 = mlib.readMVelocityPID(1)
print "M1 P= ", p1
print "M1 I= ", i1
print "M1 D= ", d1
print "M1 QPPS= ", q1
crc, p2, i2, d2, q2 = mlib.readMVelocityPID(2)
print "M2 P= ", p2
print "M2 I= ", i2
print "M2 D= ", d2
print "M2 QPPS= ", q2
crc, p3, i3, d3, q3 = mlib.readMVelocityPID(3)
print "M3 P= ", p3
print "M3 I= ", i3
print "M3 D= ", d3
print "M3 QPPS=", q3
def calibrate():
print "==Initial Values==\n"
print 'Battery Voltage: ' + str(ReadMainBatteryVoltage(128))
crc,p1,i1,d1,q1 = mlib.readMVelocityPID(1)
print "M1 P= ",p1
print "M1 I= ",i1
print "M1 D= ",d1
print "M1 QPPS=",q1
crc,p2,i2,d2,q2 = mlib.readMVelocityPID(2)
print "M2 P= ",p2
print "M2 I= ",i2
print "M2 D= ",d2
print "M2 QPPS= ",q2
crc,p3,i3,d3,q3 = mlib.readMVelocityPID(3)
print "M3 P= ",p3
print "M3 I= ",i3
print "M3 D= ",d3
print "M3 QPPS= ",q3
# This function takes multiple samples of the motor speed and then returns the average
def read(motor):
samples = 2
result = 0
for k in range(0, samples):
sample = 0
sample = mlib.readSpeedM(motor)[1]
#print sample
result = result + sample
result = result/samples
return result
speedM1Forward=0
speedM1Backward=0
speedM2Forward=0
speedM2Backward=0
speedM3Forward=0
speedM3Backward=0
speed = 50
mlib.stop();
## Forward Back
#Forwards
mlib.BackwardM(1,speed); #M1 backward sample 1
mlib.ForwardM(3,speed); #M3 forward sample 1
time.sleep(2)
speedM1Backward=speedM1Backward+read(1)
speedM3Forward=speedM3Forward+read(3)
mlib.stop();
time.sleep(1);
#Backwards
mlib.ForwardM(1,speed); #M1 forward sample 1
mlib.BackwardM(3,speed); #M3 backward sample 1
time.sleep(2)
speedM1Forward=speedM1Forward+read(1)
speedM3Backward=speedM3Backward+read(3)
mlib.stop();
time.sleep(5);
## Diagonal 1
#Left back
mlib.BackwardM(2,speed); #M2 backward sample 1
mlib.ForwardM(1,speed); #M1 forward sample 2
time.sleep(2)
speedM2Backward=speedM2Backward+read(2)
speedM1Forward=speedM1Forward+read(1)
speedM1Forward=speedM1Forward/2
mlib.stop();
time.sleep(1);
#Left forward
mlib.ForwardM(2,speed); #M2 forward sample 1
mlib.BackwardM(1,speed); #M1 backward sample 2
time.sleep(2)
speedM2Forward=speedM2Forward+read(2)
speedM1Backward=speedM1Backward+read(1)
speedM1Backward=speedM1Backward/2
mlib.stop();
time.sleep(5);
## Diagonal 2
# RightBack
mlib.ForwardM(2,speed); #M2 forward sample 2
mlib.BackwardM(3,speed); #M3 backward sample 2
time.sleep(2)
speedM2Forward=speedM2Forward+read(2)
speedM2Forward=speedM2Forward/2
speedM3Backward=speedM3Backward+read(3)
speedM3Backward=speedM3Backward/2
mlib.stop();
time.sleep(1);
# Right Forward
mlib.BackwardM(2,speed); #M2 backward sample 2
mlib.ForwardM(3,speed); #M3 forward sample 2
time.sleep(2)
speedM2Backward=speedM2Backward+read(2)
speedM2Backward=speedM2Backward/2
speedM3Forward=speedM3Forward+read(3)
speedM3Forward=speedM3Forward/2
mlib.stop();
# Make Calculations
speedM1Forward=(speedM1Forward*127)/speed
speedM1Backward=(speedM1Backward*127)/speed
speedM2Forward=(speedM2Forward*127)/speed
speedM2Backward=(speedM2Backward*127)/speed
speedM3Forward=(speedM3Forward*127)/speed
speedM3Backward=(speedM3Backward*127)/speed
#print speedM1Forward;
#print speedM1Backward;
#print speedM2Forward;
#print speedM2Backward;
#print speedM3Forward;
#print speedM3Backward;
speedM1 = (speedM1Forward - speedM1Backward)/2
speedM2 = (speedM2Forward - speedM2Backward)/2
speedM3 = (speedM3Forward - speedM3Backward)/2
print "qpps values:"
print speedM1
print speedM2
print speedM3
mlib.setMVelocityPID(1,p,i,d,speedM1)
mlib.setMVelocityPID(2,p,i,d,speedM2)
mlib.setMVelocityPID(3,p,i,d,speedM3)
print "==Final Values==\n"
crc,p1,i1,d1,q1 = mlib.readMVelocityPID(1)
print "M1 P= ",p1
print "M1 I= ",i1
print "M1 D= ",d1
print "M1 QPPS= ",q1
crc,p2,i2,d2,q2 = mlib.readMVelocityPID(2)
print "M2 P= ",p2
print "M2 I= ",i2
print "M2 D= ",d2
print "M2 QPPS= ",q2
crc,p3,i3,d3,q3 = mlib.readMVelocityPID(3)
print "M3 P= ",p3
print "M3 I= ",i3
print "M3 D= ",d3
print "M3 QPPS=",q3
#!/usr/bin/env python import mlib mlib.stop()
