void DataSending::SendTheData(){
strIndex = 0;
Dashboard &dash = DriverStation::GetInstance()->GetHighPriorityDashboardPacker();
DriverStation *drive = DriverStation::GetInstance();
Send(true);//yes this is the dataSending Code
Send(drive->GetBatteryVoltage());//battery info
Send((batteryCurrent->GetVoltage()-2.5)*AMPS_CONSTANT);
GetDriveJoystickInfo();//joystick info
GetOperatorJoystickInfo();//moar joystick info
GetVicInfo();//victor info
Send(RobotMap::launcherSolenoidLeft->Get());//solenoid info
Send(RobotMap::launcherSolenoidRight->Get());
Send(RobotMap::shifterDoubleSolenoid->Get());
Send((bool)RobotMap::launcherCompressor->GetPressureSwitchValue());//sensor info
Send(RobotMap::robotRangeFinderUltrasonicSensor->GetVoltage()/VoltsPerCM);
Send(RobotMap::driveTrainGyro->GetAngle());
Send(RobotMap::launcherPressureSwitch->GetVoltage()*PSI_CONSTANT);//transducer1
Send(RobotMap::collectorLeftRoller->Get()*-1);//talon info
Send(RobotMap::collectorRightRoller->Get());
Send(count++);//number of packets
Send(timesPerSecond);//every nth number data is sent from a 50 hz source
//a.k.a 50 / above number = Hz
Send(drive->GetMatchTime());
Send(drive->IsEnabled());
dash.AddString(strBuffer);
dash.Finalize();
UpdateUserLCD();
}
void logdata()
{
//log data
fout << logTimer->Get() << ',' << CurrentMode() << ',' << ds->GetBatteryVoltage() << ',';
fout << magicBox->getGyroAngle() << ',';
autoLog();
myDrive->log(fout);
myShooter->log(fout);
//myClimber->Log(fout);
log_gamepad(fout, driverGamepad);
log_gamepad(fout, operatorGamepad);
fout<<LEDTimer<<",";
fout<<LEDPercent(LEDTimer)<<",";
fout<<LEDPercent(LEDTimer + 30)<<",";
fout<<endl;
}
void RA14Robot::logging() {
if (fout.is_open()) {
fout << missionTimer->Get() << ",";
#ifndef DISABLE_SHOOTER
myCam->log(fout);
#endif //Ends DISABLE_SHOOTER
myDrive->log(fout);
CurrentSensorSlot * slots[4] = { camMotor1Slot, camMotor2Slot,
driveLeftSlot, driveRightSlot };
DriverStation * ds = DriverStation::GetInstance();
for (int i = 0; i < 4; ++i) {
fout << slots[i]->Get() << ",";
}
fout << auto_case << "," << gyro->GetAngle() << "," << dropSensor->GetPosition() << "," << ds->GetBatteryVoltage() << ",";
fout << target->IsValid() << "," << target->IsHot() << "," << target->GetDistance() << "," << target->GetX() << ",";
fout << target->GetY() << "," << target->IsLeft() << "," << target->IsRight() << ",";
fout << ds->GetMatchTime() << "," << auto_state << ",";
fout << endl;
}
}
void
DashboardSender::SendData(Robot1073 *p)
{
static unsigned int packetCt = 0;
float tempFloat;
DriverStation *ds = DriverStation::GetInstance();
Dashboard &dash = ds->GetHighPriorityDashboardPacker();
dash.AddCluster();
dash.AddU32(packetCt++);
dash.AddU32(0xFFFFFFFF);
unsigned short packData = 1;
if(p->IsEnabled()){packData += 2;}
if(false){packData += 4;} // has tube
if(p->IsOperatorControl()){packData += 8;}
else if(p->IsAutonomous()){packData += 16;}
else {packData += 24;}
if(false){packData += 32;} // has line
if(p->arm->IsUpLimitSwitchActive()){packData += 64;}
if(p->arm->IsDownLimitSwitchActive()){packData += 128;}
if(p->pincer->IsClosedLimitSwitchActive()){packData += 256;}
if(p->pincer->IsOpenLimitSwitchActive()){packData += 512;}
if(p->leftLineSensor->Get()){ packData += 1024; }
if(p->middleLineSensor->Get()){packData += 2048; }
if(p->rightLineSensor->Get()){packData += 4096; }
if(p->kraken->GetMode() != p->kraken->IdleMode ) { packData += 1 << 13; }
dash.AddU16(packData);
//printf("left = %d middle = %d right = %d\n", p->leftLineSensor->Get(),p->middleLineSensor->Get(),p->rightLineSensor->Get());
for (int i = 1; i <= 8; i++) {
tempFloat = (float) AnalogModule::GetInstance(1)->GetAverageVoltage(i);
dash.AddFloat(tempFloat); // analogs
//printf("Float %d = %fV\n", i, tempFloat);
}
int module = 1;
DigitalModule * digitalModule = DigitalModule::GetInstance(module);
unsigned char relayForward = digitalModule->GetRelayForward();
dash.AddU8(relayForward); // relays (forward)
dash.AddU8(DigitalModule::GetInstance(module)->GetRelayReverse()); // relays (reverse)
dash.AddU16((short)DigitalModule::GetInstance(module)->GetDIO()); // state
dash.AddU16(DigitalModule::GetInstance(module)->GetDIODirection());//direction
for (int i = 1; i <= 10; i++) {
dash.AddU8((unsigned char) DigitalModule::GetInstance(module)->GetPWM(i)); // pwm's
//printf("PWM %d %02X\n" ,i, DigitalModule::GetInstance(module)->GetPWM(i));
}
dash.AddFloat(p->matchTimer->GetTimeRemaining());
dash.AddFloat(ds->GetBatteryVoltage());
dash.AddFloat(p->gyro->GetAngle());
dash.AddFloat(p->leftJoystick->GetX());
dash.AddFloat(p->rightJoystick->GetX());
dash.AddFloat(p->leftJoystick->GetY());
dash.AddFloat(p->rightJoystick->GetY());
std::pair<double, double> lrDistance = p->encoders->GetDistance();
dash.AddFloat((float)lrDistance.first);
dash.AddFloat((float)lrDistance.second);
// Navigation Data
dash.AddFloat(p->navigation->GetX());
dash.AddFloat(p->navigation->GetY());
dash.AddFloat(5);//accel temp
dash.AddFloat(5);//accel temp
dash.AddFloat(p->navigation->GetXVelocity());
dash.AddFloat(p->navigation->GetYVelocity());
dash.AddFloat(sqrt(p->navigation->GetXVar()));
dash.AddFloat(sqrt(p->navigation->GetYVar()));
dash.AddFloat(p->navigation->GetHeading());
dash.AddFloat(p->elevator->GetCurrentPositionFeet());
dash.AddFloat(p->elevator->GetTargetPositionFeet());
// jags++
//+++++++++++++++++++++++
dash.AddFloat((float)p->leftMotorJaguar->GetOutputCurrent());
dash.AddFloat((float)p->rightMotorJaguar->GetOutputCurrent());
dash.AddFloat((float)p->pincerJaguar->GetOutputCurrent());
dash.AddFloat((float)p->armJaguar->GetOutputCurrent());
dash.AddFloat((float)p->elevatorJaguarMotorA->GetOutputCurrent());
dash.AddFloat(35); // pincher % open
dash.AddU32(dashboardIndex);
dash.AddFloat(p->matchTimer->GetElapsedTime());
dash.AddFloat((float)p->systemTimer->Get());
int x = p->GetTargetPole();
int y = (p->GetTargetFoot()+1)/2;
dash.AddU8(x);
dash.AddU8(y);
dash.AddFloat(p->navigation->GetHeadingToPeg(x));
dash.AddFloat(p->navigation->GetHeadingToBait(x));
dash.AddFloat(p->navigation->GetDistanceToPeg(x));
dash.AddFloat(p->navigation->GetDistanceToBait(x));
dash.AddFloat(p->magEncoder->GetVoltage());
// dash.AddFloat(p->arm->magEncoder->GetVoltage());
dash.AddFloat(42.0);
dash.AddU32(0xDCBA25); // temporary test data to make sure data is aligned
dash.FinalizeCluster();
dash.Finalize();
}