brendanhaines/mecanum
1
0
Fork 0
mirror of https://github.com/brendanhaines/mecanum.git synced 2024-12-26 10:58:00 -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 #define throScale 1
// Wiring Parameters // Wiring Parameters
#define frontLeftChannel 3 #define frontLeftChannel 4
#define backLeftChannel 4 #define backLeftChannel 3
#define frontRightChannel 1 #define frontRightChannel 1
#define backRightChannel 2 #define backRightChannel 2
@ -82,6 +82,7 @@ public:
{ {
double aile = InterLink->getAile(); double aile = InterLink->getAile();
double elev = InterLink->getElev(); double elev = InterLink->getElev();
double rudd = InterLink->getRudd();
double driveAngle = atan2( -aile, -elev ); double driveAngle = atan2( -aile, -elev );
SmartDashboard::PutNumber( "Drive Angle", driveAngle ); SmartDashboard::PutNumber( "Drive Angle", driveAngle );
@ -89,10 +90,18 @@ public:
double driveSpeed = sqrt( aile*aile + elev*elev ); double driveSpeed = sqrt( aile*aile + elev*elev );
SmartDashboard::PutNumber( "Drive Speed", driveSpeed ); SmartDashboard::PutNumber( "Drive Speed", driveSpeed );
frontLeft->Set( (float)( frontLeftSpeed * driveSpeed * sin( frontLeftAngle-driveAngle ) ) ); frontLeft->Set( (float)(
backLeft->Set( (float)( backLeftSpeed * driveSpeed * sin( backLeftAngle-driveAngle ) ) ); throScale * frontLeftSpeed * driveSpeed * sin( frontLeftAngle-driveAngle )
frontRight->Set( (float)( frontRightSpeed * driveSpeed * sin( frontRightAngle-driveAngle ) ) ); ) );
backRight->Set( (float)( backRightSpeed * driveSpeed * sin( backRightAngle-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 )
) );
} }
//////////////// ////////////////