Program 9 (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
print('im on line 8')
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
//beep()
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)
// beep(100,8000,1000)
}
function swingTurnRight(degreesRobotShouldTurn = 90) {
motorDegreesPerRobotDegree = (swingTurnCircumference / wheelCircumference )
acceleration = 0.25 * degreesRobotShouldTurn * motorDegreesPerRobotDegree
atTopSpeed = 0.70 * degreesRobotShouldTurn * motorDegreesPerRobotDegree
deceleration = 0.15 * degreesRobotShouldTurn * motorDegreesPerRobotDegree
stopAllMotors(true), sleep(100)
stepMotor(B, 25, acceleration, atTopSpeed, deceleration)
sleep(100)
waitHereWhile getMotorSpeed(B)==0
sleep(100)
waitHereUntil getMotorSpeed(B)==0
stopAllMotors(true)
}
function moveForward(distance) {
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 , -50 ,acceleration) // Accelerate
while abs(startingEncoder-encoderValue(B)) 30 ) { // while the light sensor sees a white ground
drawText(50, 60, lightSensorPct(2), 2) // Show the lightSensor Percentage
sleep(100) // sleep for 1/10th of a second
} // repeat the loop
stopAllMotors() // line has been detected
sleep(500)
// Follow the line
target = 40
gain = 1.5
resetEncoder( B )
while abs(encoderValue(B))< 600.00 { // 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(100)
}
stopAllMotors(true), sleep(300) // Rest
resetEncoder( B )
resetEncoder( C )
// Make a zig-zag maneuver
swingTurnRight(45) // Turn slightly toward the center model
moveBackward(13) // Drive toward the center model
swingTurnLeft(45) // Turn to face East
moveBackward(20) // Keep moving East
swingTurnLeftThree(180) // Turn toward the 3d Model
resetAccessoryMotors(40)
