void UpdateDashboard() {
float r = 0.00001 * i;
SmartDashboard::PutNumber("State", currentState + r);
SmartDashboard::PutNumber("PID Turn Error",
turnController->GetError() + r);
SmartDashboard::PutNumber("PID Target",
turnController->GetSetpoint() + r);
// SmartDashboard::PutBoolean("Straight", straight);
SmartDashboard::PutData("test", turnController);
SmartDashboard::PutNumber("Yaw:", ahrs->GetYaw() + r);
SmartDashboard::PutNumber("Roll:", ahrs->GetRoll() + r);
SmartDashboard::PutNumber("Pitch", ahrs->GetPitch() + r);
SmartDashboard::PutNumber("Scissor 1", pdp->GetCurrent(1) + r);
SmartDashboard::PutNumber("Scissor 2", pdp->GetCurrent(2) + r);
SmartDashboard::PutNumber("Left Drive 1", pdp->GetCurrent(12) + r);
SmartDashboard::PutNumber("Left Drive 2", pdp->GetCurrent(13) + r);
SmartDashboard::PutNumber("Right Drive 1", pdp->GetCurrent(14) + r);
SmartDashboard::PutNumber("Right Drive 2", pdp->GetCurrent(15) + r);
SmartDashboard::PutNumber("Constant Lift", constantLift);
SmartDashboard::PutNumber("Rotate Rate", rotateRate + r);
i = (i + 1) % 2;
printf("2.1");
// .PutLong("test1.2", 1337);
printf("2.2");
// mqServer.PutDouble("test",DriverStation::GetInstance().GetMatchTime());
printf("2.3");
// mqServer.PutString("test1.1","YOLO_SWAGINS");
printf("2.4");
// SmartDashboard::PutString("test1.2", mqServer.GetString("test1.1"));
// SmartDashboard::PutNumber("test1", mqServer.GetDouble("test"));
// SmartDashboard::PutNumber("test1.3", mqServer.GetLong("test1.2"));
// SmartDashboard::PutNumber("test2", DriverStation::GetInstance().GetMatchTime());
}
/**
* Runs the motors with Mecanum drive.
*/
void OperatorControl()
{
robotDrive.SetSafetyEnabled(false);
while (IsOperatorControl() && IsEnabled())
{
bool collisionDetected = false;
double curr_world_linear_accel_x = ahrs->GetWorldLinearAccelX();
double currentJerkX = curr_world_linear_accel_x - last_world_linear_accel_x;
last_world_linear_accel_x = curr_world_linear_accel_x;
double curr_world_linear_accel_y = ahrs->GetWorldLinearAccelY();
double currentJerkY = curr_world_linear_accel_y - last_world_linear_accel_y;
last_world_linear_accel_y = curr_world_linear_accel_y;
if ( ( fabs(currentJerkX) > COLLISION_THRESHOLD_DELTA_G ) ||
( fabs(currentJerkY) > COLLISION_THRESHOLD_DELTA_G) ) {
collisionDetected = true;
}
SmartDashboard::PutBoolean( "CollisionDetected", collisionDetected);
try {
/* Use the joystick X axis for lateral movement, */
/* Y axis for forward movement, and Z axis for rotation. */
/* Use navX MXP yaw angle to define Field-centric transform */
robotDrive.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(),
stick.GetZ(),ahrs->GetAngle());
} catch (std::exception ex ) {
std::string err_string = "Error communicating with Drive System: ";
err_string += ex.what();
DriverStation::ReportError(err_string.c_str());
}
Wait(0.005); // wait 5ms to avoid hogging CPU cycles
}
}
int main(int argc, char *argv[])
{
QApplication a(argc, argv);
AHRS w;
w.show();
return a.exec();
}
// Start auto mode
void AutonomousInit() override {
autoSelected = *((int*) chooser.GetSelected()); // autonomous mode chosen in dashboard
currentState = 1;
ahrs->ZeroYaw();
ahrs->GetFusedHeading();
autoLength = SmartDashboard::GetNumber(AUTO_LENGTH, AUTO_LENGTH_DEFAULT);
autoSpeed = SmartDashboard::GetNumber(AUTO_SPEED, AUTO_SPEED_DEFAULT);
autoIntakeSpeed = SmartDashboard::GetNumber(AUTO_INTAKE_SPEED, AUTO_INTAKE_SPEED_DEFAULT);
liftdown->Set(false);
}
/**
* Drive based upon joystick inputs, and automatically control
* motors if the robot begins tipping.
*/
void OperatorControl()
{
robotDrive.SetSafetyEnabled(false);
while (IsOperatorControl() && IsEnabled()) {
double xAxisRate = stick.GetX();
double yAxisRate = stick.GetY();
double pitchAngleDegrees = ahrs->GetPitch();
double rollAngleDegrees = ahrs->GetRoll();
if ( !autoBalanceXMode &&
(fabs(pitchAngleDegrees) >=
fabs(kOffBalanceThresholdDegrees))) {
autoBalanceXMode = true;
}
else if ( autoBalanceXMode &&
(fabs(pitchAngleDegrees) <=
fabs(kOnBalanceThresholdDegrees))) {
autoBalanceXMode = false;
}
if ( !autoBalanceYMode &&
(fabs(pitchAngleDegrees) >=
fabs(kOffBalanceThresholdDegrees))) {
autoBalanceYMode = true;
}
else if ( autoBalanceYMode &&
(fabs(pitchAngleDegrees) <=
fabs(kOnBalanceThresholdDegrees))) {
autoBalanceYMode = false;
}
// Control drive system automatically,
// driving in reverse direction of pitch/roll angle,
// with a magnitude based upon the angle
if ( autoBalanceXMode ) {
double pitchAngleRadians = pitchAngleDegrees * (M_PI / 180.0);
xAxisRate = sin(pitchAngleRadians) * -1;
}
if ( autoBalanceYMode ) {
double rollAngleRadians = rollAngleDegrees * (M_PI / 180.0);
yAxisRate = sin(rollAngleRadians) * -1;
}
try {
// Use the joystick X axis for lateral movement, Y axis for forward movement, and Z axis for rotation.
robotDrive.MecanumDrive_Cartesian(xAxisRate, yAxisRate,stick.GetZ());
} catch (std::exception ex ) {
std::string err_string = "Drive system error: ";
err_string += ex.what();
DriverStation::ReportError(err_string.c_str());
}
Wait(0.005); // wait 5ms to avoid hogging CPU cycles
}
}
int main(int argc, char *argv[])
{
//-------------------- IMU setup and main loop ----------------------------
imuSetup();
ros::init(argc, argv, "ros_erle_imu_euler");
ros::NodeHandle n;
ros::Publisher imu_euler_pub = n.advertise<ros_erle_imu_euler::euler>("euler", 1000);
ros::Rate loop_rate(50);
while (ros::ok()){
//----------------------- Calculate delta time ----------------------------
gettimeofday(&tv,NULL);
previoustime = currenttime;
currenttime = 1000000 * tv.tv_sec + tv.tv_usec;
dt = (currenttime - previoustime) / 1000000.0;
if(dt < 1/1300.0)
usleep((1/1300.0-dt)*1000000);
gettimeofday(&tv,NULL);
currenttime = 1000000 * tv.tv_sec + tv.tv_usec;
dt = (currenttime - previoustime) / 1000000.0;
//-------- Read raw measurements from the MPU and update AHRS --------------
// Accel + gyro.
imu.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);
ahrs.updateIMU(ax, ay, az, gx*0.0175, gy*0.0175, gz*0.0175, dt);
//------------------------ Read Euler angles ------------------------------
ahrs.getEuler(&roll, &pitch, &yaw);
ros_erle_imu_euler::euler msg;
msg.roll = roll;
msg.pitch = pitch;
msg.yaw = yaw;
imu_euler_pub.publish(msg);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
void imuSetup ( void ) {
//MPU initialization
imu->initialize();
printf( "Beginning Gyro calibration...\n" );
for( int i = 0 ; i < 100; i++ ) {
imu->update();
imu->read_gyroscope( &gx , &gy , &gz);
offset[0] += ( -gx );
offset[1] += ( -gy );
offset[2] += ( -gz );
usleep(10000);
}
offset[0] /= 100.0;
offset[1] /= 100.0;
offset[2] /= 100.0;
printf( "Offsets are: %f %f %f\n" ,offset[0] ,offset[1] ,offset[2] );
ahrs.setGyroOffset( offset[0] , offset[1] , offset[2] );
}
void imuSetup()
{
//----------------------- MPU initialization ------------------------------
imu->initialize();
//-------------------------------------------------------------------------
printf("Beginning Gyro calibration...\n");
for(int i = 0; i<100; i++)
{
imu->update();
imu->read_gyroscope(&gx, &gy, &gz);
gx *= 180 / PI;
gy *= 180 / PI;
gz *= 180 / PI;
offset[0] += (-gx*0.0175);
offset[1] += (-gy*0.0175);
offset[2] += (-gz*0.0175);
usleep(10000);
}
offset[0]/=100.0;
offset[1]/=100.0;
offset[2]/=100.0;
printf("Offsets are: %f %f %f\n", offset[0], offset[1], offset[2]);
ahrs.setGyroOffset(offset[0], offset[1], offset[2]);
}
/**
* Runs the motors with Mecanum drive.
*/
void OperatorControl()
{
robotDrive.SetSafetyEnabled(false);
while (IsOperatorControl() && IsEnabled())
{
bool reset_yaw_button_pressed = stick.GetRawButton(1);
if ( reset_yaw_button_pressed ) {
ahrs->ZeroYaw();
}
bool rotateToAngle = false;
if ( stick.GetRawButton(2)) {
turnController->SetSetpoint(0.0f);
rotateToAngle = true;
} else if ( stick.GetRawButton(3)) {
turnController->SetSetpoint(90.0f);
rotateToAngle = true;
} else if ( stick.GetRawButton(4)) {
turnController->SetSetpoint(179.9f);
rotateToAngle = true;
} else if ( stick.GetRawButton(5)) {
turnController->SetSetpoint(-90.0f);
rotateToAngle = true;
}
double currentRotationRate;
if ( rotateToAngle ) {
turnController->Enable();
currentRotationRate = rotateToAngleRate;
} else {
turnController->Disable();
currentRotationRate = stick.GetTwist();
}
try {
/* Use the joystick X axis for lateral movement, */
/* Y axis for forward movement, and the current */
/* calculated rotation rate (or joystick Z axis), */
/* depending upon whether "rotate to angle" is active. */
robotDrive.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(),
currentRotationRate ,ahrs->GetAngle());
} catch (std::exception ex ) {
std::string err_string = "Error communicating with Drive System: ";
err_string += ex.what();
DriverStation::ReportError(err_string.c_str());
}
Wait(0.005); // wait 5ms to avoid hogging CPU cycles
}
}
void AutonomousGyroTurn() {
switch (currentState) {
case 1:
timer->Reset();
timer->Start();
//State: Stopped
//Transition: Driving State
currentState = 2;
break;
case 2:
//State: Driving
//Stay in State until 2 seconds have passed--`
//Transition: Gyroturn State
drive->TankDrive(0.5, 0.5);
if (timer->Get() >= 1) {
drive->TankDrive(0.0, 0.0);
ahrs->ZeroYaw();
currentState = 3;
turnController->SetSetpoint(90);
turnController->Enable();
}
break;
case 3:
//State: Gyroturn
//Stay in state until navx yaw has reached 90 degrees
//Transition: Driving State
drive->TankDrive(0.5 * rotateRate, -0.5 * rotateRate);
// if (ahrs->GetYaw() >= 90) {
if (turnController->OnTarget()) {
drive->TankDrive(0.0, 0.0);
currentState = 4;
timer->Reset();
timer->Start();
}
break;
case 4:
//State:Driving
//Stay in state until 2 seconds have passed
//Transition: State Stopped
drive->TankDrive(0.5, 0.5);
if (timer->Get() >= 1) {
currentState = 5;
timer->Stop();
}
break;
case 5:
//State: Stopped
drive->TankDrive(0.0, 0.0);
break;
}
}
int main(int argc, char *argv[])
{
if (argc == 3) {
try {
AHRS *imu = new AHRS(std::string(argv[1]),getBaudrate(argv[2]));
char *buffer = NULL;
size_t bufsize;
// Connect to the AHRS.
imu->openDevice();
printf("Connected to %s.\n", (imu->getInfo()).c_str());
// Prompt the user for a new baudrate.
printf("New speed? (baud) \n");
if (getline(&buffer, &bufsize, stdin) == -1)
{
fprintf(stderr, "Unable to read in new baudrate.\n");
imu->closeDevice();
return -1;
}
// Attempt to read in a baudrate and change to it.
imu->setBaudrate(getBaudrate(buffer));
// Clean up and close resources.
free(buffer);
imu->closeDevice();
// Tell the user of the result.
fprintf(stderr, "Changed speed successfully.\n");
exit(0);
} catch (std::exception& e) {
printError(e);
}
}
printUsage();
}
/**
* Runs the motors with Mecanum drive.
*/
void OperatorControl()
{
robotDrive.SetSafetyEnabled(false);
while (IsOperatorControl() && IsEnabled())
{
bool motionDetected = ahrs->IsMoving();
SmartDashboard::PutBoolean("MotionDetected", motionDetected);
try {
/* Use the joystick X axis for lateral movement, */
/* Y axis for forward movement, and Z axis for rotation. */
/* Use navX MXP yaw angle to define Field-centric transform */
robotDrive.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(),
stick.GetZ(),ahrs->GetAngle());
} catch (std::exception ex ) {
std::string err_string = "Error communicating with Drive System: ";
err_string += ex.what();
DriverStation::ReportError(err_string.c_str());
}
Wait(0.005); // wait 5ms to avoid hogging CPU cycles
}
}
void AutonomousAdjustableStraight() {
switch (currentState) {
case 1:
timer->Reset();
timer->Start();
turnController->Reset();
turnController->SetSetpoint(ahrs->GetYaw());
turnController->Enable();
currentState = 2;
break;
case 2:
intakeLever->Set(autoIntakeSpeed);
if (timer->Get() >= 1) {
intakeLever->Set(0);
currentState = 3;
timer->Reset();
timer->Start();
}
break;
case 3:
drive->TankDrive(autoSpeed, autoSpeed);
intakeLever->Set(-0.1);
if (timer->Get() >= autoLength) {
intakeLever->Set(0.0);
drive->TankDrive(0.0, 0.0);
currentState = 4;
timer->Reset();
timer->Start();
}
break;
case 4:
intake->Set(0.5);
shooter->Set(-0.5);
if (timer->Get() >= 2) {
currentState = 5;
}
break;
case 5:
intake->Set(0.0);
shooter->Set(0.0);
drive->TankDrive(0.0, 0.0);
break;
}
}
void RobotInit() override {
lw = LiveWindow::GetInstance();
drive->SetExpiration(20000);
drive->SetSafetyEnabled(false);
//Gyroscope stuff
try {
/* Communicate w/navX-MXP via the MXP SPI Bus. */
/* Alternatively: I2C::Port::kMXP, SerialPort::Port::kMXP or SerialPort::Port::kUSB */
/* See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details. */
ahrs = new AHRS(SPI::Port::kMXP);
} catch (std::exception ex) {
std::string err_string = "Error instantiating navX-MXP: ";
err_string += ex.what();
//DriverStation::ReportError(err_string.c_str());
}
if (ahrs) {
LiveWindow::GetInstance()->AddSensor("IMU", "Gyro", ahrs);
ahrs->ZeroYaw();
// Kp Ki Kd Kf PIDSource PIDoutput
turnController = new PIDController(0.015f, 0.003f, 0.100f, 0.00f,
ahrs, this);
turnController->SetInputRange(-180.0f, 180.0f);
turnController->SetOutputRange(-1.0, 1.0);
turnController->SetAbsoluteTolerance(2); //tolerance in degrees
turnController->SetContinuous(true);
}
chooser.AddDefault("No Auto", new int(0));
chooser.AddObject("GyroTest Auto", new int(1));
chooser.AddObject("Spy Auto", new int(2));
chooser.AddObject("Low Bar Auto", new int(3));
chooser.AddObject("Straight Spy Auto", new int(4));
chooser.AddObject("Adjustable Straight Auto", new int(5));
SmartDashboard::PutNumber(AUTO_LENGTH, AUTO_LENGTH_DEFAULT);
SmartDashboard::PutNumber(AUTO_SPEED, AUTO_SPEED_DEFAULT);
SmartDashboard::PutNumber(AUTO_INTAKE_SPEED, AUTO_INTAKE_SPEED_DEFAULT);
SmartDashboard::PutData("Auto Modes", &chooser);
liftdown->Set(false);
comp->Start();
}
void AutonomousStraightSpy() {
switch (currentState) {
case 1:
timer->Reset();
timer->Start();
turnController->Reset();
turnController->SetSetpoint(ahrs->GetYaw());
turnController->Enable();
currentState = 2;
break;
case 2:
intakeLever->Set(0.25);
if (timer->Get() >= 1) {
intakeLever->Set(0);
currentState = 3;
timer->Reset();
timer->Start();
}
break;
case 3:
drive->TankDrive(0.5, 0.5);
if (timer->Get() >= 5) {
drive->TankDrive(0.0, 0.0);
currentState = 4;
timer->Reset();
timer->Start();
}
break;
case 4:
intake->Set(0.5);
if (timer->Get() >= 2) {
currentState = 5;
}
break;
case 5:
intake->Set(0.0);
drive->TankDrive(0.0, 0.0);
break;
}
}
void imuSetup()
{
//----------------------- MPU initialization ------------------------------
imu.initialize();
//-------------------------------------------------------------------------
printf("Beginning Gyro calibration...\n");
for(int i = 0; i<100; i++)
{
imu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
offset[0] += (-gx*0.0175);
offset[1] += (-gy*0.0175);
offset[2] += (-gz*0.0175);
usleep(10000);
}
offset[0]/=100.0;
offset[1]/=100.0;
offset[2]/=100.0;
printf("Offsets are: %f %f %f\n", offset[0], offset[1], offset[2]);
ahrs.setGyroOffset(offset[0], offset[1], offset[2]);
}
void imuLoop()
{
//----------------------- Calculate delta time ----------------------------
gettimeofday(&tv,NULL);
previoustime = currenttime;
currenttime = 1000000 * tv.tv_sec + tv.tv_usec;
dt = (currenttime - previoustime) / 1000000.0;
if(dt < 1/1300.0) usleep((1/1300.0-dt)*1000000);
gettimeofday(&tv,NULL);
currenttime = 1000000 * tv.tv_sec + tv.tv_usec;
dt = (currenttime - previoustime) / 1000000.0;
//-------- Read raw measurements from the MPU and update AHRS --------------
// Accel + gyro.
/*
imu->update();
imu->read_accelerometer(&ax, &ay, &az);
imu->read_gyroscope(&gx, &gy, &gz);
ax /= G_SI;
ay /= G_SI;
az /= G_SI;
gx *= 180 / PI;
gy *= 180 / PI;
gz *= 180 / PI;
ahrs.updateIMU(ax, ay, az, gx*0.0175, gy*0.0175, gz*0.0175, dt);
*/
// Accel + gyro + mag.
// Soft and hard iron calibration required for proper function.
imu->update();
imu->read_accelerometer(&ay, &ax, &az);
imu->read_gyroscope(&gy, &gx, &gz);
imu->read_magnetometer(&my, &mx, &mz);
ax /= -G_SI;
ay /= -G_SI;
az /= G_SI;
gx *= 180 / PI;
gy *= 180 / PI;
gz *= -180 / PI;
ahrs.update(ax, ay, az, gx*0.0175, gy*0.0175, gz*0.0175, my, mx, -mz, dt);
//------------------------ Read Euler angles ------------------------------
//ahrs.getEuler(&pitch, &roll, &yaw);//roll and pitch inverted
ahrs.getEuler(&roll, &pitch, &yaw);
//------------------- Discard the time of the first cycle -----------------
if (!isFirst)
{
if (dt > maxdt) maxdt = dt;
if (dt < mindt) mindt = dt;
}
isFirst = 0;
}
void imuLoop()
{
float dtsum = 0.0f; //sum of delta t's
while(dtsum < 1.0f/freq) //run this loop at 1300 Hz (Max frequency gives best results for Mahony filter)
{
//----------------------- Calculate delta time ----------------------------
gettimeofday(&tv,NULL);
previoustime = currenttime;
currenttime = 1000000 * tv.tv_sec + tv.tv_usec;
dt = (currenttime - previoustime) / 1000000.0;
if(dt < 1/1300.0) usleep((1/1300.0-dt)*1000000);
gettimeofday(&tv,NULL);
currenttime = 1000000 * tv.tv_sec + tv.tv_usec;
dt = (currenttime - previoustime) / 1000000.0;
//-------- Read raw measurements from the MPU and update AHRS --------------
// Accel + gyro.
imu->update();
imu->read_accelerometer(&ax, &ay, &az);
imu->read_gyroscope(&gx, &gy, &gz);
ax /= G_SI;
ay /= G_SI;
az /= G_SI;
gx *= 180 / PI;
gy *= 180 / PI;
gz *= 180 / PI;
ahrs.updateIMU(ax, ay, az, gx*0.0175, gy*0.0175, gz*0.0175, dt);
// Accel + gyro + mag.
// Soft and hard iron calibration required for proper function.
/*
imu->update();
imu->read_accelerometer(&ay, &ax, &az);
az *= -1;
imu->read_gyroscope(&gy, &gx, &gz);
gz *= -1;
imu->read_magnetometer(&mx, &my, &mz);
ax /= G_SI;
ay /= G_SI;
az /= G_SI;
gx *= 180 / PI;
gy *= 180 / PI;
gz *= 180 / PI;
ahrs.update(-ax, -ay, -az, gx*0.0175, gy*0.0175, gz*0.0175, mx, my, mz, dt);
*/
//------------------------ Read Euler angles ------------------------------
ahrs.getEuler(&pitch, &roll, &yaw);
//------------------- Discard the time of the first cycle -----------------
if (!isFirst)
{
if (dt > maxdt) maxdt = dt;
if (dt < mindt) mindt = dt;
}
isFirst = 0;
dtsum += dt;
}
}
void SetAHRSPosData(AHRSProtocol::AHRSPosUpdate& ahrs_update) {
/* Update base IMU class variables */
ahrs->yaw = ahrs_update.yaw;
ahrs->pitch = ahrs_update.pitch;
ahrs->roll = ahrs_update.roll;
ahrs->compass_heading = ahrs_update.compass_heading;
ahrs->yaw_offset_tracker->UpdateHistory(ahrs_update.yaw);
/* Update AHRS class variables */
// 9-axis data
ahrs->fused_heading = ahrs_update.fused_heading;
// Gravity-corrected linear acceleration (world-frame)
ahrs->world_linear_accel_x = ahrs_update.linear_accel_x;
ahrs->world_linear_accel_y = ahrs_update.linear_accel_y;
ahrs->world_linear_accel_z = ahrs_update.linear_accel_z;
// Gyro/Accelerometer Die Temperature
ahrs->mpu_temp_c = ahrs_update.mpu_temp;
// Barometric Pressure/Altitude
ahrs->altitude = ahrs_update.altitude;
ahrs->baro_pressure = ahrs_update.barometric_pressure;
// Status/Motion Detection
ahrs->is_moving =
(((ahrs_update.sensor_status &
NAVX_SENSOR_STATUS_MOVING) != 0)
? true : false);
ahrs->is_rotating =
(((ahrs_update.sensor_status &
NAVX_SENSOR_STATUS_YAW_STABLE) != 0)
? false : true);
ahrs->altitude_valid =
(((ahrs_update.sensor_status &
NAVX_SENSOR_STATUS_ALTITUDE_VALID) != 0)
? true : false);
ahrs->is_magnetometer_calibrated =
(((ahrs_update.cal_status &
NAVX_CAL_STATUS_MAG_CAL_COMPLETE) != 0)
? true : false);
ahrs->magnetic_disturbance =
(((ahrs_update.sensor_status &
NAVX_SENSOR_STATUS_MAG_DISTURBANCE) != 0)
? true : false);
ahrs->quaternionW = ahrs_update.quat_w;
ahrs->quaternionX = ahrs_update.quat_x;
ahrs->quaternionY = ahrs_update.quat_y;
ahrs->quaternionZ = ahrs_update.quat_z;
ahrs->velocity[0] = ahrs_update.vel_x;
ahrs->velocity[1] = ahrs_update.vel_y;
ahrs->velocity[2] = ahrs_update.vel_z;
ahrs->displacement[0] = ahrs_update.disp_x;
ahrs->displacement[1] = ahrs_update.disp_y;
ahrs->displacement[2] = ahrs_update.disp_z;
ahrs->yaw_angle_tracker->NextAngle(ahrs->GetYaw());
}
void imuLoop ( void ) {
//Calculate delta time
gettimeofday( &tv , NULL );
previoustime = currenttime;
currenttime = 1000000 * tv.tv_sec + tv.tv_usec;
dt = ( currenttime - previoustime ) / 1000000.0;
if( dt < 1/1300.0 ) usleep( ( 1/1300.0 - dt ) * 1000000);
gettimeofday( &tv , NULL) ;
currenttime = 1000000 * tv.tv_sec + tv.tv_usec;
dt = ( currenttime - previoustime ) / 1000000.0;
//Read raw measurements from the MPU and update AHRS
// Accel + gyro + mag.
// Soft and hard iron calibration required for proper function.
imu -> update();
imu -> read_accelerometer( &ay , &ax , &az );
imu -> read_gyroscope( &gy , &gx , &gz );
imu -> read_magnetometer( &mx , &my , &mz );
ax /= G_SI;
ay /= G_SI;
az /= G_SI;
my = - my;
ahrs.update( ax , ay , az , gx , gy , gz , mx , my , mz , dt );
//Read Euler angles
ahrs.getEuler( &pitch , &roll , &yaw );
//Discard the time of the first cycle
if ( !isFirst ) {
if ( dt > maxdt ) maxdt = dt;
if ( dt < mindt ) mindt = dt;
}
isFirst = 0;
//Console and network output with a lowered rate
dtsumm += dt;
if( dtsumm > 0.05 ) {
//Console output
//printf("ROLL: %+05.2f PITCH: %+05.2f YAW: %+05.2f PERIOD %.4fs RATE %dHz \n", roll, pitch, gz/*yaw * -1*/, dt, int(1/dt));
// Network output
//sprintf( sendline , "%10f %10f %10f %10f %dHz\n" ,ahrs.getW() ,ahrs.getX() ,ahrs.getY() ,ahrs.getZ() ,int( 1/dt ) );
//sendto( sockfd , sendline , strlen( sendline ) , 0 , ( struct sockaddr * )&servaddr , sizeof( servaddr ) );
dtsumm = 0;
}
}
void AutonomousSpy() {
// Strategy 1 - start as spy with a boulder, score in lower goal. Starts with intake facing low goal
// -------------------------------------------------------------------------------------------------------------------
switch (currentState) {
case 1:
// -State: stopped
timer->Reset();
timer->Start();
ahrs->ZeroYaw();
currentState = 2;
break;
// --transition: state Driving Forward
case 2:
// -State: Driving Forward
// --wait until lined up with low goal
// --transition: State stopped
drive->TankDrive(0.5, 0.5);
if (timer->Get() >= 1) { // NEEDS TO BE SET
// -State: stopped
// --wait until stopped
drive->TankDrive(0.0, 0.0);
currentState = 3;
timer->Reset();
timer->Start();
}
break;
// --transition: State Shooting
case 3:
// -State: Shooting
// --wait until shooting complete
intake->Set(-.5);
if (timer->Get() >= .7) { //Find Out Actual Time
intake->Set(0);
timer->Reset();
timer->Start();
currentState = 4;
}
break;
// --transition: State Backing Up
case 4:
// -State: Backing Up
// --wait until off tower ramp
drive->TankDrive(-0.5, -0.5);
if (timer->Get() > 1) {
drive->TankDrive(0.0, 0.0);
ahrs->ZeroYaw();
ahrs->Reset();
currentState = 5;
turnController->SetSetpoint(-65.5);
turnController->Enable();
}
break;
// --transition: Turning
case 5:
// -State: Turning Left
// --wait until 65 degrees has been reached to line up with low bar
drive->TankDrive(-0.5, 0.5);
if (turnController->OnTarget()) {
drive->TankDrive(0.0, 0.0);
timer->Reset();
timer->Start();
currentState = 6;
}
break;
// --transition: Backing Up
case 6:
// -State backing Up
// --wait until near guard wall
drive->TankDrive(-0.5, -0.5);
if (timer->Get() >= 1) {
drive->TankDrive(0.0, 0.0);
ahrs->ZeroYaw();
ahrs->Reset();
currentState = 7;
turnController->SetSetpoint(-24.5);
turnController->Enable();
}
break;
// --transition: Turn Left
case 7:
// -State: Turn Right
// --wait until 25 degree turn has been made to line with low bar
drive->TankDrive(-0.5, 0.5);
if (turnController->OnTarget()) {
drive->TankDrive(0.0, 0.0);
timer->Reset();
timer->Start();
currentState = 8;
}
break;
// --transition: Back Up
case 8:
// -State: Backing Up
// --wait until backed through low bar
drive->TankDrive(-0.5, -0.5);
if (timer->Get() >= 1) { // NeedTo Update Value
timer->Stop();
currentState = 9;
}
break;
// --transition: Stopped
case 9:
// -State: Stopped
drive->TankDrive(0.0, 0.0);
break;
}
}
void TeleopPeriodic() override {
float leftPower, rightPower; // Get the values for the main drive train joystick controllers
leftPower = -leftjoystick->GetY();
rightPower = -rightjoystick->GetY();
float multiplier; // TURBO mode
if (rightjoystick->GetRawButton(1))
{
multiplier = 1;
} else {
multiplier = 0.5;
}
// wtf is a setpoint - it's an angle to turn to
if (leftjoystick->GetRawButton(6)) {
turnController->Reset();
turnController->SetSetpoint(0);
turnController->Enable();
ahrs->ZeroYaw();
//ahrs->Reset();
}
// Press button to auto calculate angle to rotate bot to nearest ball
// if(leftjoystick->GetRawButton(99))
// {
// ahrs->ZeroYaw();
// turnController->Reset();
// turnController->SetSetpoint(mqServer.GetDouble("angle"));
// turnController->Enable();
// aimState = 1;
// }
switch(aimState)
{
default:
case 0: // No camera assisted turning
//Drive straight with one controller, else: drive with two controllers
if (leftjoystick->GetRawButton(1)) {
drive->TankDrive(leftPower * multiplier, leftPower * multiplier,
false);
} else if (leftjoystick->GetRawButton(2)) {
drive->TankDrive(leftPower * multiplier + rotateRate,
leftPower * multiplier + -rotateRate, false);
} else {
drive->TankDrive(leftPower * multiplier, rightPower * multiplier,
false);
}
break;
case 1: // Camera assisted turning, deny input from controllers
drive->TankDrive(rotateRate, -rotateRate, false);
if(turnController->OnTarget() || leftjoystick->GetRawButton(97)) {
aimState = 0; // Finished turning, auto assist off
turnController->Disable();
turnController->Reset();
}
break;
}
// That little flap at the bottom of the joystick
float scaleIntake = (1 - (controlstick->GetThrottle() + 1) / 2);
// Depending on the button, our intake will eat or shoot the ball
if (controlstick->GetRawButton(2)) {
intake->Set(-scaleIntake);
shooter->Set(scaleIntake);
} else if (controlstick->GetRawButton(1)) {
intake->Set(scaleIntake);
shooter->Set(-scaleIntake);
} else {
intake->Set(0);
shooter->Set(0);
}
// Control the motor that lifts and descends the intake bar
float intake_lever_power = 0;
if (controlstick->GetRawButton(6)) {
manual = true;
intake_lever_power = .3;
// intakeLever->Set(.30); // close
} else if (controlstick->GetRawButton(4)) {
manual = true;
intake_lever_power = -.4;
// intakeLever->Set(-.40); // open
} else if (controlstick->GetRawButton(3)){
manual = true;
intake_lever_power = -scaleIntake;
// intakeLever->Set(-scaleIntake);
} else if (controlstick->GetRawButton(5)) {
manual = true;
intake_lever_power = scaleIntake;
// intakeLever->Set(scaleIntake);
} else {
if (manual) {
manual = false;
lastLiftPos = intakeLever->GetEncPosition();
intakeLever->SetControlMode(CANTalon::ControlMode::kPosition);
intakeLever->SetFeedbackDevice(CANTalon::FeedbackDevice::QuadEncoder);
intakeLever->SetPID(1, 0.001, 0.0);
intakeLever->EnableControl();
}
intake_hold = true;
intakeLever->Set(lastLiftPos);
}
if (manual) {
intake_hold = false;
intakeLever->SetControlMode(CANTalon::ControlMode::kPercentVbus);
intakeLever->Set(intake_lever_power);
}
if (controlstick->GetRawButton(11)) {
lift->Set(true);
liftdown->Set(false);
} else if (controlstick->GetRawButton(12)){
lift->Set(false);
liftdown->Set(true);
} else if (controlstick->GetRawButton(7)) {
liftdown->Set(false);
}
if (controlstick->GetRawButton(9)) {
winch->Set(scaleIntake);
} else if (controlstick->GetRawButton(10)) {
winch->Set(-scaleIntake);
} else {
winch->Set(0);
}
if (controlstick->GetPOV() == 0 && !bounce ) {
constantLift -= 0.05;
bounce = true;
} else if (controlstick->GetPOV() == 180 && !bounce) {
constantLift += 0.05;
bounce = true;
} else if (controlstick->GetPOV() == 270 && !bounce) {
constantLift = 0;
bounce = true;
} else {
bounce = false;
}
UpdateDashboard();
}
int main(int argc, char *argv[])
{
if (argc == 3) {
try {
AHRS *ahrs = new AHRS(std::string(argv[1]),getBaudrate(argv[2]));
AHRS::AcqConfig config;
AHRS::FilterData filters;
long unsigned int i;
ahrs->openDevice();
printf("Connected to %s.\n", (ahrs->getInfo()).c_str());
ahrs->sendAHRSDataFormat();
printf("AHRS Configuration data:\n");
config = ahrs->getAcqConfig();
printf("Acqusition mode: %d\n", config.poll_mode);
printf("Flush Filter: %d\n", config.flush_filter);
printf("Sample delay: %f\n", config.sample_delay);
filters = ahrs->getFIRFilters();
printf("Number of filters: %lu\n", filters.size());
for (i=0; i < filters.size(); i++)
printf("Filter #%.2lu: %f\n",i, filters[i]);
printf("Mag Truth Method: %d\n", ahrs->getMagTruthMethod());
printf("Functional mode: %d\n", ahrs->getAHRSMode());
printf("Declination: %f\n", ahrs->getDeclination());
printf("UserCalNumPoints: %d\n", ahrs->getCalPoints());
printf("Mag Setting: %d\n", ahrs->getMagCalID());
printf("Accel setting: %d\n", ahrs->getAccelCalID());
printf("Mounting mode: %d\n", ahrs->getMounting());
printf("Baudrate: %d\n", ahrs->getBaudrate().baud);
printf("True north? %d\n", ahrs->getTrueNorth());
printf("Big endian? %d\n", ahrs->getBigEndian());
printf("Auto sampling? %d\n", ahrs->getAutoCalibration());
printf("Mils/Degrees? %d\n", ahrs->getMils());
printf("HPR During Cal? %d\n", ahrs->getHPRCal());
ahrs->closeDevice();
exit(0);
}
catch (std::exception& e)
{
printError(e);
}
}
printUsage();
}
