brendanhaines/mecanum
1
0
Fork 0
mirror of https://github.com/brendanhaines/mecanum.git synced 2024-09-19 12:25:57 -06:00
Code Issues Packages Projects Releases Wiki Activity

makes rudder turn robot

Browse Source
This commit is contained in:
Brendan Haines 2015-05-16 19:39:33 -06:00
parent 37719b4889
commit be2130610c
1 changed files with 6 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/Robot.cpp
View File

@ -7,6 +7,7 @@
// Tuning Parameters
#define throScale 1
#define ruddScale 1
// Wiring Parameters
#define frontLeftChannel 4
@ -83,6 +84,7 @@ public:
double aile = InterLink->getAile();
double elev = InterLink->getElev();
double rudd = InterLink->getRudd();
SmartDashboard::PutNumber( "Rudder", rudd );
double driveAngle = atan2( -aile, elev );
SmartDashboard::PutNumber( "Drive Angle", driveAngle );
@ -91,16 +93,16 @@ public:
SmartDashboard::PutNumber( "Drive Speed", driveSpeed );
frontLeft->Set( (float)(
throScale * frontLeftSpeed * driveSpeed * sin( frontLeftAngle-driveAngle )
throScale * frontLeftSpeed * ( driveSpeed * sin( frontLeftAngle-driveAngle ) + ruddScale * rudd )
) );
backLeft->Set( (float)(
throScale * backLeftSpeed * driveSpeed * sin( backLeftAngle-driveAngle )
throScale * backLeftSpeed * ( driveSpeed * sin( backLeftAngle-driveAngle ) + ruddScale * rudd )
) );
frontRight->Set( (float)(
throScale * frontRightSpeed * driveSpeed * sin( frontRightAngle-driveAngle )
throScale * frontRightSpeed * ( driveSpeed * sin( frontRightAngle-driveAngle ) + ruddScale * rudd )
) );
backRight->Set( (float)(
throScale * backRightSpeed * driveSpeed * sin( backRightAngle-driveAngle )
throScale * backRightSpeed * ( driveSpeed * sin( backRightAngle-driveAngle ) + ruddScale * rudd )
) );
}