<s> from termcolor import colored <EOL> from time import sleep <EOL> import math <EOL> from IMU import * <EOL> from Omnibot import * <EOL> from PID import * <EOL> imu = IMU ( <NUM_LIT:1> ) <EOL> zeroAngle = math . radians ( <NUM_LIT> ) <EOL> twitch = Omnibot ( ) <EOL> def getDriveAngle ( x , y , zeroAngle ) : <EOL> fallingAngle = math . atan2 ( x , y ) <EOL> if fallingAngle < <NUM_LIT:0> : <EOL> fallingAngle = fallingAngle + <NUM_LIT:2> * math . pi <EOL> drivingAngle = fallingAngle + math . pi + zeroAngle <EOL> if drivingAngle > <NUM_LIT:2> * math . pi : <EOL> drivingAngle = drivingAngle - <NUM_LIT:2> * math . pi <EOL> return drivingAngle <EOL> def getDriveSpeed ( x , y ) : <EOL> drivingSpeed = <NUM_LIT:200> * ( x ** <NUM_LIT:2> + y ** <NUM_LIT:2> ) ** ( <NUM_LIT> ) <EOL> return drivingSpeed <EOL> pitch = [ ] <EOL> roll = [ ] <EOL> pitchPIDController = PIDController ( ) <EOL> rollPIDController = PIDController ( ) <EOL> count = <NUM_LIT:0> <EOL> while True : <EOL> startTime = time . time ( ) <EOL> orientation = imu . getOrientation ( ) <EOL> driveAngle = getDriveAngle ( math . radians ( orientation . roll ) , math . radians ( orientation . pitch ) , zeroAngle ) <EOL> driveSpeedY = pitchPIDController . calculateSpeed ( <NUM_LIT:0> , orientation . pitch ) <EOL> driveSpeedX = rollPIDController . calculateSpeed ( <NUM_LIT:0> , orientation . roll ) <EOL> print "<STR_LIT>" , driveSpeedX , "<STR_LIT>" , driveSpeedY <EOL> driveSpeed = ( driveSpeedX ** <NUM_LIT:2> + driveSpeedY ** <NUM_LIT:2> ) ** ( <NUM_LIT> ) <EOL> driveSpeed = min ( driveSpeed , <NUM_LIT:50> ) <EOL> twitch . move ( driveAngle , driveSpeed ) <EOL> count = count + <NUM_LIT:1> <EOL> pitch . append ( orientation . pitch ) <EOL> roll . append ( orientation . roll ) <EOL> print colored ( '<STR_LIT>' , '<STR_LIT>' ) <EOL> print colored ( '<STR_LIT>' , '<STR_LIT>' ) . format ( orientation . roll , orientation . pitch ) <EOL> print "<STR_LIT>" , driveAngle , "<STR_LIT>" , driveSpeed <EOL> print "<STR_LIT>" , ( time . time ( ) - startTime ) <EOL> sleep ( max ( <NUM_LIT> - ( time . time ( ) - startTime ) , <NUM_LIT:0> ) ) <EOL> print "<STR_LIT>" , time . time ( ) - startTime </s>
