Robot.cpp

Go to the documentation of this file.

#include "WPILib.h"
#include <RobotComponent.h>
#include <Camera.h>
#include <MecanumDrive.h>

class Robot: public IterativeRobot
{
//private:
    Camera m_Camera;                
    MecanumDrive m_Drive;           

    Encoder encoder;
    Ultrasonic Sonic;
    DigitalInput bottomSwitch;
    DigitalInput topSwitch;
    DigitalInput frontSwitch;
    LiveWindow *lw;
    int autoLoopCounter;

public:
    Robot() :
        encoder(4, 5, true),
        Sonic(2,3, Ultrasonic::kMilliMeters),
        bottomSwitch(0),
        topSwitch(1),
        frontSwitch(6), 
        lw(NULL),
        autoLoopCounter(0)
    {
    }

private:
    void RobotInit()
    {
        lw = LiveWindow::GetInstance();
        m_Camera.ComponentInit();
        m_Drive.ComponentInit();
    }

    void DisabledInit()
    {
    }

    void DisabledPeriodic()
    {
    }


    void AutonomousInit()
    {
        autoLoopCounter = 0;
    }

    void AutonomousPeriodic()
    {
        if(autoLoopCounter < 100) //Check if we've completed 100 loops (approximately 2 seconds)
        {

            autoLoopCounter++;
            } else {

        }
    }

    void TeleopInit()
    {
    }

    void TeleopPeriodic()
    {
        if (IsEnabled())
        {
            /*
             * Sense() block for all RobotComponents
             */
            m_Camera.Sense();
            m_Drive.Sense();

            /*
             * DoWork() block for all RobotComponents
             */
            m_Camera.DoWork();
            m_Drive.DoWork();

            /***** Old-fashioned code follows *****/

            //Encoder stuff
            SmartDashboard::PutNumber("Motor Count", double ((encoder.Get()/71.164)/7)); 

            //Front Limit Switch
            if(frontSwitch.Get() == 1)
            {
                SmartDashboard::PutString("Front Switch", "Activated");
            }else
            {
                SmartDashboard::PutString("Front Switch", "_________");
            }
            
            //Bottom Switch
            if(bottomSwitch.Get() == 1)
            {
                SmartDashboard::PutString("Bottom Switch", "Activated");
            }else
            {
                SmartDashboard::PutString("Bottom Switch", "_________");
            }
            
            //Top Switch
            if(topSwitch.Get() == 1)
            {
                SmartDashboard::PutString("Top Switch", "Activated");
            }else
            {
                SmartDashboard::PutString("Top Switch", "_________");
            }

            //Ultrasonic Code
            Sonic.SetAutomaticMode(true);
            Sonic.SetEnabled(true);
            //Gets distance in mm, then changes into cm 
            double myDistance = (Sonic.GetRangeMM()/10); 
            //Outputs to SmartDashboard
            SmartDashboard::PutNumber("Ultrasonic Measurement", myDistance);
        }
        else //Do when disabled
        {
            //Resets encoder when robot is disabled
            if (encoder.Get() != 0){
                encoder.Reset();    
            } 
            //Turns off ultrasonic when disabled, it would keep runing otherwise
            Sonic.SetAutomaticMode(false);
        }
    }

    void TestInit()
    {
    }

    void TestPeriodic()
    {
        lw->Run();  // This method is called periodically to cause the sensors to send new values to the SmartDashboard.
    }
};


START_ROBOT_CLASS(Robot);