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
def default():
for k in range(1, 4):
mlib.setMVelocityPID(k, p, i, d, q)
def default():
for k in range(1,4):
mlib.setMVelocityPID(k,p,i,d,q)
