UNTESTEDgit status changes InterLinkElite implementation to be separate class. also adds some driving direction control stuff

This commit is contained in:
Brendan Haines 2015-05-16 01:08:42 -06:00
parent 7bb239c59d
commit d4a08743ef
3 changed files with 189 additions and 39 deletions

96
src/InterLinkElite.cpp Normal file
View File

@ -0,0 +1,96 @@
/*
* InterLinkElite.cpp
*
* Created on: May 16, 2015
* Author: user
*/
#include "InterLinkElite.h"
InterLinkElite::InterLinkElite( int port )
{
stick = new Joystick( port );
}
double InterLinkElite::getAile()
{
return stick->GetRawAxis( aile ) / (aileTrim[2]-aileTrim[0]);
}
double InterLinkElite::getElev()
{
return stick->GetRawAxis( elev ) / (elevTrim[2]-elevTrim[0]);
}
double InterLinkElite::getThro()
{
return stick->GetRawAxis( thro ) / (throTrim[2]-throTrim[0]);
}
double InterLinkElite::getRudd()
{
return stick->GetRawAxis( rudd ) / (ruddTrim[2]-ruddTrim[0]);
}
double InterLinkElite::getCh5()
{
if( GetCh5() )
return 1;
return -1;
}
double InterLinkElite::getCh6()
{
return stick->GetRawAxis( ch6 ) / (ch6Trim[2]-ch6Trim[0]);
}
double InterLinkElite::getCh7()
{
if( GetCh7() )
return 1;
return -1;
}
double InterLinkElite::getCh8()
{
if( GetCh8Front() )
return 1;
else if( GetCh8Back() )
return -1;
return 0;
}
double InterLinkElite::getReset()
{
if( GetReset() )
return 1;
return -1;
}
bool InterLinkElite::GetReset()
{
return stick->GetRawButton( reset );
}
bool InterLinkElite::GetCh5()
{
return stick->GetRawButton( ch5 );
}
bool InterLinkElite::GetCh7()
{
return stick->GetRawButton( ch7 );
}
bool InterLinkElite::GetCh8Back()
{
return stick->GetRawButton( ch8b );
}
bool InterLinkElite::GetCh8Front()
{
return stick->GetRawButton( ch8f );
}
InterLinkElite::~InterLinkElite() {}

58
src/InterLinkElite.h Normal file
View File

@ -0,0 +1,58 @@
/*
* InterLinkElite.h
*
* Created on: May 16, 2015
* Author: user
*/
#ifndef SRC_INTERLINKELITE_H_
#define SRC_INTERLINKELITE_H_
#include "WPIlib.h"
class InterLinkElite {
public:
InterLinkElite( int port );
virtual ~InterLinkElite();
double getAile();
double getElev();
double getThro();
double getRudd();
double getCh5();
double getCh6();
double getCh7();
double getCh8();
double getReset();
bool GetReset();
bool GetCh5();
bool GetCh7();
bool GetCh8Back();
bool GetCh8Front();
// trims { min, centered, max }
double aileTrim[3] = { -0.648, 0.016, 0.654 };
double elevTrim[3] = { 0.606, 0.000, -0.641 };
double throTrim[3] = { 0.638, 0.018, -0.602 };
double ch6Trim[3] = { -1.000, 0.000, 1.000 };
double ruddTrim[3] = { -0.641, 0.024, 0.732 };
private:
Joystick *stick;
// axis mapping
int aile = 0;
int elev = 1;
int thro = 2;
int ch6 = 3;
int rudd = 4;
// button mapping
int ch5 = 1;
int ch7 = 2;
int reset = 3;
int ch8b = 4;
int ch8f = 5;
};
#endif /* SRC_INTERLINKELITE_H_ */

View File

