Program 2 (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 // In centimeters
wheelCircumference = wheelDiameter * pi
distanceBetweenWheels = 12 // In centimeters
gearRatio = 28/20
swingTurnCircumference = distanceBetweenWheels * 2 * pi
degreesPerCentimeter =360/wheelCircumference
upDirection=1
downDirection=-1
forwardDirection=-1
backwardDirection=1
function resetAccessoryMotors(power=55) {
resetEncoder(A)
resetEncoder(D)
setMotor(A,power)
setMotor(D,power)
sleep(1000)
waitHereUntil (isMotorStalled(A) and isMotorStalled(D)) or abs(encoderValue(A))>1000 or abs(encoderValue(B))>1000
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
//beep()
//print(`${acceleration} ${atTopSpeed} ${deceleration}`)
stopAllMotors(true),sleep(100)
await stepMotors( B , C , 30 ,acceleration, atTopSpeed, deceleration)
//waitHereWhile getMotorSpeed(B) == 0
//waitHereUntil getMotorSpeed(B) == 0
//stopAllMotors(true)
// beep(100,8000,1000)
}
function swingTurnRight(degreesRobotShouldTurn = 90) {
motorDegreesPerRobotDegree = (swingTurnCircumference / wheelCircumference ) * gearRatio
acceleration = 0.25 * degreesRobotShouldTurn * motorDegreesPerRobotDegree
atTopSpeed = 0.70 * degreesRobotShouldTurn * motorDegreesPerRobotDegree
deceleration = 0.15 * degreesRobotShouldTurn * motorDegreesPerRobotDegree
stopAllMotors(true), sleep(100)
await stepMotor(B, 25, acceleration, atTopSpeed, deceleration)
}
function swingTurnLeft(degreesRobotShouldTurn = 10) {
motorDegreesPerRobotDegree = (swingTurnCircumference / wheelCircumference ) * gearRatio
acceleration = 0.10 * degreesRobotShouldTurn * motorDegreesPerRobotDegree
atTopSpeed = 0.80 * degreesRobotShouldTurn * motorDegreesPerRobotDegree
deceleration = 0.10 * degreesRobotShouldTurn * motorDegreesPerRobotDegree
stopAllMotors(true), sleep(100)
await stepMotor(B, -35, acceleration, atTopSpeed, deceleration)
}
function moveForward(distance, power=50) {
if (distance*1 > 0) {
let gain=6
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 , forwardDirection*power, acceleration) // Accelerate
while abs(startingEncoder-encoderValue(B)) 0) {
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 , forwardDirection*power ,acceleration, atTopSpeed)
waitHereWhile getMotorSpeed(B) == 0
sleep(100)
waitHereUntil getMotorSpeed(B) == 0
stopAllMotors(true)
} else {
syncMotors( B, C, -25)
}
}
function moveForwardNoDeceleration(distance, power=50) {
if (distance*1 > 0) {
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 , forwardDirection*power ,acceleration, atTopSpeed) // Accelerate and AtTopSpeed
} else {
syncMotors( B, C, -25)
}
}
function swingTurnLeftFast(degreesRobotShouldTurn = 10) {
motorDegreesPerRobotDegree = (swingTurnCircumference / wheelCircumference ) * gearRatio
acceleration = 0.10 * degreesRobotShouldTurn * motorDegreesPerRobotDegree
atTopSpeed = 0.80 * degreesRobotShouldTurn * motorDegreesPerRobotDegree
deceleration = 0.10 * degreesRobotShouldTurn * motorDegreesPerRobotDegree
stopAllMotors(true), sleep(100)
await stepMotor(C, 50, acceleration, atTopSpeed, deceleration)
stopAllMotors(true), sleep(100)
}
function swingTurnLeftThree(degreesRobotShouldTurn = 10) {
motorDegreesPerRobotDegree = (swingTurnCircumference / wheelCircumference ) * gearRatio
acceleration = 0.10 * degreesRobotShouldTurn * motorDegreesPerRobotDegree
atTopSpeed = 0.80 * degreesRobotShouldTurn * motorDegreesPerRobotDegree
deceleration = 0.10 * degreesRobotShouldTurn * motorDegreesPerRobotDegree
stopAllMotors(true), sleep(100)
stepMotor(B, -50, acceleration, atTopSpeed, deceleration)
waitHereWhile getMotorSpeed(B)==0
waitHereUntil getMotorSpeed(B)==0
stopAllMotors(true)
}
//stops motor C and moves motor B backwards
function swingTurnRightTwo(degreesRobotShouldTurn = 10) {
motorDegreesPerRobotDegree = (swingTurnCircumference / wheelCircumference ) * gearRatio
acceleration = 0.10 * degreesRobotShouldTurn * motorDegreesPerRobotDegree
atTopSpeed = 0.80 * degreesRobotShouldTurn * motorDegreesPerRobotDegree
deceleration = 0.10 * degreesRobotShouldTurn * motorDegreesPerRobotDegree
stopAllMotors(true), sleep(100)
stepMotor(B, 50, acceleration, atTopSpeed, deceleration)
waitHereWhile getMotorSpeed(B)==0
waitHereUntil getMotorSpeed(B)==0
stopAllMotors(true)
}
function swingTurnLeftBackwards(degreesRobotShouldTurn = 90) {
motorDegreesPerRobotDegree = (swingTurnCircumference / wheelCircumference ) * gearRatio
acceleration = 0.25 * degreesRobotShouldTurn * motorDegreesPerRobotDegree
atTopSpeed = 0.70 * degreesRobotShouldTurn * motorDegreesPerRobotDegree
deceleration = 0.15 * degreesRobotShouldTurn * motorDegreesPerRobotDegree
await stepMotor(C, -50, acceleration, atTopSpeed, deceleration)
stopAllMotors(true)
}
function colorTurnerKey(){
clearScreen()
resetGyroSensor()
//alert('You selected ENTER')
syncMotors(B,C,20), sleep(50)
stopAllMotors(true), sleep(100)
moveForward(67, 70) // distance, power
swingTurnLeft(50)
moveBackward(5)
ForwardAccelerationOnly(8)
moveBackward(6)
sleep(300) // Allow a fraction of a second for the model to advance to the next color
ForwardAccelerationOnly(8)
moveBackward(2)
setLED(5)
sleep(200)
//moveBackward(60)
//swingTurnRightTwo(44)
}
// Mission Color Turner
colorTurnerKey()
// Get ready to do mission Virtual 3D
swingTurnLeftFast(140 - gyroSensor())
//alert('ok')
// Drive straight to try to find the white part of the line
if ( lightSensorPct(2) < 30 ) {
syncMotors( B, C, 30 ) // Start moving
while( lightSensorPct(2) < 30 ) { // while the light sensor sees a black ground
drawText(50, 60, lightSensorPct(2), 2) // Show the lightSensor Percentage
sleep(10) // sleep for 1/10th of a second
} // repeat the loop
}
stopAllMotors() // line edge has been detected
// Follow the line
target = 40
gain = 1.5
resetEncoder( B )
while abs(encoderValue(B))< 600 { // Distance to follow line is 600 degrees on motor B
drawText(10,10, gain*(lightSensorPct(2)-target)+' ', 2)
syncMotors(B, C, 20, gain*(lightSensorPct(2)-target))
clearRect(0,90, 178, 38) // clears the bottom of the LCD screen
fillRect((90-30*0.5)-(lightSensorPct(2)-target),90, 30, 38)
sleep(30)
}
stopAllMotors(true), sleep(100) // Rest
resetEncoder( B )
resetEncoder( C )
// Make a zig-zag maneuver
swingTurnRight(35) // Turn slightly toward the center model
moveBackward(11) // Drive toward the center model
swingTurnLeftFast(90 - gyroSensor()) // Turn to face East (which is 90 degrees on the compass)
moveBackward(4) // Keep moving East
resetAccessoryMotors(40)
swingTurnLeftFast(180 - gyroSensor()) // Turn toward the 3d Model which is 180 degrees on the compass
// Get ready to do mission Virtual 3D
moveBackward(5) // Move toward the 3D Model
motorsUp(A,700) // Activate the Virtual 3D model
resetAccessoryMotors(40)
stopAllMotors(true)
moveForward(12) // Back away from the model
swingTurnLeftBackwards(40)// Turn toward base
moveForward(30) // Back away from the center of the board
resetEncoder( B )
resetEncoder( C )
swingTurnLeftBackwards(gyroSensor() - 100) // Turn more toward base
moveForwardNoDeceleration(60, 100) // Drive toward base at 100% power
