void StartITG()
{
I2C_Object->startbus(Adress);
//set register 0x16 to 0x03. This probos should only be done 1 time
Write_1_To_Register(0x16, 0x03);
}
void ITG3200::Write_1_To_Register(char registerr, char Value_To_Write) {
char Data_To_Write[2];
// The register to write to
Data_To_Write[0] = registerr;
Data_To_Write[1] = Value_To_Write;
I2C_Object->writebus(Data_To_Write);
}
void Test() {
robotDrive.SetSafetyEnabled(false);
uint8_t toSend[10];//array of bytes to send over I2C
uint8_t toReceive[10];//array of bytes to receive over I2C
uint8_t numToSend = 1;//number of bytes to send
uint8_t numToReceive = 0;//number of bytes to receive
toSend[0] = 7; //send 0 to arduino
i2c.Transaction(toSend, numToSend, toReceive, numToReceive);
bool isSettingUp = true;
pickup.setGrabber(-1);
pickup.setLifter(1);
while (isSettingUp) {
isSettingUp = false;
if (grabOuterLimit.Get() == false) {
pickup.setGrabber(0);
}
else {
isSettingUp = true;
}
if (liftLowerLimit.Get()) {
pickup.setLifter(0);
}
else {
isSettingUp = true;
}
}
gyro.Reset();
liftEncoder.Reset();
grabEncoder.Reset();
toSend[0] = 8;
i2c.Transaction(toSend, numToSend, toReceive, numToReceive);
while(IsTest() && IsEnabled());
toSend[0] = 0;
i2c.Transaction(toSend, numToSend, toReceive, numToReceive);
}
void writeRegister( I2C& i2c, char registerAdress, char *data, int nbBytes )
{
if( nbBytes == 0 ) return;
i2c.start();
int ack = i2c.write( MMA8452_SLAVE_ADRESS );
if( ack == 0 ) return;
ack = i2c.write( registerAdress );
if( ack == 0 ) return;
for( int i = 0; i < nbBytes; i++ )
{
if( i2c.write(data[i]) != 1 )
{
return;
}
}
i2c.stop();
}
bool I2CTest::execute_i2c_api_case(I2C & i2c){
bool result = true;
String test_name;
test_name.sprintf("I2C:%d", i2c.port());
open_case(test_name.str());
if( i2c.open(I2C::RDWR) < 0 ){
print_case_message("Failed %s %d: port:%d", __FILE__, __LINE__, i2c.port());
result = false;
} else {
if( i2c.close() < 0 ){
print_case_message("Failed %s %d: port:%d", __FILE__, __LINE__, i2c.port());
result = false;
}
}
close_case(result);
return result;
}
/**********************************************************
* getExtTemp - Gets the current external temperature from the sensor.
*
* @return float - The external temperature in degrees C
**********************************************************/
float TMP421::getExtTemp(void)
{
uint8_t highByte = 0x00, lowByte = 0x00;
// I2c.beginTransmission(_address);
// I2c.write(0x01);
// I2c.endTransmission();
// I2c.requestFrom((int)_address, 2); // request 2 byte from address 1001000
// while(I2c.available())
// {
// highByte = I2c.read(); // Read the first octet
// lowByte = I2c.read(); // Read the second octet
// }
I2c.write(_address, (byte) 0x01);
I2c.read(_address, (uint8_t) 2);
if(I2c.available()) {
highByte = I2c.receive();
lowByte = I2c.receive();
}
return getFraction(highByte, lowByte);
}
virtual bool _onStart() {
xa=0, ya=0, za=0;
gc=0;
Info() << "Initializing I2C";
if (i2c.openDevice("/dev/i2c-1") < 0) {
Error() << "Unable to open i2c device";
return false;
}
Info() << "Initializing sensors";
if (l3gd20h.initialize() < 0) {
Error() << "Unable to initialize gyroscope";
return 255;
}
l3gd20h.onData = [&](float x, float y, float z) {
xa += x; ya += y; za +=z;
gc++;
};
if (l3gd20h.start(L3GD20H_RATE_OCTA) < 0) {
Error() << "Unable to start gyroscope data colection";
return 255;
}
Info() << "Initializing timers";
stats_timer.onTimeout = [&]() {
float epoll_mono, callback_mono, epoll_cpu, callback_cpu;
_event_poller->getTimings(epoll_mono, callback_mono, epoll_cpu, callback_cpu);
Info() << "Real Time: epoll" << epoll_mono << "callbacks" << callback_mono
<< "ratio" << roundTo(epoll_mono / (epoll_mono + callback_mono) * 100, 0.1) << "%"
<< "/" << roundTo(callback_mono / (epoll_mono + callback_mono) * 100, 0.1) << "%";
Info() << "CPU Time: epoll" << epoll_cpu << "callbacks" << callback_cpu
<< "ratio" << roundTo(epoll_cpu / (epoll_cpu + callback_cpu) * 100, 0.1) << "%"
<< "/" << roundTo(callback_cpu / (epoll_cpu + callback_cpu) * 100, 0.1) << "%";
};
stats_timer.start(5000);
l3gd20h_timer.onTimeout = [&]() {
Info() << "Gyroscope XYZ" << xa / gc << ya / gc << za / gc
<< "samples: " << gc;
xa = ya = za = 0;
gc = 0;
};
l3gd20h_timer.start(1000);
return Application::_onStart();
}
void readRegister( I2C& i2c, int registerAdress, char buff[], size_t nbBytes )
{
if( nbBytes == 0 ) return;
i2c.start();
int ack = i2c.write( MMA8452_SLAVE_ADRESS );
if( ack == 0 ) return;
ack = i2c.write( registerAdress );
if( ack == 0 ) return;
i2c.start();
ack = i2c.write( MMA8452_SLAVE_ADRESS | 1 );
if( ack == 0 ) return;
for( int i = 0; i < nbBytes; ++i )
{
buff[ i ] = i2c.read( i == nbBytes - 1 ? 0 : 1 );
}
i2c.stop();
}
void HardwareInit()
{
// Set this only if nRF is power at 2V or more
//nrf_power_dcdcen_set(true);
NRF_POWER->DCDCEN = 1;
IOPinCfg(s_GpioPins, s_NbGpioPins);
IOPinClear(0, BLUEIO_TAG_BME680_LED2_BLUE_PIN);
IOPinClear(0, BLUEIO_TAG_BME680_LED2_GREEN_PIN);
IOPinClear(0, BLUEIO_TAG_BME680_LED2_RED_PIN);
g_Timer.Init(s_TimerCfg);
// Initialize I2C
#ifdef NEBLINA_MODULE
g_Spi.Init(s_SpiCfg);
#else
g_I2c.Init(s_I2cCfg);
#endif
bsec_library_return_t bsec_status;
// NOTE : For BME680 air quality calculation, this library is require to be initialized
// before initializing the sensor driver.
bsec_status = bsec_init();
if (bsec_status != BSEC_OK)
{
printf("BSEC init failed\r\n");
return;
}
// Inititalize sensor
g_TphSensor.Init(s_TphSensorCfg, g_pIntrf, &g_Timer);
// g_TphSensor.Disable();
// g_I2c.Disable();
// while(1) __WFE();
if (g_TphSensor.DeviceID() == BME680_ID)
{
g_GasSensor.Init(s_GasSensorCfg, g_pIntrf, NULL);
}
g_TphSensor.StartSampling();
usDelay(300000);
// Update sensor data
TPHSENSOR_DATA tphdata;
g_TphSensor.Read(tphdata);
if (g_TphSensor.DeviceID() == BME680_ID)
{
GASSENSOR_DATA gdata;
g_GasSensor.Read(gdata);
}
g_TphSensor.StartSampling();
g_AdvData.Type = BLEADV_MANDATA_TYPE_TPH;
// Do memcpy to adv data. Due to byte alignment, cannot read directly into
// adv data
memcpy(g_AdvData.Data, ((uint8_t*)&tphdata) + sizeof(tphdata.Timestamp), sizeof(BLEADV_MANDATA_TPHSENSOR));
g_I2c.Disable();
#ifdef NRF52_SERIES
g_Adc.Init(s_AdcCfg);
g_Adc.OpenChannel(s_ChanCfg, s_NbChan);
g_Adc.StartConversion();
#endif
#ifdef USE_TIMER_UPDATE
// Only with SDK14
uint64_t period = g_Timer.EnableTimerTrigger(0, 500UL, TIMER_TRIG_TYPE_CONTINUOUS);
#endif
}
void ReadPTHData()
{
static uint32_t gascnt = 0;
TPHSENSOR_DATA data;
GASSENSOR_DATA gdata;
#if 1
g_I2c.Enable();
g_TphSensor.Read(data);
/*
if (g_TphSensor.DeviceID() == BME680_ID && (gascnt & 0x3) == 0)
{
g_GasSensor.Read(gdata);
}
*/
if ((gascnt & 0xf) == 0)
{
#ifdef NRF52_SERIES
g_Adc.Enable();
g_Adc.OpenChannel(s_ChanCfg, s_NbChan);
g_Adc.StartConversion();
g_AdvData.Type = BLEADV_MANDATA_TYPE_BAT;
memcpy(&g_AdvBat, &g_BatData, sizeof(BLUEIO_DATA_BAT));
#endif
}
else if ((gascnt & 0x3) == 0)
{
BLEADV_MANDATA_GASSENSOR gas;
g_GasSensor.Read(gdata);
g_AdvData.Type = BLEADV_MANDATA_TYPE_GAS;
gas.GasRes = gdata.GasRes[gdata.MeasIdx];
gas.AirQIdx = gdata.AirQualIdx;
memcpy(&g_GasData, &gas, sizeof(BLEADV_MANDATA_GASSENSOR));
g_TphSensor.StartSampling();
}
else
{
g_AdvData.Type = BLEADV_MANDATA_TYPE_TPH;
// NOTE : M0 does not access unaligned data
// use local 4 bytes align stack variable then mem copy
// skip timestamp as advertising pack is limited in size
memcpy(&g_TPHData, ((uint8_t*)&data) + sizeof(data.Timestamp), sizeof(BLEADV_MANDATA_TPHSENSOR));
}
// g_TphSensor.StartSampling();
g_I2c.Disable();
#ifdef NRF52_SERIES
g_Adc.Enable();
g_Adc.OpenChannel(s_ChanCfg, s_NbChan);
g_Adc.StartConversion();
#endif
#endif
// Update advertisement data
BleAppAdvManDataSet(g_AdvDataBuff, sizeof(g_AdvDataBuff), NULL, 0);
// BleAppAdvManDataSet((uint8_t*)&gascnt, sizeof(gascnt), NULL, 0);
gascnt++;
}
/**
* Runs the motors with Mecanum drive.
*/
void OperatorControl()//teleop code
{
robotDrive.SetSafetyEnabled(false);
gyro.Reset();
grabEncoder.Reset();
timer.Start();
timer.Reset();
double liftHeight = 0; //variable for lifting thread
int liftHeightBoxes = 0; //another variable for lifting thread
int liftStep = 0; //height of step in inches
int liftRamp = 0; //height of ramp in inches
double grabPower;
bool backOut;
uint8_t toSend[10];//array of bytes to send over I2C
uint8_t toReceive[10];//array of bytes to receive over I2C
uint8_t numToSend = 1;//number of bytes to send
uint8_t numToReceive = 0;//number of bytes to receive
toSend[0] = 1;//set the byte to send to 1
i2c.Transaction(toSend, 1, toReceive, 0);//send over I2C
bool isGrabbing = false;//whether or not grabbing thread is running
bool isLifting = false;//whether or not lifting thread is running
bool isBraking = false;//whether or not braking thread is running
float driveX = 0;
float driveY = 0;
float driveZ = 0;
float driveGyro = 0;
bool liftLastState = false;
bool liftState = false; //button pressed
double liftLastTime = 0;
double liftTime = 0;
bool liftRan = true;
Timer switchTimer;
Timer grabTimer;
switchTimer.Start();
grabTimer.Start();
while (IsOperatorControl() && IsEnabled())
{
// Use the joystick X axis for lateral movement, Y axis for forward movement, and Z axis for rotation.
// This sample does not use field-oriented drive, so the gyro input is set to zero.
toSend[0] = 1;
numToSend = 1;
driveX = driveStick.GetRawAxis(Constants::driveXAxis);//starts driving code
driveY = driveStick.GetRawAxis(Constants::driveYAxis);
driveZ = driveStick.GetRawAxis(Constants::driveZAxis);
driveGyro = gyro.GetAngle() + Constants::driveGyroTeleopOffset;
if (driveStick.GetRawButton(Constants::driveOneAxisButton)) {//if X is greater than Y and Z, then it will only go in the direction of X
toSend[0] = 6;
numToSend = 1;
if (fabs(driveX) > fabs(driveY) && fabs(driveX) > fabs(driveZ)) {
driveY = 0;
driveZ = 0;
}
else if (fabs(driveY) > fabs(driveX) && fabs(driveY) > fabs(driveZ)) {//if Y is greater than X and Z, then it will only go in the direction of Y
driveX = 0;
driveZ = 0;
}
else {//if Z is greater than X and Y, then it will only go in the direction of Z
driveX = 0;
driveY = 0;
}
}
if (driveStick.GetRawButton(Constants::driveXYButton)) {//Z lock; only lets X an Y function
toSend[0] = 7;
driveZ = 0;//Stops Z while Z lock is pressed
}
if (!driveStick.GetRawButton(Constants::driveFieldLockButton)) {//robot moves based on the orientation of the field
driveGyro = 0;//gyro stops while field lock is enabled
}
driveX = Constants::scaleJoysticks(driveX, Constants::driveXDeadZone, Constants::driveXMax * (.5 - (driveStick.GetRawAxis(Constants::driveThrottleAxis) / 2)), Constants::driveXDegree);
driveY = Constants::scaleJoysticks(driveY, Constants::driveYDeadZone, Constants::driveYMax * (.5 - (driveStick.GetRawAxis(Constants::driveThrottleAxis) / 2)), Constants::driveYDegree);
driveZ = Constants::scaleJoysticks(driveZ, Constants::driveZDeadZone, Constants::driveZMax * (.5 - (driveStick.GetRawAxis(Constants::driveThrottleAxis) / 2)), Constants::driveZDegree);
robotDrive.MecanumDrive_Cartesian(driveX, driveY, driveZ, driveGyro);//makes the robot drive
if (pdp.GetCurrent(Constants::grabPdpChannel) < Constants::grabManualCurrent) {
pickup.setGrabber(Constants::scaleJoysticks(grabStick.GetX(), Constants::grabDeadZone, Constants::grabMax, Constants::grabDegree)); //defines the grabber
if(grabTimer.Get() < 1) {
toSend[0] = 6;
}
}
else {
pickup.setGrabber(0);
grabTimer.Reset();
toSend[0] = 6;
}
if (Constants::grabLiftInverted) {
pickup.setLifter(-Constants::scaleJoysticks(grabStick.GetY(), Constants::liftDeadZone, Constants::liftMax, Constants::liftDegree)); //defines the lifter
}
else {
pickup.setLifter(Constants::scaleJoysticks(grabStick.GetY(), Constants::liftDeadZone, Constants::liftMax, Constants::liftDegree)); //defines the lifter
}
SmartDashboard::PutNumber("Lift Power", Constants::scaleJoysticks(grabStick.GetY(), Constants::liftDeadZone, Constants::liftMax, Constants::liftDegree));
SmartDashboard::PutBoolean("Is Lifting", isLifting);
if (Constants::scaleJoysticks(grabStick.GetY(), Constants::liftDeadZone, Constants::liftMax, Constants::liftDegree) != 0 || isLifting) { //if the robot is lifting
isBraking = false; //stop braking thread
SmartDashboard::PutBoolean("Braking", false);
}
else if(!isBraking) {
isBraking = true; //run braking thread
pickup.lifterBrake(isBraking);//brake the pickup
}
if (grabStick.GetRawButton(Constants::liftFloorButton)) {
liftHeight = 0;
pickup.lifterPosition(liftHeight, isLifting, grabStick);//start lifting thread
liftRan = true;
}
liftTime = timer.Get();
liftState = grabStick.GetRawButton(Constants::liftButton);
if (liftState) { //if button is pressed
if (!liftLastState) {
if (liftTime - liftLastTime < Constants::liftMaxTime) {
if (liftHeightBoxes < Constants::liftMaxHeightBoxes) {
liftHeightBoxes++; //adds 1 to liftHeightBoxes
}
}
else {
liftHeightBoxes = 1;
liftRamp = 0;
liftStep = 0;
}
}
liftLastTime = liftTime;
liftLastState = true;
liftRan = false;
}
else if (grabStick.GetRawButton(Constants::liftRampButton)) {
if (liftTime - liftLastTime > Constants::liftMaxTime) {
liftHeight = 0;
liftStep = 0;
}
liftRamp = 1; //prepares to go up ramp
liftLastTime = liftTime;
liftRan = false;
}
else if (grabStick.GetRawButton(Constants::liftStepButton)) {
if (liftTime - liftLastTime > Constants::liftMaxTime) {
liftHeight = 0;
liftRamp = 0;
}
liftStep = 1; //prepares robot for step
liftLastTime = liftTime;
liftRan = false;
}
else {
if (liftTime - liftLastTime > Constants::liftMaxTime && !liftRan) {
liftHeight = liftHeightBoxes * Constants::liftBoxHeight + liftRamp * Constants::liftRampHeight + liftStep * Constants::liftStepHeight; //sets liftHeight
if (liftHeightBoxes > 0) {
liftHeight -= Constants::liftBoxLip;
}
pickup.lifterPosition(liftHeight, isLifting, grabStick);//start lifting thread
liftRan = true;
}
liftLastState = false;
}
if (grabStick.GetRawButton(Constants::grabToteButton)) {//if grab button is pressed
grabPower = Constants::grabToteCurrent;
backOut = true;
if (!isGrabbing) {
pickup.grabberGrab(isGrabbing, grabPower, backOut, grabStick);//start grabber thread
}
}
else if (grabStick.GetRawButton(Constants::grabBinButton)) {//if grab button is pressed
grabPower = Constants::grabBinCurrent;
backOut = false;
if (!isGrabbing) {
pickup.grabberGrab(isGrabbing, grabPower, backOut, grabStick);//start grabber thread
}
}
else if (grabStick.GetRawButton(Constants::grabChuteButton)) {//if grab button is presset
SmartDashboard::PutBoolean("Breakpoint -2", false);
SmartDashboard::PutBoolean("Breakpoint -1", false);
SmartDashboard::PutBoolean("Breakpoint 0", false);
SmartDashboard::PutBoolean("Breakpoint 1", false);
SmartDashboard::PutBoolean("Breakpoint 2", false);
SmartDashboard::PutBoolean("Breakpoint 3", false);
SmartDashboard::PutBoolean("Breakpoint 4", false);
//Wait(.5);
if (!isGrabbing) {
//pickup.grabberChute(isGrabbing, grabStick);//start grabber thread
}
}
//determines what the LED's look like based on what the Robot is doing
if (isGrabbing) {
toSend[0] = 5;
numToSend = 1;
}
if (isLifting) {//if the grabbing thread is running
if (Constants::encoderToDistance(liftEncoder.Get(),Constants::liftEncoderTicks, Constants::liftEncoderBase, Constants::liftEncoderRadius) < liftHeight) {
toSend[0] = 3;
}
else {
toSend[0] = 4;
}
numToSend = 1;//sends 1 byte to I2C
}
if(!grabOuterLimit.Get()) { //tells if outer limit is hit with lights
if(switchTimer.Get() < 1) {
toSend[0] = 6;
}
}
else {
switchTimer.Reset();
}
if (driveStick.GetRawButton(Constants::sneakyMoveButton)) {
toSend[0] = 0;
numToSend = 1;
}
float distance = prox.GetVoltage() * Constants::ultrasonicVoltageToInches / 12; // distance from ultrasonic sensor
float rotations = (float) liftEncoder.Get(); // rotations on encoder
SmartDashboard::PutNumber("Distance", distance); // write stuff to smart dash
SmartDashboard::PutNumber("Current", pdp.GetCurrent(Constants::grabPdpChannel));
SmartDashboard::PutNumber("LED Current", pdp.GetCurrent(Constants::ledPdpChannel));
SmartDashboard::PutNumber("Lift Encoder", rotations);
SmartDashboard::PutNumber("Lift Height", liftHeight);
SmartDashboard::PutNumber("Grab Encoder", grabEncoder.Get());
SmartDashboard::PutBoolean("Grab Inner", grabInnerLimit.Get());
SmartDashboard::PutBoolean("Grab Outer", grabOuterLimit.Get());
SmartDashboard::PutNumber("Drive Front Left Current", pdp.GetCurrent(Constants::driveFrontLeftPin));
SmartDashboard::PutNumber("Drive Front Right Current", pdp.GetCurrent(Constants::driveFrontRightPin));
SmartDashboard::PutNumber("Drive Rear Left Current", pdp.GetCurrent(Constants::driveRearLeftPin));
SmartDashboard::PutNumber("Drive Rear Right Current", pdp.GetCurrent(Constants::driveRearRightPin));
SmartDashboard::PutNumber("Throttle", grabStick.GetZ());
i2c.Transaction(toSend, 1, toReceive, 0);//send and receive information from arduino over I2C
Wait(0.005); // wait 5ms to avoid hogging CPU cycles
} //end of teleop
isBraking = false;
toSend[0] = 0;
i2c.Transaction(toSend, numToSend, toReceive, numToReceive);
}
void Autonomous()
{
Timer timer;
float power = 0;
bool isLifting = false;
bool isGrabbing = false;
double liftHeight = Constants::liftBoxHeight-Constants::liftBoxLip;
double grabPower = Constants::grabAutoCurrent;
bool backOut;
uint8_t toSend[1];//array of bytes to send over I2C
uint8_t toReceive[0];//array of bytes to receive over I2C
uint8_t numToSend = 1;//number of bytes to send
uint8_t numToReceive = 0;//number of bytes to receive
toSend[0] = 2;//set the byte to send to 1
i2c.Transaction(toSend, numToSend, toReceive, numToReceive);//send over I2C
bool isSettingUp = true;
//pickup.setGrabber(-1); //open grabber all the way
pickup.setLifter(0.8);
while (isSettingUp && IsEnabled() && IsAutonomous()) {
isSettingUp = false;
/*if (grabOuterLimit.Get() == false) {
pickup.setGrabber(0); //open until limit
}
else {
isSettingUp = true;
}*/
if (liftLowerLimit.Get()) {
pickup.setLifter(0); //down till bottom
}
else {
isSettingUp = true;
}
}
gyro.Reset();
liftEncoder.Reset();
grabEncoder.Reset();
if (grabStick.GetZ() > .8) {
timer.Reset();
timer.Start();
while (timer.Get() < 1) {
robotDrive.MecanumDrive_Cartesian(0, power, 0, gyro.GetAngle()); // drive back
if(power>-.4){
power-=0.005;
Wait(.005);
}
}
robotDrive.MecanumDrive_Cartesian(0, 0, 0, gyro.GetAngle()); // STOP!!!
timer.Stop();
timer.Reset();
Wait(1);
}
power = 0;
while (isLifting && IsEnabled() && IsAutonomous()) {
Wait(.005);
}
backOut = Constants::autoBackOut;
pickup.grabberGrab(isGrabbing, grabPower, backOut, grabStick);
Wait(.005);
while (isGrabbing && IsEnabled() && IsAutonomous()) {
Wait(.005);
}
liftHeight = 3*Constants::liftBoxHeight;
Wait(.005);
pickup.lifterPosition(liftHeight, isLifting, grabStick);
Wait(.005);
while (isLifting && IsEnabled() && IsAutonomous()) {
Wait(.005);
}
while(prox.GetVoltage() * Constants::ultrasonicVoltageToInches / 12 < 2 && IsEnabled() && IsAutonomous()); // while the nearest object is closer than 2 feet
timer.Start();
while(prox.GetVoltage() * Constants::ultrasonicVoltageToInches < Constants::autoBackupDistance && timer.Get() < Constants::autoMaxDriveTime && IsEnabled() && IsAutonomous()) { // while the nearest object is further than 12 feet
if (power < .45) { //ramp up the power slowly
power += .00375;
}
robotDrive.MecanumDrive_Cartesian(0, power, 0, gyro.GetAngle()); // drive back
float distance = prox.GetVoltage() * Constants::ultrasonicVoltageToInches / 12; // distance from ultrasonic sensor
SmartDashboard::PutNumber("Distance", distance); // write stuff to smart dash
SmartDashboard::PutNumber("Drive Front Left Current", pdp.GetCurrent(Constants::driveFrontLeftPin));
SmartDashboard::PutNumber("Drive Front Right Current", pdp.GetCurrent(Constants::driveFrontRightPin));
SmartDashboard::PutNumber("Drive Rear Left Current", pdp.GetCurrent(Constants::driveRearLeftPin));
SmartDashboard::PutNumber("Drive Rear Right Current", pdp.GetCurrent(Constants::driveRearRightPin));
SmartDashboard::PutNumber("Gyro Angle", gyro.GetAngle());
SmartDashboard::PutNumber("Distance (in)", prox.GetVoltage() * Constants:: ultrasonicVoltageToInches);
Wait(.005);
}
timer.Reset();
while(timer.Get() < Constants::autoBrakeTime && IsEnabled() && IsAutonomous()) { // while the nearest object is further than 12 feet
robotDrive.MecanumDrive_Cartesian(0,Constants::autoBrakePower,0); ///Brake
}
float turn = 0;
while (fabs(turn) < 85 && IsEnabled() && IsAutonomous()) { //turn 90(ish) degrees
robotDrive.MecanumDrive_Cartesian(0,0,.1);
turn = gyro.GetAngle();
if (turn > 180) {
turn -= 360;
}
}
robotDrive.MecanumDrive_Cartesian(0,0,0); ///STOP!!!
timer.Stop();
toSend[0] = 8;
i2c.Transaction(toSend, numToSend, toReceive, numToReceive);
while(IsAutonomous() && IsEnabled());
toSend[0] = 0;
i2c.Transaction(toSend, numToSend, toReceive, numToReceive);
}
void SetData(char* data, int StartRegister, int EndRegister) {
prepread(StartRegister);
usleep(Delay);
I2C_Object->readbus(data + StartRegister, EndRegister - StartRegister);
}
void CloseITG()
{
I2C_Object->closebus();
}
// Prepares reading of a register
void ITG3200::prepread(int address) {
static char* send = new char[1];
send[0] = address - 3;
usleep(Delay);
I2C_Object->writebus(send);
}
/**
* Initialize the sensor
*/
TMP421::TMP421(uint8_t addr)
{
_address = addr;
I2c.begin();
}
