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

corrects motor channels & adds rotation control

Browse Source
This commit is contained in:
Brendan Haines 2015-05-16 19:23:27 -06:00
parent 699fcdae6f
commit e1266a62b8
1 changed files with 15 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
src/Robot.cpp
View File

@ -9,8 +9,8 @@
#define throScale 1
// Wiring Parameters
#define frontLeftChannel 3
#define backLeftChannel 4
#define frontLeftChannel 4
#define backLeftChannel 3
#define frontRightChannel 1
#define backRightChannel 2
@ -82,6 +82,7 @@ public:
{
double aile = InterLink->getAile();
double elev = InterLink->getElev();
double rudd = InterLink->getRudd();
double driveAngle = atan2( -aile, -elev );
SmartDashboard::PutNumber( "Drive Angle", driveAngle );
@ -89,10 +90,18 @@ public:
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 ) ) );
frontLeft->Set( (float)(
throScale * frontLeftSpeed * driveSpeed * sin( frontLeftAngle-driveAngle )
) );
backLeft->Set( (float)(
throScale * backLeftSpeed * driveSpeed * sin( backLeftAngle-driveAngle )
) );
frontRight->Set( (float)(
throScale * frontRightSpeed * driveSpeed * sin( frontRightAngle-driveAngle )
) );
backRight->Set( (float)(
throScale * backRightSpeed * driveSpeed * sin( backRightAngle-driveAngle )
) );
}
////////////////