Program 2 (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.
/**
* @param {number} inches
* @param {number} pwr
* @param {number} extraSleep
* @returns {void}
*/
function driveForward(inches, pwr, extraSleep) {
var myPower = 100
if (pwr * 1 > 0) myPower = pwr // Same as: if(typeof pwr == 'number') myPower=pwr*1
resetEncoder(B) // Reset the encoder
var totalDistance = inches * 53
syncMotors(B, C, -1 * myPower)
sleep(extraSleep)
// Start driving forward
//stepMotors(B, C, pwr, totalDistance * .2,totalDistance * .6, totalDistance * .2)
while (abs(encoderValue(B)) < totalDistance - 10) { // Convert inches to degrees by multiplying by 58
clearScreen() // Clear the screen
drawText(10, 10, encoderValue(B)) // Show the encoder value
sleep(100) // Move for a period of time
} // Repeat the loop
beep(100, 1600, 1000)
}
var up = true
var down = false
/**@param {boolean} direction */
function rotateArm(direction) {
if (direction == up) {
setMotor(D, -40)
}
if (direction == down) {
setMotor(D, 40)
}
sleep(500)
stopAllMotors()
}
/**
* @param {number} inches
* @param {number} pwr
* @returns {void}
*/
function driveBackward(inches, pwr) {
beep()
//sleep(5000)
beep()
var myPower = 100
if (pwr * 1 > 0) myPower = pwr // Same as: if(typeof pwr == 'number') myPower=pwr*1
syncMotors(B, C, myPower) // Start driving backward
resetEncoder(B) // Reset the encoder
while (abs(encoderValue(B)) < inches * 53) { // Convert inches to degrees by multiplying by 58
clearScreen() // Clear the screen
drawText(10, 10, encoderValue(B), 2) // Show the encoder value
sleep(100) // Move for a period of time
} // Repeat the loop
// setLED(6)
//sleep(5000)
}
/**
* @param {unknown} degrees
* @returns {void}
*/
function attachRight(degrees) {
setMotor(D, 40)
resetEncoder(D)
waitHereUntil(abs(encoderValue(D)) < 165)
stopAllMotors()
}
/**
* @param {number} forewardTime
* @param {number} backwardeTime
* @returns {void}
*/
function PUSHDATRUCK(forewardTime, backwardeTime) {
setLED(6)
alert(forewardTime)
driveForward(16, 60, 2000)
sleep(forewardTime)
driveBackward(30, 60)
sleep(backwardeTime)
stopAllMotors()
}
function followTheLine() {
var whiteColor = 80
var blackColor = 20
var POWER = -60
var turningAngle = 5
var lightSensorOutput
lightSensorOutput = lightSensorPct()
if (lightSensorOutput > whiteColor) {
syncMotors(B, C, POWER, -1 * turningAngle) //change -5 to a number that makes it turn left the right amount
}
if (lightSensorOutput < blackColor) {
syncMotors(B, C, POWER, turningAngle) //change 5 to a number that makes it turn right the right amount
}
if (lightSensorOutput < whiteColor && lightSensorOutput > blackColor) {
syncMotors(B, C, POWER, 0) //go foreward
}
}
function whatTheBridgeDoin() {
var POWER = -60
var cmAway = 5
drawText(10, 10, ultrasonicSensorValue(), 2)
while (ultrasonicSensorValue() > cmAway) {//it doesnt have to be 5(cm), it prob wont, its just a number 4 now.
followTheLine()
drawText(10, 10, ultrasonicSensorValue(), 2)
}
//push down the bridge w/ the arm
while (ultrasonicSensorValue() < cmAway) {
drawText(10, 10, ultrasonicSensorValue(), 2)
driveForward(5, POWER, 0) // the 5 (inches) is temporary, and prob not the right numbers.
// *looking at my label for inches* "I like it!" - Matt McHenry, 2:06PM, november 14. 2021
}
//drive AWAYYYYYYYYYYYYY bye bye :)
driveBackward(41, POWER) //numbers are temporary! the 60(power) is prob right tho
//*nods* - Matt McHenry, 2:10PM, november 14. 2021
}
while (true) {
setLED(0)
clearScreen()
drawText(5, 15, 'Press a key to')
drawText(5, 30, 'select a program')
drawText(5, 45, 'UPPP=get the blue box')
drawText(5, 60, 'CENTER=deliver the blue box')
drawText(5, 75, 'DOWN=Truck to Circle')
drawText(5, 90, 'LEFT=Plane')
drawText(5, 105, 'RIGHT=Bridge')
var keyPressed = waitForPress()
switch (keyPressed) {
case 1:
alert('You selected UP')
alert('getting the blue box...')
driveForward(8, 60, 2000)
sleep(500)
syncMotors(B, C, -50, -75)
sleep(2000)
driveForward(13, 60, 2000)
sleep(500)
stopAllMotors()
break
case 2:
alert('You selected ENTER')
alert('delivering the blue box...')
driveForward(16, 60, 2000)
sleep(1000)
driveBackward(30, 60)
sleep(1300)
stopAllMotors()
break
case 3:
drawText('You selected DOWN') //do we have to put coords in here???????
alert('truck in circle')
PUSHDATRUCK(1500, 1800)
break
case 4:
alert('YOU SELECTED Right')
alert('Bridge Mission')
driveForward(15, 60, 0) //go to line up with the bridge
syncMotors(B, C, -50, 75) //turn to line up
sleep(1000)
driveForward(15, 60, 0) //go to line up more
syncMotors(B, C, -50, 75) // turn to line up more
sleep(900)
//IMPORTANT NOTE!!! MUST READ!!!
//we need to change the timing of teh arm because it WAS NOT WORKING.
driveForward(13, 60, 0) //gor twards the bridge
stopAllMotors()
rotateArm(down)
driveForward(3, 60, 0) //push down bridge part one
rotateArm(up)
driveForward(7, 60, 0) //go forewarde more
stopAllMotors()
rotateArm(down)
driveBackward(3, 60) //push down bridge part two
stopAllMotors()
rotateArm(up)
driveBackward(40, 60) //return to base (PLEASE NOTE: THIS WILL NOT STOP FOR A VERY LONG TIME. JUST KILL THE PROGRAM.
//we may or may not want to reverse the turning manuvers at the begining so that we dont run into the orange plane?
stopAllMotors()
//driveForward(20,60,0)
//sleep(1500)
//driveBackward(25 ,60)
//sleep(1500)
//stopAllMotors()
//whatTheBridgeDoin() //this dowsnt look like much, but it calls a really big function up there^^^
break
//old bridge mission stuff that i dont want to look at
/* driveForward(12, 60, 0)
sleep(1000)
syncMotors(B, C, -50, 75)
sleep(2000)
driveForward(13, 60)
sleep(500)
stopAllMotors()*/
case 5:
alert('You Selected orange plane')
alert('second bold line + one square')
driveForward(4.5, 60, 0) //we had to change this from 5 to 4.5 because of the bumper but it NOT WORRRRRRRRRKKKKKKKKKKKKKKKKKKKKKKKK
beep(50, 800, 1000)
sleep(1800)
syncMotors(B, C, -50, -65)
sleep(1000)
stopAllMotors()
rotateArm(down)
syncMotors(B, C, -50, -75)
sleep(1000)
driveForward(15, 60, 0)
stopAllMotors() // this here marks the end of the orange plane mision
// right here is the start of the green helicopter mission (but it runs continuesly from teh first program, so you dont havr to press any toher buttons.)
setMotor(D, -40) //raise arm (i didnt change this to a function because we call a sleep int eh function, and the sleep for this one was added in addition to the sleep for the drive forward, so im just being safe)
setLED(1)
driveBackward(7, 60) //drive twards the helicopter
syncMotors(B, C, 50, 75) //turn to line up with the helicopter
sleep(1000)
stopAllMotors()
driveBackward(0.5, 60) //line up with the helico0pter more
stopAllMotors()
setLED(7) // this is just for debugging
rotateArm(down)//lower arm to knock down teh helicoptyer lever
driveForward(0.1, 0.1, 0) //this was put here for debugging purposes, it is usless
rotateArm(up) //raise arm to get it out of the way
//might need to turn backwardes here
driveBackward(3, 40)
rotateArm(down) //lower arm to grab box
// maybe add something here to pick up the helicopter package and bring it back to base???
//idk if we should do this with an attachment or a motor.
//i personally would want to do it with a motor, but we might not have time (OR ROOM!?!?!?!) for that
driveForward(20, 60, 0) //return to base (maybe make this more??)
stopAllMotors()
break
//driveForward(34, 60, 0)
//stopAllMotors()
//setMotor(D, 50)
//sleep(300)
//stopAllMotors()
}
}
