/**
* Constructor.
*
* @param moduleNumber The digital module that the sensor is plugged into (1 or 2).
*/
HiTechnicColorSensor::HiTechnicColorSensor(UINT8 moduleNumber)
: m_i2c (NULL)
{
m_table = NULL;
DigitalModule *module = DigitalModule::GetInstance(moduleNumber);
m_mode = kActive;
if (module)
{
m_i2c = module->GetI2C(kAddress);
// Verify Sensor
const UINT8 kExpectedManufacturer[] = "HiTechnc";
const UINT8 kExpectedSensorType[] = "ColorPD ";
if ( ! m_i2c->VerifySensor(kManufacturerBaseRegister, kManufacturerSize, kExpectedManufacturer) )
{
wpi_setWPIError(CompassManufacturerError);
return;
}
if ( ! m_i2c->VerifySensor(kSensorTypeBaseRegister, kSensorTypeSize, kExpectedSensorType) )
{
wpi_setWPIError(CompassTypeError);
}
nUsageReporting::report(nUsageReporting::kResourceType_HiTechnicColorSensor, moduleNumber - 1);
}
}
Multiplexer::Multiplexer()
{
DigitalModule *module = DigitalModule::GetInstance(1);
if (module)
{
m_i2c = module->GetI2C(kAddress);
}
}
void RobotTelemetry::updateTelemetryData() {
unsigned int buttonIndex = 0;
unsigned int index = 0;
DigitalModule* dio = DigitalModule::GetInstance(1);
AnalogModule* aio = AnalogModule::GetInstance(1);
//micro seconds to milliseconds
robotDiagnostics->time = (long)((GetFPGATime() - startTime) * 1.0e-3);
//robotDiagnostics->batteryVoltage = DriverStation::GetInstance()->GetBatteryVoltage();
for(index = 0; index < sizeof(robotDiagnostics->pwmOutputs)/sizeof(*robotDiagnostics->pwmOutputs); index++) {
robotDiagnostics->pwmOutputs[index] = dio->GetPWM(index+1);
}
for(index = 0; index < sizeof(robotDiagnostics->relayOutputs)/sizeof(*robotDiagnostics->relayOutputs); index++) {
robotDiagnostics->relayOutputs[index] = (dio->GetRelayForward(index+1) ? 1 : (dio->GetRelayReverse(index+1) ? 1 : 0));
}
for(index = 0; index < sizeof(robotDiagnostics->digitalInputs)/sizeof(*robotDiagnostics->digitalInputs); index++) {
robotDiagnostics->digitalInputs[index] = dio->GetDIO(index+1);
}
for(index = 0; index < sizeof(robotDiagnostics->analogInputs)/sizeof(*robotDiagnostics->analogInputs); index++) {
robotDiagnostics->analogInputs[index] = aio->GetAverageVoltage(index+1);
}
for(index = 0; index < sizeof(robotDiagnostics->pneumatics)/sizeof(*robotDiagnostics->pneumatics); index++) {
robotDiagnostics->pneumatics[index] = 0;
}
for(index = 0; index < sizeof(robotDiagnostics->joysticks)/sizeof(*robotDiagnostics->joysticks); index++) {
robotDiagnostics->joysticks[index].x = Joystick::GetStickForPort(index+1)->GetAxis(Joystick::kXAxis);
robotDiagnostics->joysticks[index].y = Joystick::GetStickForPort(index+1)->GetAxis(Joystick::kYAxis);
robotDiagnostics->joysticks[index].z = Joystick::GetStickForPort(index+1)->GetAxis(Joystick::kZAxis);
for(buttonIndex = 0; buttonIndex < 8; buttonIndex++) {
robotDiagnostics->joysticks[index].buttons[buttonIndex] = Joystick::GetStickForPort(index+1)->GetRawButton(buttonIndex+1);
}
}
for(index = 0; index < sizeof(robotDiagnostics->gamepads)/sizeof(*robotDiagnostics->gamepads); index++) {
robotDiagnostics->gamepads[index].x = Joystick::GetStickForPort(index+1)->GetAxis(Joystick::kXAxis);
robotDiagnostics->gamepads[index].y = Joystick::GetStickForPort(index+1)->GetAxis(Joystick::kXAxis);
robotDiagnostics->gamepads[index].z = Joystick::GetStickForPort(index+1)->GetAxis(Joystick::kXAxis);
for(buttonIndex = 0; buttonIndex < 8; buttonIndex++) {
robotDiagnostics->gamepads[index].buttons[buttonIndex] = Joystick::GetStickForPort(index+1)->GetRawButton(buttonIndex+1);
}
}
}
LSM303_I2C::LSM303_I2C(UINT32 slot) : accelI2C_r(NULL), accelI2C_w(NULL), magI2C(NULL)
{
DigitalModule *module = DigitalModule::GetInstance(slot);
accelI2C_r = module->GetI2C(accelAddressR);
accelI2C_w = module->GetI2C(accelAddressW);
magI2C = module->GetI2C(magAddress);
// Turn on the accelerometer
accelI2C_w->Write(CTRL_REG1_A, accelPowerOn);
// Set up accelerometer data format.
accelI2C_w->Write(CTRL_REG4_A, accelData);
// Speed up the magnetic sensor
magI2C->Write(CRA_REG_M, magSpeed);
// Wake up the magnetic sensor
magI2C->Write(MR_REG_M, magContinuous);
}
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();
}
