DanceDemo.cpp

From IPRE Wiki
Jump to: navigation, search
#include <Myro.h>
#include <iostream>
#include <cstdlib>

using namespace std;

// Function Prototypes
bool coinFlip();
void grapevineRight();
void fancyTurn(int howLong, bool turnBothWays);
void backSteps(int howMany);

void simpleDance(){
    while(timeRemaining(15)){
        grapevineRight();

        int turnDuration = random()%3+1;
        bool turnBothWays = coinFlip();
        fancyTurn(turnDuration, turnBothWays);
        backSteps(2);
    }
    cout << "Phew, I'm bushed" << endl;
}

/**
 * Do a zig zag pattern to the right
 */
void grapevineRight(){
    //twist left and then go back
    robot.turnLeft(1, 0.1); // Full speed, one tenth of a second
    robot.backward(0.7, .5); // 0.7 speed, half a second

    //now twist right and go forward
    robot.turnRight(1, 0.2);
    robot.forward (.7, .5);

    //twist left and then go back
    robot.turnLeft(1, 0.2);
    robot.backward(0.7, .5);

    //straighten out & pause
    robot.turnRight(1, 0.1);
    wait(0.5);
}

/**
 * Take half second rolling "steps" back at half speed with
 * pauses inbetween. Then pause a little bit more for effect
 *
 * @param howMany How many times to repeat
 */
void backSteps(int howMany){
    for (int i = 0; i < howMany; i++){
        robot.backward(1, .5); // Full speed, half a second
        wait(.1); // wait specified in seconds
    }
}

/**
 * Spin to the right for the number of seconds indicated by 
 * the first argument. If the second argument is true then also 
 * spin to the left
 *
 * @param howLong How long ot spin to the right
 * @param turnBothWays Flag to also turn left after turning right
 */
void fancyTurn(int howLong, bool turnBothWays){

    // We could, of course just use the command
    //     robot.turnRight(1,howLong)
    // but figured we'd show how to specify motor
    // speeds directly
    
    robot.motors(1,-1); // Left motor full speed forward, right full back
    wait(howLong);
    robot.stop();

    if (turnBothWays){
        robot.turnLeft(1,howLong);
    }
}

bool coinFlip(){
    int rand = random();
    if ( rand % 2 )
        return true;
    else
        return false;
}

int main(){
    connect();
    simpleDance();
    disconnect();
}