Program 1 (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
pointTurnCircumference=distanceBetweenWheels*pi
gearRatio=28/20
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 pointTurnRight(degreesRobotShouldTurn = 90) {
let motorDegreesPerRobotDegree = (pointTurnCircumference / wheelCircumference )
let acceleration = 0.15 * gearRatio * degreesRobotShouldTurn * motorDegreesPerRobotDegree
let atTopSpeed = 0.70 * gearRatio * degreesRobotShouldTurn * motorDegreesPerRobotDegree
let deceleration = 0.15 * gearRatio * degreesRobotShouldTurn * motorDegreesPerRobotDegree
stepMotor(B, 25, acceleration, atTopSpeed, deceleration) // Pass the parameters to the stepMotor() function.
stepMotor(C, -25, acceleration, atTopSpeed, deceleration) // Notice that one of the motors has positive speed and the other negative speed.
// Now check the speed of the motors to see if the robot has started or completed the maneuver.
// Because two motors are being used to make the turn, you will need to check the speed of both motors.
// The stepMotor() function always starts with the speed at zero.
waitHereWhile getMotorSpeed(B)==0 || getMotorSpeed(C)==0 // Wait here while the robot has not yet started to move.
waitHereUntil getMotorSpeed(B)==0 && getMotorSpeed(C)==0 // Wait here as the motor is turning and only exit from this line when the speed returns to back to zero.
stopAllMotors(true) // Apply the brake to the motor(s)
}
function pointTurnLeft(degreesRobotShouldTurn = 90) {
let motorDegreesPerRobotDegree = (pointTurnCircumference / wheelCircumference )
let acceleration = 0.15 * gearRatio * degreesRobotShouldTurn * motorDegreesPerRobotDegree
let atTopSpeed = 0.70 * gearRatio * degreesRobotShouldTurn * motorDegreesPerRobotDegree
let deceleration = 0.15 * gearRatio * degreesRobotShouldTurn * motorDegreesPerRobotDegree
stepMotor(C, 25, acceleration, atTopSpeed, deceleration) // Pass the parameters to the stepMotor() function.
stepMotor(B, -25, acceleration, atTopSpeed, deceleration) // Notice that one of the motors has positive speed and the other negative speed.
// Now check the speed of the motors to see if the robot has started or completed the maneuver.
// Because two motors are being used to make the turn, you will need to check the speed of both motors.
// The stepMotor() function always starts with the speed at zero.
waitHereWhile getMotorSpeed(B)==0 || getMotorSpeed(C)==0 // Wait here while the robot has not yet started to move.
waitHereUntil getMotorSpeed(B)==0 && getMotorSpeed(C)==0 // Wait here as the motor is turning and only exit from this line when the speed returns to back to zero.
stopAllMotors(true) // Apply the brake to the motor(s)
}
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, 25, 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, -25, acceleration, atTopSpeed, deceleration) //reduced pwr from 50
waitHereWhile getMotorSpeed(C)==0
waitHereUntil getMotorSpeed(C)==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 pointTurnLeft(diffrince)
if newVector-currentVector < 0 pointTurnRight(diffrince)
}
function geforce(){
resetVector(90)
moveForward (60)
turnVector(180)
moveForward (8)
moveBackward(5)
turnVector(90)
turnVector(10)
//alert(vectorValue())
// 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, -35 )
sleep( 100 )
resetEncoder( A )
sleep(100)
waitHereUntil isMotorMoving(A)
waitHereUntil abs(encoderValue( A )) > 315 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 )) > 45 or isMotorStalled(A)
stopAllMotors()
sleep(2000)
moveForward(1)
setMotorSpeed( A, 100 )
//sleep( 100 )
resetEncoder( A )
sleep(100)
waitHereUntil isMotorMoving(A)
waitHereUntil abs(encoderValue( A )) > 100 or isMotorStalled(A)
stopAllMotors()
sleep(2000)
beep()
moveForward(2)
}
function can(key){
if key == 2 {
syncMotors( B, C, 25 )
sleep(7000)
stopAllMotors()
setMotor( A,-43.3 )
sleep(750)
setMotor(A,0)
syncMotors( B, C, -25 )
sleep(7000)
stopAllMotors()
}
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,"CS")
drawText(40, 40,"Coral and mast")
drawText(40,55,"C5000")
drawText(40,70,"ES")
drawText(40,85,"ZA")
}
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>5 option = 5
drawCursor()
if key==2 or key==5 break
}
if option==1 {
alert("CS")
can(key)
} else if option==2 {
alert("Coral and mast")
geforce()
} else if option==3 {
alert("can5000")
moveForward(210)
} else if option==4 {
alert("ES")
moveForward(85)
setMotor( A, -100 )
sleep(1500)
setMotor(A,0)
moveBackward(85)
} else if option==5 {
alert("ZA2")
}
option++
}
