Program 1 (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(2000)
stopAllMotors()
}
function closeArms() {
setMotor(A,50)
setMotor(D,-50)
sleep(3000)
stopAllMotors()
}
function getChicken() {
openArms()
driveForward(28,80)
closeArms()
driveBackward(30,80)
openArms()
sleep(5000)
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
*/
filters= [
{ r: 256, g: 65, b: 22, programNumber: 1, label: 'Get Chicken' }, //red (Get chicken)
{ r: 45, g: 85, b: 17, programNumber: 2, label: 'Deliver Chicken' }, //green (Deliver Chicken)
{ r: 164, g: 153, b: 31, programNumber: 3, label: 'Deliver Package' }, //yellow (Deliver Package)
{ r: 13, g: 39, b: 12, programNumber: 4, label: 'Get blue box' }, //black (Get Blue Box)
{ r: 35, g: 28, b: 17, programNumber: 5, label: 'brown ' }, //brown
{ r: 46, g: 112, b: 126, programNumber: 6, label: 'blue ' }, //blue
{ r: 342, g: 68, b: 20, programNumber: 7, label: 'Get chicken 2 ' }, //orange
{ r: 158, g: 153, b: 76, programNumber: 8, label: 'grey ' } //grey
]
foundColor=false
color={}
programNumber=0
menuDegrees=180
while(true) {
programNumber=0
setLED(6)
clearScreen()
usingColorSensor=true
resetEncoder(B)
resetEncoder(C)
stopAllMotors()
while(buttonPressed(2)==0 || programNumber==0) {
portion=abs(encoderValue(B))/menuDegrees
arrayIndex=portion % filters.length
drawText( 0,45, filters[arrayIndex].label,2)
// drawText( 0,65, arrayIndex,2)
arrayIndex2=arrayIndex*1
programNumber=filters[arrayIndex2].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*19) { // 71 degrees per inch times 16 inches
currPower = lightSensorPct(4)/100 * defaultPower
setMotor(C, direction * (currPower + constantPower))
setMotor(B, direction * ((defaultPower - currPower) + constantPower))
}
stopAllMotors()
swingTurnRight(20)
driveBackward(6, 20)
swingTurnLeft(20)
break
case 4:
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(10, 60)
swingTurnLeftC(100)
driveForward(10, 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)
}
}
