From be2130610c47b2384920d140009f48540877eba1 Mon Sep 17 00:00:00 2001 From: Brendan Date: Sat, 16 May 2015 19:39:33 -0600 Subject: [PATCH] makes rudder turn robot --- src/Robot.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/Robot.cpp b/src/Robot.cpp index 509ff70..7c2d7f8 100644 --- a/src/Robot.cpp +++ b/src/Robot.cpp @@ -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 ) ) ); }