/**
* continous code for driving and turning
* call this inside of a while(1) loop.
**/
void LV_DoDrive() {
int dVal = 0, tVal = -1;
// handle driving
if (LV_DRIVE_ENABLED != 0) {
dVal = getJSAnalog(LV_DRIVE_JOYSTICK, LV_DRIVE_CHANNEL);
if (dVal != 0) {
// handle turning
if (LV_TURN_ENABLED) {
tVal = getJSAnalog(LV_TURN_JOYSTICK, LV_TURN_CHANNEL);
if (tVal > 10) {
setMotors(LV_DRIVE_MOTORS, LV_DRIVE_ENABLED == 2 ? (127 * (dVal > 0 ? 1 : -1)) : dVal);
} else if (tVal != 0) {
setSMotors(LV_DRIVE_LW, 2, LV_DRIVE_MOTORS, 32 * (dVal > 0 ? -1 : 1));
setSMotors(LV_DRIVE_RW, 2, LV_DRIVE_MOTORS, LV_DRIVE_ENABLED == 2 ? (127 * (dVal > 0 ? 1 : -1)) : dVal);
}
} else {
setMotors(LV_DRIVE_MOTORS, LV_DRIVE_ENABLED == 2 ? (127 * (dVal > 0 ? 1 : -1)) : dVal);
}
} else {
setMotors(LV_DRIVE_MOTORS, 0);
}
}
}
/** * Drive until an encoder value is reached * * @author Sean Kelley [email protected] * * @param encoder_count encoder ticks to drive forward * @param timeout_in_seconds timeout for motors if encoder value is surpassed * @param speed speed of motors * */ int driveByEncoder( int encoder_count, int timeout_in_seconds = 5 , int speed=MOTOR_SPEED) { int timeout; // Drive motor until encoder has moved a number counts or // timeout_in_seconds seconds have passed // Zero the encoder SensorValue[ encoder ] = 0; // Run the motor forwards or backwards if( encoder_count > 0 ) { setMotors(speed); } else { setMotors(-speed); } // run loop until encoder hits encoder_count counts or timeout reached for( timeout=(timeout_in_seconds*TIMEOUT_CNT_PER_SEC); timeout > 0; timeout-- ) { // check encoder if( encoder_count > 0 ) { // going forwards if( SensorValue[ encoder ] >= encoder_count ) { break; } else { // going backwards if( SensorValue[ encoder ] <= encoder_count ) { break; } } } // wait 1/10 second wait1Msec( 100 ); } // Stop the motor clearMotors(); // See if we sucessfully found the right encoder value if( timeout <= 0 ) { // there was an error - perhaps do something // return error return (-1); } else { // return success return 0; } }
// Set Robot Angle
void setAngle(int degreesToTurn, int motorSpeed)
{
float degreesSoFar = 0;
int initialTurnReading = SensorValue[gyroSensor];
setMotors(-motorSpeed * sgn(degreesToTurn), -motorSpeed * sgn(degreesToTurn), -motorSpeed * sgn(degreesToTurn), -motorSpeed * sgn(degreesToTurn));
while(abs(degreesSoFar) < abs(degreesToTurn))
{
wait1Msec(10);
int currentGyroReading = SensorValue(gyroSensor) - initialTurnReading;
degreesSoFar = degreesSoFar + currentGyroReading * 0.01;
}
setMotors(0, 0, 0, 0);
}
/** * Moves bot backward for a given amount of seconds * * @author Bernard Suwirjo [email protected] * * @param seconds amount of seconds to move backward * @param speed speed of motors * */ void backwardSeconds(float seconds, int speed=MOTOR_SPEED) { //Set all motors to negative target value setMotors(-speed); wait1Msec(seconds * 1000);//Wait given amount of time clearMotors(); }
int Hardware::safeClose() {
setMotors(0,0);
//buzz->stopSound();
//setLed(0,0,0);
usleep(100000);
}
/**
* set relative power of all motors in a group
* @param group the initialized motor group
* @param rpower the relative power to write (from 0 to 100, or from 0 to 1)
* @param inverse whether or not to invert the power (1=Yes,0=No)
* @return integer the absolute value of the relative power
**/
int setRMotors(LV_MOTOR_GROUP *group, int rpower, int inverse) {
// from relative to absolute
int power = 127 * (rpower > 1 ? (rpower / 100) : rpower) * (inverse == 1 ? -1 : 1);
// write absolute value
setMotors(group, power);
// return the used power
return power;
}
void freeRoam() {
while (1) {
writeLed(0xFF);
setMotors(SPEED, SPEED);
_delay_ms(100);
while (1) {
uint16_t dist = readSonarBlocking();
writeLed(dist);
if (dist < STOP_DIST) {
break;
}
// drive
}
setMotors(0, 0);
_delay_ms(200);
uint16_t left, right;
setServo(SERVO0, 0);
_delay_ms(300);
right = readSonarBlocking();
setServo(SERVO0, 180);
_delay_ms(400);
left = readSonarBlocking();
setServo(SERVO0, 90);
_delay_ms(200);
leftEncoder = 0;
rightEncoder = 0;
if(left > right) {
setMotors(-SPEED, SPEED);
} else {
setMotors(SPEED, -SPEED);
}
while (leftEncoder + rightEncoder < ROTATE_BY) {
// rotate
}
setMotors(0, 0);
//_delay_ms(1000);
}
}
task main()
{
wait1Msec(100);//Wait 100 Msec for sensors to init
wallDistance = SensorValue(sonarSensor);//Set distance away from wall to follow
while(true){//Main loop
currentDistance = SensorValue(sonarSensor);//Set current ultrasonic sensor value for better use
if(currentDistance <= wallDistance){//To far away from wall, turn LEFT
setMotors(secondarySpeed, primarySpeed);
} else if(currentDistance >= wallDistance){//To close to wall, turn RIGHT
setMotors(primarySpeed, secondarySpeed);
} else{//Good distance from wall, go straight
setMotors(primarySpeed, primarySpeed);
}
}
}
task main()
{
initializeRobot();
//waitForStart();
while (true)
{
if(abs(joystick.joy1_x2) > 3 || abs(joystick.joy1_y2) > 3) //Account for dead zones on the controller
{
setMotors();
}
else //If there is a dead zone then just stop the motors
{
motor[leftMotor] = 0;
motor[rightMotor] = 0;
}
}
}
void robotMotorClass::processRequestMsg(Stream *stream)
{
int arg0 =-1, arg1 = -1;
int request = stream->read();
// parse the correct number of arguments for each method
if( request == tag_SET_MOTOR || request == tag_SET_MOTORS || request == tag_STOP_MOTOR) {
arg0 = stream->parseInt();
if( request != tag_STOP_MOTOR) {
arg1 = stream->parseInt();
}
}
switch(request) {
case tag_SET_MOTOR: setMotor(arg0,arg1); break;
case tag_SET_MOTORS: setMotors(arg0,arg1);break; // TODO this assumes only two motors
case tag_STOP_MOTOR: stopMotor(arg0); break;
case tag_STOP_MOTORS: stopMotors(); break;
default: reportError(ServiceId, request, ERR_UNKNOWN_REQUEST, stream);
}
}
task main()
{
initializeRobot();
while(nNxtButtonPressed != nxtOrange)
{
displayStatus();
}
waitTillNoButton();
eraseDisplay();
resetEncoders();
while(nNxtButtonPressed != nxtOrange)
{
while(nxtGrey != nNxtButtonPressed)
{
displayString(4,"PICK UP AND PRESS GREY BTN");
}
while((nMotorEncoder[rightFront] < 1800)||(nMotorEncoder[rightBack] < 1800)||(nMotorEncoder[leftBack] < 1800)||(nMotorEncoder[leftFront]< 1800))
{
eraseDisplay();
setMotors(70,70,70,70);
}
stopMotors();
displayString(1, "should be 1800");
displayString(3, "rightback encoder: %d", nMotorEncoder[rightFront]);
displayString(2, "rightfront encoder: %d", nMotorEncoder[rightBack]);
displayString(4, "leftfront encoder: %d", nMotorEncoder[leftFront]);
displayString(5, "leftback encoder: %d", nMotorEncoder[leftBack]);
}
waitTillNoButton();
}
/**
* forward a joystick axis to a motor group and restrict by encoding
* @param joystick the joystick number
* @param channel the axis channel
* @param group the initialized motor group
* @param IME the pin of the motor with an IME
* @param min the minimum encoding
* @param max the maximum encoding
**/
void JSToMotorGroupIME(int joystick, int channel, LV_MOTOR_GROUP *group, int IME, long min, long max) {
long enc = GetIntegratedMotorEncoder(IME);
setMotors(group, enc > min && enc < max ? getJSAnalog(joystick, channel) : 0);
}
void Gdc2005Demo::optionUpdate()
{
const hkVector4 UP(0,1,0);
// Handle load save
//Set capsule size.
{
const hkReal totalHeight = m_options.m_Proxy.m_height;
const hkReal radius = m_options.m_Proxy.m_radius;
const hkReal capsulePoint = totalHeight*0.5f - radius;
updateProxyDisplay(m_options.m_Proxy.m_radius, m_options.m_Proxy.m_height);
hkVector4 vertexA(0, capsulePoint * 2 + radius, 0);
hkVector4 vertexB(0, radius, 0);
hkpCapsuleShape* capsule = static_cast<hkpCapsuleShape*>( const_cast<hkpShape*>(m_characterProxy->getShapePhantom()->getCollidable()->getShape() ));
capsule->setRadius( radius );
capsule->setVertex(0, vertexA);
capsule->setVertex(1, vertexB);
}
// Set gravity
hkVector4 gravity;
gravity.setMul4( -m_options.m_Physics.m_gravity, UP);
m_world->setGravity( gravity );
// Ragdoll bodies (green)
if (m_options.m_Display.m_ragdoll != m_ragdollDisplayBodiesVisible )
{
toggleRagdollVisibility();
}
if (m_options.m_Display.m_proxy != m_proxyVisible)
{
toggleProxyVisibility();
}
// Graphics
hkDefaultDemo::forceShadowState( m_options.m_Display.m_shadows );
if (m_lightmapDisplay)
{
if (m_options.m_Display.m_lightmaps)
{
int vertsIn = m_env->m_displayWorld->findDisplayObject( m_vertexColorDisplay );
if (vertsIn >= 0 )
{
m_env->m_displayWorld->removeDisplayObject( vertsIn );
m_vertexColorDisplay->release(); // already have a ref anyway
m_env->m_displayWorld->addDisplayObject( m_lightmapDisplay );
}
}
else
{
int lmIn = m_env->m_displayWorld->findDisplayObject( m_lightmapDisplay );
if (lmIn >= 0 )
{
m_env->m_displayWorld->removeDisplayObject( lmIn );
m_lightmapDisplay->release(); // already have a ref anyway
m_env->m_displayWorld->addDisplayObject( m_vertexColorDisplay );
}
}
}
hkReal w = hkMath::max2(
m_animatedSkeleton->getAnimationControl(GDC_DEATH_CONTROL)->getWeight(),
m_animatedSkeleton->getAnimationControl(GDC_DYING_CONTROL)->getWeight());
setMotors(m_ragdollInstance,
m_options.m_Matching.m_force * w,
m_options.m_Matching.m_tau,
m_options.m_Matching.m_proportinalRecovery,
m_options.m_Matching.m_constantRecovery);
// Do rebuild walls, save and load from XML, etc., when we have finished tweaking
if (m_tweaking == false)
{
switch (m_options.m_Misc.m_settings)
{
case hkGdcMiscOptions::SAVE:
{
hkBinaryPackfileWriter pw;
pw.setContents(&m_options, Gdc2005DemoOptionsClass);
hkOfstream settingsFile("GdcDemoSettings.bin");
if (settingsFile.isOk())
{
hkPackfileWriter::Options o;
pw.save( settingsFile.getStreamWriter(), o );
}
}
break;
case hkGdcMiscOptions::LOAD:
{
hkBinaryPackfileReader pw;
hkIfstream settingsFile("GdcDemoSettings.bin");
if (settingsFile.isOk())
{
pw.loadEntireFile( settingsFile.getStreamReader() );
Gdc2005DemoOptions* options = (Gdc2005DemoOptions*)pw.getContents(Gdc2005DemoOptionsClass.getName());
m_options = *options;
}
}
break;
case hkGdcMiscOptions::DEFAULT:
{
// Copy from defaults
m_options = Gdc2005DemoOptions();
m_selected = "/";
}
break;
case hkGdcMiscOptions::NONE:
break;
}
m_options.m_Misc.m_settings = hkGdcMiscOptions::NONE;
if (m_options.m_Physics.m_rebuildWall)
{
rebuildBrickWall();
m_options.m_Physics.m_rebuildWall = false; // reset
}
}
}
// Moves the swarmbot according to the state
void move(){
switch (state){
case GO_UNTIL_COLLIDE:
setMotors(FORWARD,FAST);
break;
case HUNTING_FOR_COLOR:
setMotors(FORWARD,FAST);
break;
case FOUND_RED:
setMotors(FORWARD,SLOW);
break;
case FOUND_BLUE:
setMotors(FORWARD, SLOW);
break;
case LLOST_BLUE:
setMotors(ROTATEL, SLOW);
break;
case RLOST_BLUE:
setMotors(ROTATER,SLOW);
break;
case LLOST_RED:
setMotors(ROTATEL, SLOW);
break;
case RLOST_RED:
setMotors(ROTATER,SLOW);
break;
case TURN_AROUND_BLUE:
setMotors(ROTATER, FAST);
break;
case TURN_AROUND_RED:
setMotors(ROTATEL, FAST);
break;
// covers halt
default:
setMotors(STOPPED, 0);
break;
}
}
// Function To Simplify Auton Movement
void loadDirections(int *directionCommands, int arraySize)
{
// Current Command Index
int currentIndex = 0;
// Loop Through Commands And Act Accordingly
while(currentIndex < arraySize)
{
if(directionCommands[currentIndex] == 0)
{
// Stop
setMotors(0, 0, 0, 0);
wait1Msec(directionCommands[currentIndex + 1]);
currentIndex += 2 ;
}
else if(directionCommands[currentIndex] == 1)
{
// Forward
setMotors(-directionCommands[currentIndex + 2], directionCommands[currentIndex + 2], -directionCommands[currentIndex + 2], directionCommands[currentIndex + 2]);
wait1Msec(directionCommands[currentIndex + 1]);
currentIndex += 3;
}
else if(directionCommands[currentIndex] == 2)
{
// Backward
setMotors(directionCommands[currentIndex + 2], -directionCommands[currentIndex + 2], directionCommands[currentIndex + 2], -directionCommands[currentIndex + 2]);
wait1Msec(directionCommands[currentIndex + 1]);
currentIndex += 3;
}
else if(directionCommands[currentIndex] == 3)
{
// Strafe Left
setMotors(-directionCommands[currentIndex + 2], -directionCommands[currentIndex + 2], directionCommands[currentIndex + 2], directionCommands[currentIndex + 2]);
wait1Msec(directionCommands[currentIndex + 1]);
currentIndex += 3;
}
else if(directionCommands[currentIndex] == 4)
{
// Strafe Right
setMotors(directionCommands[currentIndex + 2], directionCommands[currentIndex + 2], -directionCommands[currentIndex + 2], -directionCommands[currentIndex + 2]);
wait1Msec(directionCommands[currentIndex + 1]);
currentIndex += 3;
}
else if(directionCommands[currentIndex] == 5)
{
// Rotate Left
setMotors(directionCommands[currentIndex + 2], directionCommands[currentIndex + 2], directionCommands[currentIndex + 2], directionCommands[currentIndex + 2]);
wait1Msec(directionCommands[currentIndex + 1]);
currentIndex += 3;
}
else if(directionCommands[currentIndex] == 6)
{
// Rotate Right
setMotors(-directionCommands[currentIndex + 2], -directionCommands[currentIndex + 2], -directionCommands[currentIndex + 2], -directionCommands[currentIndex + 2]);
wait1Msec(directionCommands[currentIndex + 1]);
currentIndex += 3;
}
else if(directionCommands[currentIndex] == 7)
{
// Spinner On
setSpinner(directionCommands[currentIndex + 2]);
wait1Msec(directionCommands[currentIndex + 1]);
currentIndex += 3;
}
else if(directionCommands[currentIndex] == 8)
{
// Spinnner Off
setSpinner(0);
wait1Msec(directionCommands[currentIndex + 1]);
currentIndex += 2;
}
else if(directionCommands[currentIndex] == 9)
{
// Set Angle
setAngle(directionCommands[currentIndex + 3], directionCommands[currentIndex + 2]);
wait1Msec(directionCommands[currentIndex + 1]);
currentIndex += 4;
}
else if(directionCommands[currentIndex] == 10)
{
// FR Diagonal
setMotors(0, directionCommands[currentIndex + 2], -directionCommands[currentIndex + 2], 0);
wait1Msec(directionCommands[currentIndex + 1]);
currentIndex += 3;
}
else if(directionCommands[currentIndex] == 11)
{
// BL Diagonal
setMotors(0, -directionCommands[currentIndex + 2], directionCommands[currentIndex + 2], 0);
wait1Msec(directionCommands[currentIndex + 1]);
currentIndex += 3;
}
else if(directionCommands[currentIndex] == 12)
{
// FL Diagonal
setMotors(-directionCommands[currentIndex + 2], 0, 0,directionCommands[currentIndex + 2]);
wait1Msec(directionCommands[currentIndex + 1]);
currentIndex += 3;
}
else if(directionCommands[currentIndex] == 13)
{
// BR Diagonal
setMotors(directionCommands[currentIndex + 2], 0, 0, -directionCommands[currentIndex + 2]);
wait1Msec(directionCommands[currentIndex + 1]);
currentIndex += 3;
}
}
}
void Drive::setLeft(double value) {
setMotors((reversed_ ? leftMotors_ : rightMotors_), value);
}
void leftTurn(const int power)
{
setMotors(-power, power);
}
// Function: stopMotors
// this function stops all the motors
// Parameters: none
//Outputs: none
void stopMotors()
{
setMotors(0,0);
}
void setMotion(const int y1, const int y2)
{
setMotors(rescale(y1), rescale(y2));
}
// reposition according to pad that was hit, and the last time you were hit
void reposition(){
// Serial.print("reposition, state: ");Serial.println(state);
// Serial.print("numInts:"); Serial.println(numInts);
// Serial.print("debounceTimer: ");Serial.println(debounceTimer);
switch(state){
case PAD0:
setMotors(REVERSE,MEDIUM);
if(timeOut()){
collisionTimeout = 500;
collisionTimer = millis();
state = PAD0_STATE2;
}
break;
case PAD0_STATE2:
if(previousState == GO_UNTIL_COLLIDE){
setMotors(ROTATER, FAST);
}else if(previousState == FOUND_RED){
setMotors(ROTATEL, FAST);
}else if(previousState == FOUND_BLUE){
setMotors(ROTATER, FAST);
}else{
setMotors(FSWINGR,MEDIUM);
}
if(timeOut()){
if(previousState == GO_UNTIL_COLLIDE){
state = HUNTING_FOR_COLOR;
collide = false;
}else if(previousState == FOUND_RED){
state = TURN_AROUND_RED;
counter = 0;
collide = false;
}else if(previousState == FOUND_BLUE){
state = TURN_AROUND_BLUE;
counter = 0;
collide = false;
}else{
endCollide();
}
setMotors(STOPPED,0);
}
break;
case PAD1:
setMotors(REVERSE,MEDIUM);
if(timeOut()){
collisionTimeout = 500;
collisionTimer = millis();
state = PAD1_STATE2;
}
break;
case PAD1_STATE2:
setMotors(ROTATER,MEDIUM);
if(timeOut()){
endCollide();
}
break;
case PAD2:
setMotors(REVERSE, MEDIUM);
if(timeOut()){
collisionTimeout = 500;
collisionTimer = millis();
state = PAD2_STATE2;
}
break;
case PAD2_STATE2:
setMotors(ROTATEL,MEDIUM);
if(timeOut()){
endCollide();
}
break;
case PAD3:
setMotors(STOPPED, 0);
if(timeOut()){
collisionTimeout = 500;
collisionTimer = millis();
state = PAD3_STATE2;
}
break;
case PAD3_STATE2:
endCollide();
break;
case PAD4:
setMotors(STOPPED, 0);
if(timeOut()){
collisionTimeout = 500;
collisionTimer = millis();
state = PAD4_STATE2;
}
break;
case PAD4_STATE2:
endCollide();
break;
case PAD5:
setMotors(STOPPED,0);
if(timeOut()){
collisionTimeout = 500;
collisionTimer = millis();
state = PAD5_STATE2;
}
break;
case PAD5_STATE2:
if(timeOut()){
endCollide();
}
break;
default:
endCollide();
state = HALT;
break;
}
}
void forward(const int power)
{
setMotors(power, power);
}
void rightTurn(const int power)
{
setMotors(power, -power);
}
/**
* forward a joystick axis to a motor group
* @param joystick the joystick number
* @param channel the axis channel
* @param group the initialized motor group
**/
void JSToMotorGroup(int joystick, int channel, LV_MOTOR_GROUP *group) {
setMotors(group, getJSAnalog(joystick, channel));
}
void Drive::setRight(double value) {
setMotors((reversed_ ? rightMotors_ : leftMotors_), value);
}
void backward(const int power)
{
setMotors(-power, -power);
}
// Go back to state before first collision, exit collision states
void endCollide(){
collide = false;
setMotors(STOPPED,0);
state = previousState;
}
