Program 5 (Fall 2023)
The code below is a set of functions, variables, constants, and algorithms that are the work of RoboCatz for the Fall 2023 season.
const pi = 3.14159
wheelDiameter = 5.6
wheelCircumference = wheelDiameter * pi
distanceBetweenWheels = 90
gearRatio = 28/20
degreesPerCentimeter =360/wheelCircumference
upDirection=1
downDirection=-1
function moveForwardLizard (distance) {
acceleration = degreesPerCentimeter * gearRatio * distance*.1 //20.571 is degrees per centimeter
atTopSpeed = degreesPerCentimeter * gearRatio * distance*.8
deceleration = degreesPerCentimeter * gearRatio * distance*.1
//beep()
print(`${acceleration} ${atTopSpeed} ${deceleration}`)
stepMotors( B , C , 50 ,acceleration, atTopSpeed, deceleration)
waitHereWhile getMotorSpeed(B) == 0
waitHereUntil getMotorSpeed(B) == 0
stopAllMotors(true)
beep(100,8000,1000)
}
function moveBackwardLizard(distance) {
acceleration = degreesPerCentimeter * gearRatio * distance*.05
atTopSpeed = degreesPerCentimeter * gearRatio * distance*.9
deceleration = degreesPerCentimeter * gearRatio *distance*.05
if ( acceleration == 0 ) alert('sorry. try again.')
stepMotors( B , C , -50 ,acceleration, atTopSpeed, deceleration)
waitHereWhile getMotorSpeed(B) == 0
sleep(100)
waitHereUntil getMotorSpeed(B) == 0
stopAllMotors(true)
}
function liftDaLever(power=55){
setMotor(A,power)
setMotor(D,power)
sleep(1000)
waitHereUntil isMotorStalled(A) and isMotorStalled(D)
beep(100,800,10)
stopAllMotors()
resetEncoder(A)
resetEncoder(D)
}
function lowerDaLever(power=-55){
setMotor(A,power)
setMotor(D,power)
sleep(1000)
waitHereUntil isMotorStalled(A) and isMotorStalled(D)
beep(100,800,10)
stopAllMotors()
resetEncoder(A)
resetEncoder(D)
}
// Lizard Mission
liftDaLever()
moveForwardLizard(25)
lowerDaLever()
moveBackwardLizard(25)
waitHereUntil touchSensorPressed()
function resetAccessoryMotors(power=55) {
setMotor(A,power)
setMotor(D,power)
sleep(1000)
waitHereUntil isMotorStalled(A) and isMotorStalled(D)
beep(100,800,10)
stopAllMotors()
resetEncoder(A)
resetEncoder(D)
}
function motorsUp(motor,degree){
print(`Working on motor ${motor} tp degree ${degree}`)
originalEncoderValue = encoderValue(motor)
setMotor(motor,40*updirection)
print(`Motor should have power now of ${40*updirection}`)
waitHereUntil abs(encoderValue(motor)-originalEncoderValue) > degree
print(encoderValue(motor))
stopAllMotors()
}
function moveBothDown(degree){
originalEncoderValue = encoderValue(A)
syncMotors(A, D, 40*downDirection)
waitHereUntil abs(encoderValue(A)-originalEncoderValue) > degree
stopAllMotors()
}
function moveBackward (distance) {
acceleration = degreesPerCentimeter * gearRatio * distance*.1 //20.571 is degrees per centimeter
atTopSpeed = degreesPerCentimeter * gearRatio * distance*.8
deceleration = degreesPerCentimeter * gearRatio * distance*.1
print(`${acceleration} ${atTopSpeed} ${deceleration}`)
stopAllMotors(true),sleep(100)
stepMotors( B , C , 30 ,acceleration, atTopSpeed, deceleration)
waitHereWhile getMotorSpeed(B) == 0
waitHereUntil getMotorSpeed(B) == 0
stopAllMotors(true)
}
function moveForward(distance) {
acceleration = degreesPerCentimeter * gearRatio * distance*.05
atTopSpeed = degreesPerCentimeter * gearRatio * distance*.9
deceleration = degreesPerCentimeter * gearRatio *distance*.05
if ( acceleration == 0 ) alert('sorry. try again.')
currGyro=gyroSensorValue()
startingEncoder=encoderValue(B)
stopAllMotors(true), sleep(100)
await stepMotors( B , C , -50 ,acceleration) // Accelerate
while abs(startingEncoder-encoderValue(B))