Ejemplo n.º 1
0
void setup() 
{
  //to prevent fuse from blowing
  motor.speed(leftWheelPin, 0)  ;
  motor.speed(rightWheelPin, 0)  ;
  
  //attach driving servo
  RCServo1.attach(RCServo1Output) ;
  RCServo1.write(neutral)  ; 
  
  //set ports to inputs
  portMode(0, INPUT) ;      	 
  portMode(1, INPUT) ;
  
  Serial.begin(9600);
  
    while(1){
    delay(50);
    while( !(stopbutton()) ){
      delay(50);
        LCD.setCursor(0,0);  LCD.clear();  LCD.home() ;
        LCD.print("kP");
        LCD.print(" ");
        LCD.print(kP);
        kP = knob(knob_one)/10; 
        LCD.print(" ");
        LCD.setCursor(0,1);
        LCD.print("kD");
        LCD.print(" ");
        LCD.print(kD);
        kD = knob(knob_two)/10;
        if (startbutton())
        {
          goto loopstart;
        }
    }
    delay(50);
    while( !(stopbutton())) { 
        delay(50);
        LCD.setCursor(0,0);  LCD.clear();  LCD.home() ;
        LCD.print("motorSpeed");
        LCD.print(" ");
        LCD.print(motorSpeed);
        motorSpeed = knob(knob_two);
        LCD.setCursor(0,1);
        LCD.print("Adjust");
        LCD.print(" ");
        LCD.print(knob(knob_one));
        motorAdjustment = knob(knob_one)/512;
        if (startbutton())
        {
          goto loopstart;
        }
    }
    
  }
        loopstart: ;
}
Ejemplo n.º 2
0
// Determines if the stop button is being pressed
// Optional: Debounces the button for the specified number of milliseconds
bool StopButton(int debounceTime = 80)
{
	if(!stopbutton()) return false;
	delay(debounceTime);
	return stopbutton();
}
Ejemplo n.º 3
0
int main()
{
    sf::RenderWindow window({ 640, 480 }, "Motion Playback Example"); // create the SFML window
    window.setVerticalSyncEnabled(true);

    // Create some buttons to control the playback
    sf::VertexArray playbutton(sf::PrimitiveType::Triangles);
    playbutton.append({ { 255, 350 }, sf::Color::Green });
    playbutton.append({ { 275, 360 }, sf::Color::Green });
    playbutton.append({ { 255, 370 }, sf::Color::Green });
    sf::VertexArray pausebutton(sf::PrimitiveType::Quads);
    pausebutton.append({ { 305, 350 }, sf::Color::Blue });
    pausebutton.append({ { 310, 350 }, sf::Color::Blue });
    pausebutton.append({ { 310, 370 }, sf::Color::Blue });
    pausebutton.append({ { 305, 370 }, sf::Color::Blue });
    pausebutton.append({ { 320, 350 }, sf::Color::Blue });
    pausebutton.append({ { 325, 350 }, sf::Color::Blue });
    pausebutton.append({ { 325, 370 }, sf::Color::Blue });
    pausebutton.append({ { 320, 370 }, sf::Color::Blue });
    sf::VertexArray stopbutton(sf::PrimitiveType::Quads);
    stopbutton.append({ { 355, 350 }, sf::Color::Red });
    stopbutton.append({ { 375, 350 }, sf::Color::Red });
    stopbutton.append({ { 375, 370 }, sf::Color::Red });
    stopbutton.append({ { 355, 370 }, sf::Color::Red });

    mt::DataSource datasource; // create the data source from which playback will happen
    if (!datasource.LoadFromFile("{video file path}")) // load a file into the data source
        return EXIT_FAILURE;

    mt::SFMLAudioPlayback audioplayback(datasource); // create an audio playback from our data source
    mt::SFMLVideoPlayback videoplayback(datasource); // create a video playback from our data source

    // scale video to fit the window
    videoplayback.setScale(640.f / static_cast<float>(datasource.GetVideoSize().x), 480.f / static_cast<float>(datasource.GetVideoSize().y));

    // standard SFML game and event loops
    while (window.isOpen())
    {
        sf::Event event;
        while (window.pollEvent(event))
        {
            if (event.type == sf::Event::Closed)
                window.close();
            else if (event.type == sf::Event::MouseButtonPressed)
            {
                if (playbutton.getBounds().contains(window.mapPixelToCoords({ event.mouseButton.x, event.mouseButton.y })))
                    datasource.Play(); // play button was pressed - so start the playback
                else if (pausebutton.getBounds().contains(window.mapPixelToCoords({ event.mouseButton.x, event.mouseButton.y })))
                    datasource.Pause(); // pause button was pressed - so pause the playback
                else if (stopbutton.getBounds().contains(window.mapPixelToCoords({ event.mouseButton.x, event.mouseButton.y })))
                    datasource.Stop(); // stop button was pressed - stop the playback
            }
        }

        datasource.Update(); // update the data source - this is required for any playbacks

        window.clear();
        window.draw(videoplayback); // draw the video playback
        // draw the buttons
        window.draw(playbutton);
        window.draw(pausebutton);
        window.draw(stopbutton);
        window.display();
    }
}
Ejemplo n.º 4
0
void loop()
{
  RCServo1.write(angle_neutral);
  collector_switchState = LOW;


    if (timeTicker == 50) {
        timeTicker = 0; 

      if (startbutton() && !startButtonState) {
                if (menuState == 3) {
                    menuState = 1;
                } else {
                    menuState++;
                }
                editable = false;
                startButtonState = true;
                delay(500);
            } else if (stopbutton() && !stopButtonState) {
                editable = true;
                stopButtonState = true;
            } else {
                stopButtonState = false;
                startButtonState = false;
            }
            LCD.clear();    
    

            // Menu System
                // 1:   

            switch (menuState) {    

                case 1:
                    LCD.print("Rest: ");
                    userSet = (int)(knob(6) * 180.0/1024.0);
                    if (userSet != angle_neutral && editable){
                        angle_neutral = userSet;
                    }
                    LCD.print(angle_neutral, DEC);  

                    LCD.setCursor(0,1); 

                    LCD.print("Collect?: Y/N  ");
                    userSet = (int)(knob(7) * 2.0/1024.0);
                    if(userSet != collect_status && editable){
                        collect_status = userSet;
                    }
                    LCD.print(collect_status, DEC); 

                    break;
              
                case 2:
                    LCD.print("Min: ");
                    userSet = (int)(knob(6) * 180.0/1024.0);
                    if(userSet != angle_min && editable){
                        angle_min = userSet;
                    }
                    LCD.print(angle_min, DEC);  

                    LCD.setCursor(0,1); 

                    LCD.print("Max: ");
                    userSet = (int)(knob(7) * 180.0/1024.0);
                    if(userSet != angle_max && editable){
                        angle_max = userSet;
                    }
                    LCD.print(angle_max, DEC);  

                    break;

                case 3:
                    LCD.print("TestRange: ");
                    userSet = 1.5*(int)(knob(6) * 180.0/1024.0);
                    if(userSet != angle_neutral && editable){
                        if (userSet >= 180){
                            angle_neutral = 180;
                        } else{
                            angle_neutral = userSet;
                        }
                    }
                    LCD.print(angle_neutral, DEC);

                    break;
                }   

    }


    if(collect_status == 1){
        collector_switchState = digitalRead(COLLECTOR_SWITCHPIN);   

        if(collector_switchState){
            RCServo1.write(angle_min);
            delay(200);
            RCServo0.write(angle_max);
            delay(200);
            pusher_activate();
            RCServo1.write(angle_neutral);
            collector_switchState = false;
        }   

    }

    timeTicker++;

}
Ejemplo n.º 5
0
void Shoot(uint16 diff)
{
    switch (shooterState)
    {
    case ssAim: // controls if robot is to shoot or to move back
        StartShooterControl();
        if (beginBackIR == -1)
            beginBackIR = GetRearAveragedSum();
        // try to load more balls
        if (!IsLoaded())
        {
            StopMoving();
            GuardUp();
            if (UpdateTimer(diff, loadTimer, cLoadTimer)) // ran out of time to load new ball - have no balls
                shooterState = ssAlignBack;
            sDebugPhase = "Loading";
        }
        else
        {
            loadTimer = cLoadTimer;
            if (GetTargAveragedSum() < cTargOPMinThresh) //need to coarse adj
            {
                if (doMidAdjust)
                {
                    StopMoving();
                    doMidAdjust = false;
                    midAjustComplete = true;
                    /*
                    if (PID::OPDriveAdjustCompleted(sCalibratedBackIRVal * cIRFalloffCenter))
                    {
                        StopMoving();
                        doMidAdjust = false;
                        midAjustComplete = true;
                    }

                    PID::OPDriveUpdate(diff, sCalibratedBackIRVal * cIRFalloffCenter);
                    */
                }
                else
                {
                    if (aimState == asCW)
                    {
                        //RotateCW(cAimSpeed);
                        SetMotorSpeed(tLeftDriveMotor, 100 + cAimSpeed);
                        SetMotorSpeed(tRightDriveMotor, 100 - cAimSpeed);
                    }
                    else if (aimState == asCCW)
                    {
                        //RotateCCW(cAimSpeed);
                        SetMotorSpeed(tLeftDriveMotor, 100 - cAimSpeed);
                        SetMotorSpeed(tRightDriveMotor, 100 + cAimSpeed);
                    }
                }
                
                if (coarseMaxThreshPassed) //coarse adj control
                {
                    if (GetRearAveragedSum() < beginBackIR * (aimState == asCW ? cTurnIRFalloffCW : cTurnIRFalloffCCW))
                    {
                        //switch
                        aimState = (aimState == asCW ? asCCW : asCW);
                        coarseMaxThreshPassed = false;
                        midAjustComplete = false;
                        doMidAdjust = false;
                    }
                    sDebugPhase = "Coarse 2";
                }
                else 
                {
                    if (GetRearAveragedSum() >= beginBackIR * 0.75f)
                    {
                        if (!midAjustComplete)
                            doMidAdjust = true;
                        
                        coarseMaxThreshPassed = true;
                    }
                    sDebugPhase = "Coarse 1";
                }
            }
            else // fine
            {
                midAjustComplete = false;
                doMidAdjust = false;
                if (GetRearAveragedSum() >= beginBackIR * 0.75f)
                    coarseMaxThreshPassed = true;
                if (PID::TargOPAdjustCompleted())
                {
                    StopMoving();
                    shooterState = ssFire;
                    //sAimTmr = cMaxAimDurationTimer;
                }

                PID::OPUpdate(true, diff, PID::OPPGainFront, 0, 0);

                if (IsLoaded())
                    if (UpdateTimer(diff, sAimTmr, cMaxAimDurationTimer))
                    {
                        StopMoving();
                        shooterState = ssFire;
                    }

                sDebugPhase = "Fine Adj";
            }
        }
        break;
    
    case ssFire:
        StopMoving();
        
        if (shooterReady)
            GuardDown();

        if (!IsLoaded())
            if (UpdateTimer(diff, sShootTmr, cShootTimer))
            {
                sAimTmr = cMaxAimDurationTimer;
                shooterState = ssAim;
            }

        sDebugPhase = "Fire";
        break;

    case ssAlignBack:
		StopShooterControl();

        if (PID::RearOPAdjustCompleted())
        {
            StopMoving();
            shooterState = ssAim;
            robotState = rsNavigateBack;
            beginBackIR = -1;
            sDebugPhase = "******************";
        }

        PID::OPUpdate(false, diff, PID::OPPGainBack, 0, 0);
        sDebugPhase = "Align Back";
        break;
    }

    if (UpdateTimer(diff, LCDTimer, LCD_UPDATE_TIMER))
    {
        LCD_PRINTNEW("S:");
        LCD.print(sDebugPhase);
        LCD.setCursor(0, 1);
        switch (shooterState)
        {
        case ssAim:
            LCD.print("A ");
            LCD.print(GetAveragedLeftTargOPReading());
            LCD.print(" ");
            LCD.print(GetAveragedRightTargOPReading());
            LCD.print(" ");
            LCD.print(beginBackIR * cTurnIRFalloffCW);
            LCD.print(" ");
            LCD.print(GetRearAveragedSum());
            break;
        case ssFire:
            LCD.print("SPD ");
            LCD.print(shooterTopFreq);
            LCD.print(" ");
            LCD.print(shooterBotFreq);
            LCD.print(" ");
            break;
        case ssAlignBack:
            LCD.print("B ALGN ");
            LCD.print(GetLeftRearOPReading());
            LCD.print(" ");
            LCD.print(GetRightRearOPReading());
            LCD.print(" ");
            break;
        }
        
    }

    if (stopbutton())
    {
        StopShooterControl();
        shooterTopFreq = SetConstant("Shooter Top Freq", cShooterTopFreq, 1/2.0f);
        shooterBotFreq = SetConstant("Shooter Bot Freq", cShooterBotFreq, 1/2.0f);
        SetConstant("Right OP Gain", cFrontRightOPGain, 1/20.0f);
        StartShooterControl();
    }
}