Program 1 (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 "controlling" robot (i.e., the robot that is in control of the others).
The "sendEventTo" function will send a "message" to another (specified) robot.
//sendEventTo('Robot2', 1, 3, 2) // Name, Event.Value, MessagePort#, Protocol
The "event" will have a 'value', 'message number', and 'protocol'.
The 'protocol' is either Bluetooth or WiFi. So far, only Bluetooth seems to be working.
In summary, robot A will send messages to robot B. When the message is received, the handler function
is executed. In this example, the sendEventTo() function will send a message to another robot.
-------------------------------------------- */
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)
settled(1)
}
/*----
function standOneLeg(hip,knee) {
clearScreen()
setLED(3)
setTarget(hip, motor[hip].extend*45) // Target value for the Hip
setTarget(knee, motor[knee].extend * motor[knee].gearRatio * 120) // Target value for the Knee
motor[hip].targetMet = isTargetMetFor(hip)
motor[knee].targetMet = isTargetMetFor(knee)
setMotor(hip,0)
setMotor(knee,0)
waitHereUntil( moveMotorToTarget(hip) && moveMotorToTarget(knee) )
stopAllMotors(true)
sleep(500)
setLED(1)
}
function standTwoLegs(knee1,hip1,hip2,knee2) { // A B C D
clearScreen()
setLED(3)
setTarget(hip1, motor[hip1].extend*70) // Target value for the Hip1
setTarget(hip2, motor[hip2].extend*70) // Target value for the Hip2
setTarget(knee1, motor[knee1].extend * motor[knee1].gearRatio * 70) // Target value for the Knee1
setTarget(knee2, motor[knee2].extend * motor[knee2].gearRatio * 70) // Target value for the Knee2
motor[hip1].targetMet = isTargetMetFor(hip1)
motor[hip2].targetMet = isTargetMetFor(hip2)
motor[knee1].targetMet = isTargetMetFor(knee1)
motor[knee2].targetMet = isTargetMetFor(knee2)
setMotor(hip1,0)
setMotor(hip2,0)
setMotor(knee1,0)
setMotor(knee2,0)
while( !motor[hip1].targetMet || !motor[hip2].targetMet || !motor[knee1].targetMet || !motor[knee2].targetMet) {
moveMotorToTarget(hip1)
moveMotorToTarget(knee1)
moveMotorToTarget(hip2)
moveMotorToTarget(knee2)
}
stopAllMotors(true)
sleep(500)
setLED(1)
}
----*/
// Return legs to starting positions
sendEventTo('Robot2', 1, 3, 2) // Name, Event.Value, MessagePort#, Protocol
resetLegs(A, B, C, D) // Knee, Hip, Hip, Knee
//standTwoLegs(A, B, C, D)
stopAllMotors(true)
sleep(16000)
