diff --git a/src/Robot.cpp b/src/Robot.cpp index 61117ab..62b2b39 100644 --- a/src/Robot.cpp +++ b/src/Robot.cpp @@ -1,5 +1,13 @@ #include "WPILib.h" #include "Shooter.h" +#include +#include +#include +#include +#include +#include +#include +#include #include #ifndef BUTTON_LAYOUT @@ -22,6 +30,10 @@ #endif // BUTTON_LAYOUT +#ifndef LOG +#define LOG(X) logA << X; logB << X; logC << X //for writing data to the csv file +#endif + class Robot: public IterativeRobot { TalonSRX left_drive, right_drive; @@ -31,6 +43,8 @@ class Robot: public IterativeRobot RobotDrive drive; Shooter shooter; Joystick driver_stick, operator_stick; + //DriverStation DriverStation::GetInstance(); + //float power; public: Robot(): left_drive(0), // Left DriveTrain Talons plug into PWM channel 1 with a Y-splitter @@ -65,6 +79,20 @@ private: const std::string autoNameCustom = "My Auto"; std::string autoSelected; + /*void LogData() + { + static PowerDistributionPanel pdp; + static DriverStation DriverStation::GetInstance() = DriverStation::GetInstance(); + static std::vector motors; + + static std::ofstream logA, logB, logC; + timeval tm; + + SmartDashboard::PutBoolean("log A", logA.is_open()); + SmartDashboard::PutBoolean("log B", logB.is_open()); + SmartDashboard::PutBoolean("log C", logC.is_open()); + }*/ + void RobotInit() { chooser = new SendableChooser(); @@ -112,7 +140,12 @@ private: void AutonomousPeriodic() { - + LogCSVData(); + if(autoSelected == autoNameCustom){ + //Custom Auto goes here + } else { + //Default Auto goes here + } } void TeleopInit() @@ -122,6 +155,7 @@ private: void TeleopPeriodic() { + LogCSVData(); std::cout << "arm encoder position: " << arms.GetEncPosition() << std::endl; std::cout << "arm encoder velocity: " << arms.GetEncVel() << std::endl; drive.ArcadeDrive(-driver_stick.GetY(), -driver_stick.GetX()*0.75); @@ -285,6 +319,87 @@ private: void TestPeriodic() { lw->Run(); + LogCSVData(); + } + void LogCSVData() + { + static PowerDistributionPanel pdp; // preparing to read from the pdp + static std::vector motors; + + static std::ofstream logA, logB, logC; + timeval tm; + + SmartDashboard::PutBoolean("log A", logA.is_open()); + SmartDashboard::PutBoolean("log B", logB.is_open()); + SmartDashboard::PutBoolean("log C", logC.is_open()); + + if (!logA.is_open() && !logB.is_open() && !logC.is_open()) + { + std::fstream logNumFile; + int logNum; + logNumFile.open("/home/lvuser/logNum"); + logNumFile >> logNum; + logNum++; + logNumFile.seekp(0); + logNumFile << logNum; + // writing to /home/lvuser/logs/[unixtime].log + logA.open("/media/sda1/logs/log" + std::to_string(logNum) + ".csv"); + std::cerr << (logA.is_open() ? "Opened" : "Failed to open") << "log A." << std::endl; + logB.open("/media/sdb1/logs/log" + std::to_string(logNum) + ".csv"); + std::cerr << (logB.is_open() ? "Opened" : "Failed to open") << "log B." << std::endl; + logC.open("/home/lvuser/logs/log" + std::to_string(logNum) + ".csv"); + std::cerr << (logC.is_open() ? "Opened" : "Failed to open") << "log C." << std::endl; + + LOG("Time\tpdpInput voltage\tpdpTemperature\tpdpTotal Current\t"); + for (int ii = 0; ii < 16; ii++) + { + LOG("pdpChannel " << ii << " current\t"); + } + + LOG("tlaunch_spinny Bus Voltage\ttlaunch_spinny Output Current\ttlaunch_spinny Output Voltage\ttlaunch_spinny Temperature"); + motors.push_back(&ramp); + LOG("\tShooter1 Bus Voltage\tShooter1 Output Current\tShooter1 Output Voltage\tShooter1 Temperature"); + motors.push_back(&shooter1); + LOG("\tShooter2 Bus Voltage\tShooter2 Output Current\tShooter2 Output Voltage\tShooter2 Temperature"); + motors.push_back(&shooter2); + + LOG("\tJoystick X\tJoystick Y\tJoystick Twist"); + LOG("\tAlliance\tLocation\tMatch Time\tFMS Attached\tBrowned Out"); + LOG("\tTestStage"); + LOG(std::endl); + } + gettimeofday(&tm, NULL); + LOG(time(0) << '.' << std::setfill('0') << std::setw(3) << tm.tv_usec/1000); + // Some general information + LOG("\t" << pdp.GetVoltage()); + LOG("\t" << pdp.GetTemperature()); + LOG("\t" << pdp.GetTotalCurrent()); + // current on each channel + for (int ii = 0; ii < 16; ii++) + { + LOG("\t" << pdp.GetCurrent(ii)); + } + + //Talon Data + for(int ii = 0; ii < motors.size(); ii++) + { + LOG("\t" << motors[ii]->GetBusVoltage()); + LOG("\t" << motors[ii]->GetOutputVoltage()); + LOG("\t" << motors[ii]->GetOutputCurrent()); + LOG("\t" << motors[ii]->GetTemperature()); + } + //control data + LOG("\t" << driver_stick.GetX()); + LOG("\t" << driver_stick.GetY()); + LOG("\t" << driver_stick.GetTwist()); + + //DriverStation Data + LOG("\t" << DriverStation::GetInstance().GetAlliance()); + LOG("\t" << DriverStation::GetInstance().GetLocation()); + LOG("\t" << DriverStation::GetInstance().GetMatchTime()); + LOG("\t" << DriverStation::GetInstance().IsFMSAttached()); + LOG("\t" << DriverStation::GetInstance().IsSysBrownedOut()); + LOG(std::endl); } void SimpleDrive()