Program 2 (Spring 2022)
The code below is a set of functions, variables, constants, and algorithms that are the work of RoboCatz for the Spring 2022 season.
/* ---------------------------------------------
This program was part of an experiment in robot-to-robot communication through a Bluetooth connection.
This code is placed on the "receiving" robot. At the bottom of this code, an EventListener is being added
to listen for "message" events. A "message" event will be sent by the other robot code.
In summary, robot A will send messages to robot B. When the message is received, the handler function
is executed. In this example, the handler function (called: forSpecialEvents) is also at the bottom of this file.
-------------------------------------------- */
var motor = [0,
{ isStopped: true, targetValue: 0, targetMet: false, powerLevel: 50, flex: 1, extend: -1, gearRatio: 24, shouldExtend: false }, // Motor A : Knee
{ isStopped: true, targetValue: 0, targetMet: false, powerLevel: 50, flex: 1, extend: -1, gearRatio: 24, shouldExtend: false }, // Motor B : Hip
{ isStopped: true, targetValue: 0, targetMet: false, powerLevel: 50, flex: 1, extend: -1, gearRatio: 24, shouldExtend: false }, // Motor C : Hip
{ isStopped: true, targetValue: 0, targetMet: false, powerLevel: 50, flex: 1, extend: -1, gearRatio: 24, shouldExtend: false } // Motor D : Knee
]
const flex = 1
const extend = -1
function toCheckSpeed(e) { // Handles the motor speed event
if ( e.value < 10 ) motor[e.port].isStopped = true
}
addEventListener('motorSpeed', toCheckSpeed, A)
addEventListener('motorSpeed', toCheckSpeed, B)
addEventListener('motorSpeed', toCheckSpeed, C)
addEventListener('motorSpeed', toCheckSpeed, D)
function setTarget(joint, targetValue) {
motor[joint].shouldExtend = ( encoderValue(joint) > targetValue )
motor[joint].targetValue = targetValue
}
function isTargetMetFor(joint) {
if ( motor[joint].shouldExtend ) return (encoderValue(joint) <= motor[joint].targetValue)
if ( !motor[joint].shouldExtend ) return (encoderValue(joint) >= motor[joint].targetValue)
return ( abs(encoderValue(joint) - motor[joint].targetValue) < 10 )
}
function isMotorStalled(joint) {
drawText( 0, joint*20, motor[joint].isStopped, 2 )
drawText( 10, joint*20, motor[joint].targetValue + ' ' + encoderValue(joint) + ' ' + joint, 2 )
if ( getMotorSpeed(joint) <= 5 ) {
motor[joint].isStopped = true
stopMotor(joint, true)
}
}
function moveMotorToTarget(joint) {
drawText( 10, joint*20, motor[joint].targetValue + ' ' + encoderValue(joint), 2 )
if ( isTargetMetFor(joint) ) {
stopMotor(joint, true)
motor[joint].targetMet = true
return true
} else if ( encoderValue(joint) < motor[joint].flex*motor[joint].targetValue ) {
setMotor( joint, motor[joint].flex*motor[joint].powerLevel)
} else if ( encoderValue(joint) > motor[joint].flex*motor[joint].targetValue ) {
setMotor( joint, motor[joint].extend*motor[joint].powerLevel)
}
return false
}
function resetLegs(knee1,hip1,hip2,knee2) { // A B C D
clearScreen()
setLED(2)
setMotorSpeed( hip1, motor[hip1].flex*80)
setMotorSpeed( hip2, motor[hip2].flex*80)
setMotorSpeed( knee1, motor[knee1].flex*40)
setMotorSpeed( knee2, motor[knee2].flex*40)
sleep(200)
motor[hip1].isStopped = (getMotorSpeed(hip1)==0) ? true : false
motor[hip2].isStopped = (getMotorSpeed(hip2)==0) ? true : false
motor[knee1].isStopped=(getMotorSpeed(knee1)==0) ? true : false
motor[knee2].isStopped=(getMotorSpeed(knee2)==0) ? true : false
while(!motor[hip1].isStopped || !motor[hip2].isStopped || !motor[knee1].isStopped || !motor[knee2].isStopped) {
setLED(3)
isMotorStalled(hip2)
isMotorStalled(hip1)
isMotorStalled(knee1)
isMotorStalled(knee2)
}
stopAllMotors(true)
setLED(0)
beep()
resetEncoder(hip1)
resetEncoder(hip2)
resetEncoder(knee1)
resetEncoder(knee2)
setTarget(hip1, motor[hip1].extend * motor[knee1].gearRatio * 50) // Target value for the Hip1
setTarget(hip2, motor[hip2].extend * motor[knee1].gearRatio * 50) // Target value for the Hip2
setMotorSpeed( hip1, motor[hip1].extend*80)
setMotorSpeed( hip2, motor[hip2].extend*80)
motor[hip1].targetMet = false
motor[hip2].targetMet = false
while( !motor[hip1].targetMet || !motor[hip2].targetMet) {
moveMotorToTarget(hip1)
moveMotorToTarget(hip2)
}
stopAllMotors(true)
sleep(500)
setLED(1)
}
function forSpecialEvents(event) {
alert(event.value)
if ( event.value == 1 ) {
resetLegs(A, B, C, D) // Knee, Hip, Hip, Knee
stopAllMotors(true)
}
}
addEventListener('message',forSpecialEvents, 3)
setLED(5)
waitHereWhile(true)