@ -1,4 +1,7 @@
#include "WPILib.h"
#include "InterLinkElite.h"
#define PI 3.141592653589793238462643383279
///// USER PARAMETERS /////
@ -11,23 +14,21 @@
#define frontRightChannel 1
#define backRightChannel 2
#define InterLink_Elite_ID 0
// adjust sign so that (+)*speed results in bot turning clockwise
#define frontLeftSpeed 1.0
#define backLeftSpeed 1.0
#define frontRightSpeed 1.0
#define backRightSpeed 1.0
const double frontLeftAngle = PI/4;
const double backLeftAngle = 3*PI/4;
const double frontRightAngle = -PI/4;
const double backRightAngle = -3*PI/4;
#define InterLink_ID 0
///// END USER PARAMETERS /////
// InterLink Elite Channel Mapping
#define InterLink_Elite_AILE 0
#define InterLink_Elite_ELEV 1
#define InterLink_Elite_THRO 2
#define InterLink_Elite_CH6 3
#define InterLink_Elite_RUDD 4
#define InterLink_Elite_CH5 1
#define InterLink_Elite_CH7 2
#define InterLink_Elite_RESET 3
#define InterLink_Elite_CH8_BACK 4
#define InterLink_Elite_CH8_FRONT 5
class Robot: public IterativeRobot
{
LiveWindow *lw;
@ -40,21 +41,23 @@ class Robot: public IterativeRobot
Talon *backRight;
// Input Devices
Joystick *InterLink;
InterLinkElite *InterLink;
public:
Robot()
////////////////////////////////
///// ROBOT INITIALIZATION /////
////////////////////////////////
void RobotInit()
{
frontLeft = new Talon( frontLeftChannel );
backLeft = new Talon( backLeftChannel );
frontRight = new Talon( frontRightChannel );
backRight = new Talon( backRightChannel );
InterLink = new InterLinkElite( InterLink_ID );
ds = DriverStation::GetInstance();
InterLink = new Joystick( InterLink_Elite_ID );
}
void RobotInit()
{
lw = LiveWindow::GetInstance();
}
@ -77,26 +80,19 @@ public:
void TeleopPeriodic()
{
// Write Buttons to SmartDashboard
SmartDashboard::PutBoolean( "CH5", InterLink->GetRawButton( InterLink_Elite_CH5 ) );
SmartDashboard::PutBoolean( "CH7", InterLink->GetRawButton( InterLink_Elite_CH7 ) );
SmartDashboard::PutBoolean( "RESET", InterLink->GetRawButton( InterLink_Elite_RESET ) );
if( InterLink->GetRawButton( InterLink_Elite_CH8_BACK ) )
SmartDashboard::PutNumber( "CH8", -1 );
else if( InterLink->GetRawButton( InterLink_Elite_CH8_FRONT ) )
SmartDashboard::PutNumber( "CH8", 1 );
else
SmartDashboard::PutNumber( "CH8", 0 );
double aile = InterLink->getAile();
double elev = InterLink->getElev();
// Write axes to SmartDashboard
SmartDashboard::PutNumber( "AILE", InterLink->GetRawAxis( InterLink_Elite_AILE ) );
SmartDashboard::PutNumber( "ELEV", InterLink->GetRawAxis( InterLink_Elite_ELEV ) );
SmartDashboard::PutNumber( "THRO", InterLink->GetRawAxis( InterLink_Elite_THRO ) );
SmartDashboard::PutNumber( "RUDD", InterLink->GetRawAxis( InterLink_Elite_RUDD ) );
SmartDashboard::PutNumber( "CH6", InterLink->GetRawAxis( InterLink_Elite_CH6 ) );
double driveAngle = atan2( -aile, -elev );
SmartDashboard::PutNumber( "Drive Angle", driveAngle );
// Write system into to SmartDashboard
SmartDashboard::PutNumber( "BATT", DriverStation::GetInstance()->GetBatteryVoltage() );
double driveSpeed = sqrt( aile*aile + elev*elev );
SmartDashboard::PutNumber( "Drive Speed", driveSpeed );
frontLeft->Set( (float)( frontLeftSpeed * driveSpeed * sin( frontLeftAngle-driveAngle ) ) );
backLeft->Set( (float)( backLeftSpeed * driveSpeed * sin( backLeftAngle-driveAngle ) ) );
frontRight->Set( (float)( frontRightSpeed * driveSpeed * sin( frontRightAngle-driveAngle ) ) );
backRight->Set( (float)( backRightSpeed * driveSpeed * sin( backRightAngle-driveAngle ) ) );
}
////////////////