brendanhaines/mecanum
1
0
Fork 0
mirror of https://github.com/brendanhaines/mecanum.git synced 2024-11-09 21:14:51 -07: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 // Tuning Parameters
#define throScale 1 #define throScale 1
#define ruddScale 1
// Wiring Parameters // Wiring Parameters
#define frontLeftChannel 4 #define frontLeftChannel 4
@ -83,6 +84,7 @@ public:
double aile = InterLink->getAile(); double aile = InterLink->getAile();
double elev = InterLink->getElev(); double elev = InterLink->getElev();
double rudd = InterLink->getRudd(); double rudd = InterLink->getRudd();
SmartDashboard::PutNumber( "Rudder", rudd );
double driveAngle = atan2( -aile, elev ); double driveAngle = atan2( -aile, elev );
SmartDashboard::PutNumber( "Drive Angle", driveAngle ); SmartDashboard::PutNumber( "Drive Angle", driveAngle );
@ -91,16 +93,16 @@ public:
SmartDashboard::PutNumber( "Drive Speed", driveSpeed ); SmartDashboard::PutNumber( "Drive Speed", driveSpeed );
frontLeft->Set( (float)( frontLeft->Set( (float)(
throScale * frontLeftSpeed * driveSpeed * sin( frontLeftAngle-driveAngle ) throScale * frontLeftSpeed * ( driveSpeed * sin( frontLeftAngle-driveAngle ) + ruddScale * rudd )
) ); ) );
backLeft->Set( (float)( backLeft->Set( (float)(
throScale * backLeftSpeed * driveSpeed * sin( backLeftAngle-driveAngle ) throScale * backLeftSpeed * ( driveSpeed * sin( backLeftAngle-driveAngle ) + ruddScale * rudd )
) ); ) );
frontRight->Set( (float)( frontRight->Set( (float)(
throScale * frontRightSpeed * driveSpeed * sin( frontRightAngle-driveAngle ) throScale * frontRightSpeed * ( driveSpeed * sin( frontRightAngle-driveAngle ) + ruddScale * rudd )
) ); ) );
backRight->Set( (float)( backRight->Set( (float)(
throScale * backRightSpeed * driveSpeed * sin( backRightAngle-driveAngle ) throScale * backRightSpeed * ( driveSpeed * sin( backRightAngle-driveAngle ) + ruddScale * rudd )
) ); ) );
} }