def readSpeedM(motor): if motor == 1: return roboclaw.ReadSpeedM1(addr1) elif motor == 2: return roboclaw.ReadSpeedM2(addr1) elif motor == 3: return roboclaw.ReadSpeedM1(addr2) else: raise mlibExcept(e_type.motorNumOff)
def displayspeed(): enc1 = roboclaw.ReadEncM1(address) enc2 = roboclaw.ReadEncM2(address) speed1 = roboclaw.ReadSpeedM1(address) speed2 = roboclaw.ReadSpeedM2(address) print("Encoder1:"), if (enc1[0] == 1): print enc1[1], print format(enc1[2], '02x'), else: print "failed", print "Encoder2:", if (enc2[0] == 1): print enc2[1], print format(enc2[2], '02x'), else: print "failed ", print "Speed1:", if (speed1[0]): print speed1[1], else: print "failed", print("Speed2:"), if (speed2[0]): print speed2[1] else: print "failed "
def SeekNext(t): # Get speeds and currents (a,cur1,cur2) = rc.ReadCurrents(address)y w1 = rc.ReadSpeedM1(address) w2 = rc.ReadSpeedM2(address) if(a): # Find increments to PWM thisError1 = target1-cur1 increment1 = _kp*(thisError1)+_kd*(thisError1-lastError1)+_kw*(w1) thisError2 = target2-cur2 increment2 = _kp*(thisError2)+_kd*(thisError2-lastError2)+_kw*(w2) # Update errors lastError1 = thisError1 lastError2 = thisError2 PWM1 = PWM1 + increment1 PWM2 = PWM2 + increment2 if PWM1 > 255: PWM1 = 255 if PWM1 < -255: PWM1 = -255 if PWM2 > 255: PWM2 = 255 if PWM2 < -255: PWM2 = -255 if PWM1 >= 0: rc.ForwardM1(address,PWM1) else: rc.BackwardM1(address,-PWM1) if PWM2 >= 0: rc.ForwardM2(address,PWM2) else: rc.BackwardM2(address,-PWM2)
def falling(): print('') print('FALLING: Here I go falling!') beginning = time.clock() count = 0 index = 0 lastError1 = 0 lastError2 = 0 pwm1 = 0 pwm2 = 0 while time.clock() - beginning < 0.8: target1, target2 = targetsList(index) (a, cur1, cur2) = rc.ReadCurrents(address) w1 = rc.ReadSpeedM1(address) w2 = rc.ReadSpeedM2(address) # TODO: Save all fields collected during control loop if (a): # Find increments to PWM thisError1 = target1 - cur1 increment1 = kp * thisError1 + kd * (thisError1 - lastError1) + kw * w1 thisError2 = target2 - cur2 increment2 = kp * thisError2 + kd * (thisError2 - lastError2) + kw * w2 # Update errors lastError1 = thisError1 lastError2 = thisError2 pwm1 = +increment1 pwm2 = +increment2 if pwm1 > 255: pwm1 = 255 if pwm1 < -255: pwm1 = -255 if pwm2 > 255: pwm2 = 255 if pwm2 < -255: pwm2 = -255 if pwm1 >= 0: rc.ForwardM1(address, pwm1) else: rc.BackwardM1(address, -pwm1) if pwm2 >= 0: rc.ForwardM2(address, pwm2) else: rc.BackwardM2(address, -pwm2) count = +1 index = count % timing if index > numberOfTargets: index = numberOfTargets else: rc.ForwardM1(address, 0) rc.ForwardM2(address, 0) return [0] * 5