pitch = [] roll = [] pitchPIDController = PIDController() rollPIDController = PIDController() count = 0 while True: startTime = time.time() orientation = imu.getOrientation() driveAngle = getDriveAngle(math.radians(orientation.roll), math.radians(orientation.pitch), zeroAngle) driveSpeedY = pitchPIDController.calculateSpeed(0, orientation.pitch) driveSpeedX = rollPIDController.calculateSpeed(0, orientation.roll) print "speedX:", driveSpeedX, "speedY:", driveSpeedY driveSpeed = (driveSpeedX**2 + driveSpeedY**2)**(.5) driveSpeed = min(driveSpeed, 50) twitch.move(driveAngle, driveSpeed) count = count + 1 pitch.append(orientation.pitch) roll.append(orientation.roll) print colored('----------------------------------', 'cyan') print colored('roll = {0:.2f}, pitch = {1:.2f}', 'white').format(orientation.roll, orientation.pitch) print "direction:", driveAngle, "speed:", driveSpeed print "PID useful time:", (time.time() - startTime) sleep(max(.04 - (time.time() - startTime), 0)) print "PID controller loop time:", time.time() - startTime # twitch.stop() # def average(s): return sum(s) * 1.0 / len(s)
import time import math from Omnibot import * twitch = Omnibot() twitch.move(math.radians(77), 2) time.sleep(2) while True: #twitch.move(math.radians(77), 2) time.sleep(60.0 / 400 / 50)
import time import math from Omnibot import * zeroAngle = math.radians(103) twitch = Omnibot() twitch.move(0, 2) while True: time.sleep(.1)
pitch = [] roll = [] pitchPIDController = PIDController() rollPIDController = PIDController() count = 0 while True: startTime = time.time() orientation = imu.getOrientation() driveAngle = getDriveAngle(math.radians(orientation.roll), math.radians(orientation.pitch), zeroAngle) driveSpeedY = pitchPIDController.calculateSpeed(0, orientation.pitch) driveSpeedX = rollPIDController.calculateSpeed(0, orientation.roll) print "speedX:", driveSpeedX, "speedY:", driveSpeedY driveSpeed = (driveSpeedX ** 2 + driveSpeedY ** 2) ** (.5) driveSpeed = min(driveSpeed, 50) twitch.move(driveAngle, driveSpeed) count = count + 1 pitch.append(orientation.pitch) roll.append(orientation.roll) print colored('----------------------------------','cyan') print colored('roll = {0:.2f}, pitch = {1:.2f}','white') .format(orientation.roll, orientation.pitch) print "direction:", driveAngle, "speed:", driveSpeed print "PID useful time:", (time.time() - startTime) sleep(max(.04 - (time.time() - startTime), 0)) print "PID controller loop time:", time.time() - startTime # twitch.stop() # def average(s): return sum(s) * 1.0 / len(s)
import time import math from Omnibot import * twitch = Omnibot() twitch.move(math.radians(77), 2) time.sleep(2) while True: #twitch.move(math.radians(77), 2) time.sleep(60.0 / 400 / 50)