mirror of
https://github.com/brendanhaines/mecanum.git
synced 2024-12-25 10:27:12 -07:00
UNTESTEDgit status changes InterLinkElite implementation to be separate class. also adds some driving direction control stuff
This commit is contained in:
parent
7bb239c59d
commit
d4a08743ef
96
src/InterLinkElite.cpp
Normal file
96
src/InterLinkElite.cpp
Normal 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
58
src/InterLinkElite.h
Normal 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_ */
|
||||
|
|
@ -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 ) ) );
|
||||
}
|
||||
|
||||
////////////////
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user