Program 2 (Fall 2024)
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 = 12
degreesPerCentirmeter=360/wheelCircumference
swingTurnDiameter = distanceBetweenWheels * 2
gearRatio=28/20
function spinGears (distance) {
acceleration = distance*.1
atTopSpeed = distance*.8
deceleration = distance*.1
stepMotors( A , D , 50 ,acceleration, atTopSpeed, deceleration)
sleep(1000)
}
function driveForward (distance) {
acceleration =gearRatio* degreesPerCentirmeter * distance*.1
atTopSpeed =gearRatio* degreesPerCentirmeter * distance*.8
deceleration = gearRatio* degreesPerCentirmeter * distance*.1
stepMotors( B , C , 80 ,acceleration, atTopSpeed, deceleration)
waitHereWhile getMotorSpeed(B) == 0
waitHereUntil getMotorSpeed(B) == 0
}
function driveBackward(distance, power=25){ // If no power was passed into the function, then set a default power level of 25%
acceleration =gearRatio* degreesPerCentirmeter * distance*.1
atTopSpeed =gearRatio* degreesPerCentirmeter * distance*.8
deceleration = gearRatio* degreesPerCentirmeter * distance*.1
stepMotors( B , C , -85 ,acceleration, atTopSpeed, deceleration)
waitHereWhile getMotorSpeed(B) == 0
waitHereUntil getMotorSpeed(B) == 0
beep()
}
function moveForward (distance) {
acceleration =gearRatio* degreesPerCentirmeter * distance*.1
atTopSpeed =gearRatio* degreesPerCentirmeter * distance*.8
deceleration = gearRatio* degreesPerCentirmeter * distance*.1
stepMotors( B , C , 80 ,acceleration, atTopSpeed, deceleration)
waitHereWhile getMotorSpeed(B) == 0
waitHereUntil getMotorSpeed(B) == 0
stopAllMotors(true)
}
function swingTurnLeft(degreesRobotShouldTurn = 90) {
motorDegreesPerRobotDegree = (swingTurnDiameter*pi / wheelCircumference )
acceleration = 0.25 *gearRatio* degreesRobotShouldTurn * motorDegreesPerRobotDegree
atTopSpeed = 0.70 *gearRatio* degreesRobotShouldTurn * motorDegreesPerRobotDegree
deceleration = 0.15 *gearRatio* degreesRobotShouldTurn * motorDegreesPerRobotDegree
stepMotor(C, 50, acceleration, atTopSpeed, deceleration) //reduced pwr from 50
waitHereWhile getMotorSpeed(C)==0
waitHereUntil getMotorSpeed(C)==0
stopAllMotors(true)
}
function swingTurnRight(degreesRobotShouldTurn = 90) {
motorDegreesPerRobotDegree = (swingTurnDiameter*pi / wheelCircumference )
acceleration = 0.25 *gearRatio* degreesRobotShouldTurn * motorDegreesPerRobotDegree
atTopSpeed = 0.70 *gearRatio* degreesRobotShouldTurn * motorDegreesPerRobotDegree
deceleration = 0.15 *gearRatio* degreesRobotShouldTurn * motorDegreesPerRobotDegree
stepMotor(C, -50, acceleration, atTopSpeed, deceleration) //reduced pwr from 50
waitHereWhile getMotorSpeed(C)==0
waitHereUntil getMotorSpeed(C)==0
stopAllMotors(true)
}
function swingTurnRight2(degreesRobotShouldTurn = 90) {
motorDegreesPerRobotDegree = (pi * swingTurnDiameter) / wheelCircumference
acceleration = 0.25 * degreesRobotShouldTurn * motorDegreesPerRobotDegree
atTopSpeed = 0.70 * degreesRobotShouldTurn * motorDegreesPerRobotDegree
deceleration = 0.15 * degreesRobotShouldTurn * motorDegreesPerRobotDegree
stepMotor(B, 50, acceleration, atTopSpeed, deceleration)
waitHereWhile getMotorSpeed(B)==0
waitHereUntil getMotorSpeed(B)==0
stopAllMotors(true)
}
function swingTurnLeft2(degreesRobotShouldTurn = 90) {
motorDegreesPerRobotDegree = (pi * swingTurnDiameter) / wheelCircumference
print('I am here')
print(motorDegreesPerRobotDegree)
acceleration = 0.25 * degreesRobotShouldTurn * motorDegreesPerRobotDegree
atTopSpeed = 0.70 * degreesRobotShouldTurn * motorDegreesPerRobotDegree
deceleration = 0.15 * degreesRobotShouldTurn * motorDegreesPerRobotDegree
print(acceleration)
stepMotor(B, -50, acceleration, atTopSpeed, deceleration)
waitHereWhile getMotorSpeed(B)==0
waitHereUntil getMotorSpeed(B)==0
stopAllMotors(true)
}
function moveBackward(distance) {
if (distance > 0) {
acceleration = gearRatio* degreesPerCentirmeter * distance*.1
atTopSpeed = gearRatio* degreesPerCentirmeter * distance*.8
deceleration = gearRatio* degreesPerCentirmeter * distance*.1
stepMotors( B , C , -50 ,acceleration, atTopSpeed, deceleration)
waitHereWhile getMotorSpeed(B) == 0
waitHereUntil getMotorSpeed(B) == 0
stopAllMotors(true)
} else {
syncMotors( B, C, -25)
}
}
function turnVector(newVector) {
currentVector=vectorValue()
diffrince= abs(newVector-currentVector)
//alert(diffrince)
if newVector-currentVector > 0 swingTurnLeft(diffrince)
if newVector-currentVector < 0 swingTurnRight(diffrince)
}
function geforce(){
resetVector(90)
moveForward (60)
turnVector(180)
moveForward (8)
moveBackward(5)
turnVector(0)
// Accessory Motor Example 1
// Run the accessory motors for n degrees or until it is stalled.
// The abs() function converts the encoderValue() to a postive amount
// even if turning in a direction that would create negative numbers.
setMotor( A, -40 )
sleep( 100 )
resetEncoder( A )
sleep(100)
waitHereUntil isMotorMoving(A)
waitHereUntil abs(encoderValue( A )) > 330 or isMotorStalled(A)
stopAllMotors()
sleep(2000)
moveForward(1)
// Accessory Motor Example 1
// Run the accessory motors for n degrees or until it is stalled.
// The abs() function converts the encoderValue() to a postive amount
// even if turning in a direction that would create negative numbers.
setMotorSpeed( A, 100 )
sleep( 100 )
resetEncoder( A )
sleep(100)
waitHereUntil isMotorMoving(A)
waitHereUntil abs(encoderValue( A )) > 175 or isMotorStalled(A)
stopAllMotors()
sleep(2000)
beep()
moveForward(2)
}
function can(){
moveForward (74)
setMotor( A,-43.3 )
sleep(750)
setMotor(A,0)
moveBackward(73)
alert("aleti kaldir")
setMotor( D,-70)
sleep(750)
setMotor(D,0)
setMotor( D,70 )
sleep(750)
setMotor(D,0)
}
function drawMenu() {
clearScreen()
rect(10, 10, 150, 110)
drawText(40, 25,"ZA")
drawText(40,40,"ES")
drawText(40,55,"CS5000")
drawText(40,70,"CS")
drawText(40,85,"AB")
drawText(40,100,"SW")
}
option = 1
function clearCursor() {
drawText(20, option*15+10, ' ')
}
function drawCursor() {
drawText(20, option*15+10, '>>')
}
forever{
while(true) {
drawMenu()
drawCursor()
key=waitForPress()
clearCursor()
if key==1 option-=1
if key==3 option+=1
if option<1 option = 1
if option>6 option = 6
drawCursor()
if key==2 break
}
if option==1 {
alert('ZA2')
driveForward(65)
swingTurnRight2(10)
moveForward(1)
spinGears(360)
syncMotors( B, C )
driveBackward(5)
swingTurnLeft2(20)
syncMotors( B, C )
driveBackward(60)
alert("ZA 3")
driveForward(65)
swingTurnRight2(2)
driveForward(16)
spinGears(100)
syncMotors( B, C )
driveBackward(5)
swingTurnLeft2`(15)
syncMotors( B, C )
driveBackward(65)
} else if option==2 {
alert('apple cat')
moveForward(85)
setMotor( A, -100 )
sleep(750)
setMotor(A,0)
moveBackward(85)
} else if option==3 {
alert('CS 5000')
moveForward(180)
} else if option==4 {
alert('CS su')
can()
} else if option==5 {
alert("Coral and mast")
geforce()
} else if option==6 {
driveBackward(15)
alert('Did you move backwards?')
beep()
swingTurnLeft2(20)
alert('Did you turn Left?')
}
option++
}
