Program 3 (Fall 2021)
The code below is a set of functions, variables, constants, and algorithms that are the work of RoboCatz for the Fall 2021 season.
//hi
function driveForward(inches, power) {
acceleration=inches*71*.1
atSpeed=inches*71*.8
deceleration=inches*71*.1
stepMotors( B, C, -1 * power, acceleration, atSpeed, deceleration, true, true )
}
function driveForwardSync(inches, power) {
resetEncoder(B)
syncMotors( B, C, -1 * power)
while(abs(encoderValue(B))<71*inches){
sleep(5)
}
stopAllMotors(true)
}
function driveBackward(inches, power) {
acceleration=inches*71*.1
atSpeed=inches*71*.8
deceleration=inches*71*.1
stepMotors( B, C, power, acceleration, atSpeed, deceleration, true, true )
}
function swingTurnRight(degrees) {
acceleration=degrees*5.35*.2
atSpeed=degrees*5.35*.6
deceleration=degrees*5.35*.2
stepMotor( B, -30, acceleration, atSpeed, deceleration, true, true )
}
function swingTurnLeft(degrees) {
acceleration=degrees*5.35*.2
atSpeed=degrees*5.35*.6
deceleration=degrees*5.35*.2
stepMotor( B, 30, acceleration, atSpeed, deceleration, true, true )
}
function swingTurnRightC(degrees) {
acceleration=degrees*5.35*.2
atSpeed=degrees*5.35*.6
deceleration=degrees*5.35*.2
stepMotor( C, -30, acceleration, atSpeed, deceleration, true, true )
}
function swingTurnLeftC(degrees) {
acceleration=degrees*5.35*.2
atSpeed=degrees*5.35*.6
deceleration=degrees*5.35*.2
stepMotor( C, 30, acceleration, atSpeed, deceleration, true, true )
}
function driveForwardFastDecel(inches, power) {
accleration=inches*71*.1
atSpeed=inches*71*.86
deceleration=inches*71*.04
stepMotors( B, C, -1 * power, accleration, atSpeed, deceleration, true, true )
}
function openArms() {
setMotor(A,20)
setMotor(D,-20)
sleep(500)
//shorten sleep time
stopAllMotors()
}
function closeArms() {
setMotor(A,-50)
setMotor(D,50)
sleep(500)
//shorten sleep time
stopAllMotors()
}
function getChicken() {
openArms()
driveForward(26,80)
//shorten drive forward
closeArms()
driveBackward(30,80)
openArms()
sleep(3000)
beep()
}
function getChicken2() {
//openArms()
beep(2000)
driveForward(28,80)
swingTurnRight(45)
driveForward(6,80)
swingTurnLeft(45)
//closeArms()
driveBackward(30,80)
//openArms()
sleep(5000)
beep()
}
function deliverChicken() {
stopAllMotors()
driveForward(23,80)
driveBackward(23,50)
waitForPress()
}
// The colorSensorValue() function returns an object with three properties: r, g, b
// If defined as a literal, the object would look like:
// colorSensorValueObject = { r: redValue, g: greenValue, b: blueValue }
//
// In the example below, the object is assigned to the variable 'x'
/*
while(true) {
x=colorSensorValue() // Assign the color sensor value object to the variable x
drawText( 10, 10, 'Red: ' + x.r, 2 ) // Display the value for red
drawText( 10, 40, 'Green:' + x.g, 2 ) // Display the value for green
drawText( 10, 70, 'Blue: ' + x.b, 2 ) // Display the value for blue
sleep(500) // Wait for 500 milliseconds
clearScreen() // Clear the screen
} // Repeat
*/
menu= [
{ programNumber: 1, label: 'Get Chicken' }, //red (Get chicken)
{ programNumber: 2, label: 'Deliver Chicken' }, //green (Deliver Chicken)
{ programNumber: 3, label: 'Deliver Package' }, //yellow (Deliver Package)
{ programNumber: 4, label: 'Get blue box' }, //black (Get Blue Box)
{ programNumber: 5, label: 'brown ' }, //brown
{ programNumber: 6, label: 'blue ' }, //blue
{ programNumber: 7, label: 'Get chicken 2 ' }, //orange
{ programNumber: 8, label: 'grey ' } //grey
]
foundColor=false
color={}
programNumber=0
menuDegrees=180
while(true) {
programNumber=0
setLED(6)
clearScreen()
for(i=0;i<6;i++) drawText( 0, i*20, menu[i].label,2)
usingColorSensor=true
resetEncoder(B)
resetEncoder(C)
stopAllMotors()
oldIndex=1;
oldEncoderValue=encoderValue(B)
arrayIndex=0
while(buttonPressed(2)==0 || programNumber==0) {
if(oldEncoderValue!=encoderValue(B)) { // When the encoder value has changed
oldEncoderValue=encoderValue(B)
// Which portion of the menu have we rotated to?
portion=abs(oldEncoderValue)/menuDegrees
arrayIndex=portion % filters.length // do not exceed the array bounds
}
newIndex = 0
// Convert arrayIndex to an integer
if(arrayIndex >= 1) newIndex = 1
if(arrayIndex >= 2) newIndex = 2
if(arrayIndex >= 3) newIndex = 3
if(arrayIndex >= 4) newIndex = 4
if(arrayIndex >= 5) newIndex = 5
if(arrayIndex >= 6) newIndex = 6
// If the index has changed, then show it
if(oldIndex!=newIndex) {
clearRect(0, oldIndex*20, 178, 20);
drawText( 0, oldIndex*20, menu[oldIndex].label,2)
fillRect(0, newIndex*20, 178, 20);
drawWhiteText( 0, newIndex*20, menu[arrayIndex].label,2)
oldIndex = newIndex
programNumber=menu[arrayIndex].programNumber
}
sleep(50)
}
stopAllMotors()
resetEncoder(B)
resetEncoder(C)
setLED(5)
beep(30, 220, 30)
clearScreen()
sleep(1000)
switch(programNumber) {
case 1://Get Chicken from its circle and bring it back.
drawText( 0, 5, 'Red Mission', 2)
drawText( 0, 30, 'Get the', 2)
drawText( 0, 55, 'Robot', 2)
drawText( 0, 80, 'Ready', 2)
while(buttonPressed(2)==0) { sleep(20) }
beep(30, 440, 30)
setLED(4)
sleep(500)
getChicken()
waitForPress()
break
case 2://Take Chicken back out in coop with chicken feed.
drawText( 0, 5, 'Green Mission', 2)
drawText( 0, 30, 'Get the', 2)
drawText( 0, 55, 'Robot', 2)
drawText( 0, 80, 'Ready', 2)
while(buttonPressed(2)==0) { sleep(20) }
beep(30, 440, 30)
setLED(4)
sleep(500)
deliverChicken()
break
case 3:// Deliver the Airplane Wing and Package
drawText( 0, 5, 'Yellow Mission', 2)
drawText( 0, 30, 'Get the', 2)
drawText( 0, 55, 'Robot', 2)
drawText( 0, 80, 'Ready', 2)
while(buttonPressed(2)==0) { sleep(20) }
beep(30, 440, 30)
setLED(4)
sleep(500)
driveForward(10, 30)
defaultPower = 40
constantPower = 10
currPower = 0
direction = -1
resetEncoder(B)
while(abs(encoderValue(B))<71*12) { // 71 degrees per inch times 12 inches
currPower = lightSensorPct(4)/100 * defaultPower
setMotor(C, direction * (currPower + constantPower))
setMotor(B, direction * ((defaultPower - currPower) + constantPower))
}
driveForward(9, 35)
stopAllMotors()
resetEncoder(B)
while(abs(encoderValue(B))<71*17) { // 71 degrees per inch times 16 inches
currPower = lightSensorPct(4)/100 * defaultPower
setMotor(C, direction * (currPower + constantPower))
setMotor(B, direction * ((defaultPower - currPower) + constantPower))
driveBackward(8, 20)
//add drive backward
}
stopAllMotors()
syncMotors( B, C, -1 * 30)
sleep(2000)
stopAllMotors()
swingTurnRight(20)
driveBackward(6, 20)
swingTurnLeft(20)
break
case 4://Get the blue box
drawText( 0, 5, 'Black Mission', 2)
drawText( 0, 30, 'Get the', 2)
drawText( 0, 55, 'Robot', 2)
drawText( 0, 80, 'Ready', 2)
while(buttonPressed(2)==0) { sleep(20) }
beep(30, 440, 30)
setLED(4)
sleep(500)
driveForward(20, 60)
swingTurnLeftC(150)
driveBackward(20, 100)
break //This is our mission
case 5:
drawText( 0, 5, 'Brown Mission', 2)
drawText( 0, 30, 'Get the', 2)
drawText( 0, 55, 'Robot', 2)
drawText( 0, 80, 'Ready', 2)
while(buttonPressed(2)==0) { sleep(20) }
beep(30, 440, 30)
setLED(4)
sleep(500)
driveForward(6, 60)
swingTurnRight(45)
beep(100, 1000, 1000)
break
case 6:
drawText( 0, 5, 'Blue Mission', 2)
drawText( 0, 30, 'Get the', 2)
drawText( 0, 55, 'Robot', 2)
drawText( 0, 80, 'Ready', 2)
while(buttonPressed(2)==0) { sleep(20) }
beep(30, 440, 30)
setLED(4)
sleep(500)
break
case 7:
drawText( 0, 5, 'Orange Mission', 2)
drawText( 0, 30, 'Get the', 2)
drawText( 0, 55, 'Robot', 2)
drawText( 0, 80, 'Ready', 2)
while(buttonPressed(2)==0) { sleep(20) }
beep(30, 440, 30)
setLED(4)
sleep(500)
stopAllMotors()
stepMotors( B, C, -20 , 720, 720, 720, true, true )
stopAllMotors()
sleep(1000)
getChicken2()
break
case 8:
drawText( 0, 5, 'Gray Mission', 2)
drawText( 0, 30, 'Get the', 2)
drawText( 0, 55, 'Robot', 2)
drawText( 0, 80, 'Ready', 2)
while(buttonPressed(2)==0) { sleep(20) }
beep(30, 440, 30)
setLED(4)
sleep(500)
break
default:
alert('none'+color.r+' '+color.g+' '+color.b)
}
}
