In this code:
#region VEXcode Generated Robot Configuration
from vex import *
import urandom
# Brain should be defined by default
brain=Brain()
# Robot configuration code
brain_inertial = Inertial()
left_drive_smart = Motor(Ports.PORT6, 0.75, True)
right_drive_smart = Motor(Ports.PORT7, 0.75, False)
drivetrain = SmartDrive(left_drive_smart, right_drive_smart, brain_inertial, 200)
Lifter = Motor(Ports.PORT4, False)
H_Wheel = Motor(Ports.PORT5, False)
Combine = Motor(Ports.PORT10, False)
# Make random actually random
def setRandomSeedUsingAccel():
wait(100, MSEC)
xaxis = brain_inertial.acceleration(XAXIS) * 1000
yaxis = brain_inertial.acceleration(YAXIS) * 1000
zaxis = brain_inertial.acceleration(ZAXIS) * 1000
urandom.seed(int(xaxis + yaxis + zaxis))
# Set random seed
setRandomSeedUsingAccel()
vexcode_initial_drivetrain_calibration_completed = False
def calibrate_drivetrain():
# Calibrate the Drivetrain Inertial
global vexcode_initial_drivetrain_calibration_completed
sleep(200, MSEC)
brain.screen.print("Calibrating")
brain.screen.next_row()
brain.screen.print("Inertial")
brain_inertial.calibrate()
while brain_inertial.is_calibrating():
sleep(25, MSEC)
vexcode_initial_drivetrain_calibration_completed = True
brain.screen.clear_screen()
brain.screen.set_cursor(1, 1)
#endregion VEXcode Generated Robot Configuration
global distTraveled
def posPID(setPoint):
wTime = 15
distTraveled = 0
prevError = 0
power = 0
derivative = 0
while True: #some condition
error = setPoint - distTraveled
derivative = error - prevError
prevError = error
kP = 0.1
kD = 0.1
power = error*kP + derivative*kD
drivetrain.set_drive_velocity(power, PERCENT)
toTravel = drivetrain.velocity(RPM) * 200 / 60 / wTime
drivetrain.drive_for(FORWARD, toTravel, MM)
distTraveled = distTraveled + toTravel
if (error <= setPoint + 1) and (error >= setPoint - 1):
break
wait(wTime)
pass
pass
brain_inertial.calibrate()
brain_inertial.set_heading(0, DEGREES)
brain_inertial.set_rotation(0, DEGREES)
posPID(276) # Drives 1 tile length
the robot dosent move. If I replace toTravel with a constant (i.e. 78), the robot will go ~78 mm. By my calculations, toTravel should be 15.5555 on the first iteration. However, if I add:
brain.screen.print(toTravel)
wait(20000000)
after defining the value for toTravel, it shows the value as 0.00.
Does anyone know what is wrong?