windriver c++?

[yara92]

Hello
we are trying to write a code on windriver /C++ for mecanum wheels but tell now we haven't succeed to use the HolonomicDrive function and open declaration for it from MyRobot.cpp !!
any suggestion on how to find the declaration for the HolonomicDrive functionand how to use it?
we also tried to upload our prier code which written by EasyCPro 2008 into windriver
any suggestion or advices on how we can use our EsayCPro code in Windirver ,[quote]

 the code:
void OmniDrive ( void )
{
      int LeftFront; 
      int RightFront; 
      int LeftRear; 
      int RightRear; 
      int spin_speed = 80; 
      int gray_area = 15; 
      int Y_indicator = 0; 
      int solonoid_move_up = 0; 
      int solonoid_move_down = 0; 
      int solonoid_gray_area = 70; 
      int left_greper_motor = 0; 
      int right_greper_motor = 0; 

      InitPressureSwitch ( 7 , 2 ) ;
      // this function mixes the inputs to the motors 
      // and gives the motors PWM commands to drive 
      LeftRear = RightFront = RightRear = LeftFront =0 ;
      //PrintToScreen ( "Xaxis = %d\n" , (int)XAxis ) ;
      //PrintToScreen ( "Yaxis = %d\n" , (int)YAxis ) ;
      if ( YAxis > gray_area  )
      {
            Y_indicator = 1;
      }
      else
      {
      }
      if ( (gray_area * -1) < XAxis && XAxis < gray_area )
      {
             XAxis =0;
      }
      if ( (gray_area * -1) < YAxis && YAxis < gray_area  )
      {
            YAxis =0;
      }
      if (  XAxis ==0 || (XTrig && YAxis==0) ) // X or Y move (90 degrees)
      {
            //PrintToScreen ( "Triger=%d%d\n" , (int)XTrig ) ;
            if ( XAxis > gray_area&& XTrig  )
            {
                  degree_90_fix = - DEGREE_90_FIX;
            }
            else if ( XAxis < - gray_area&& XTrig  )
            {
                  degree_90_fix = DEGREE_90_FIX;
            }
            else
            {
                  degree_90_fix = 0;
            }
            LeftRear =YAxis -XAxis ;
            LeftFront =YAxis + XAxis + degree_90_fix  ;
            RightRear =YAxis + XAxis ;
            RightFront =YAxis  -XAxis + degree_90_fix   ;
      }
      else if ( RTrig && YAxis==0 )
      {
            LeftFront = LeftRear = XAxis ;
            RightFront = RightRear = -XAxis ;
      }
      else if ( XAxis > gray_area ) // Right Turn
      {
            LeftFront = LeftRear = Y_indicator * XAxis + YAxis ;
            RightFront = RightRear = ((127 - XAxis ) /(float) 127.0) *  
YAxis ;
            //LeftRear = LeftFront = YAxis + ( YAxis *.75  * Y_indicator )  
;
            //RightRear = RightFront = YAxis - ( YAxis *0.25 * Y_indicator  
) ;
            PrintToScreen("Right Turn: XaXIS =%d ,  Yaxis=%d, leftrear=% 
d,leftfront=%d,rightrear=%d,rightfront=%d\n-----------Percent= %f---- 
\n",XAxis ,YAxis , LeftRear , LeftFront , RightRear , RightFront , (float)  
RightFront/(float)LeftFront );
      }
      else if ( XAxis < (-1 * gray_area) ) // Left Turn
      {
            RightFront = RightRear = -XAxis* Y_indicator  + YAxis ;
            LeftRear = LeftFront = ((127 + XAxis ) /(float) 127.0) * YAxis  
;
            //LeftRear = LeftFront = YAxis - ( YAxis *0.25 * Y_indicator )  
;
            //RightRear = RightFront = YAxis + ( YAxis *0.75 * Y_indicator  
) ;
            //PrintToScreen("Left Turn:   Xaxis = %d, Yaxis=%d,leftrear=% 
d,leftfront=%d,rightrear=%d,rightfront=%d\n----------------turn percent =  
%f-----------\n",XAxis ,YAxis , LeftRear , LeftFront , RightRear ,  
RightFront ,(float) LeftRear/(float)RightRear);
      }
      // The following nominalizes the values for the speed 
      if ( LeftFront < MAX_SPEED_B  )
      {
            LeftFront = MAX_SPEED_B  ;
      }
      if ( LeftFront > MAX_SPEED_F  )
      {
            LeftFront = MAX_SPEED_F  ;
      }
      if ( RightFront < MAX_SPEED_B  )
      {
            RightFront = MAX_SPEED_B  ;
      }
      if ( RightFront > MAX_SPEED_F  )
      {
            RightFront = MAX_SPEED_F  ;
      }
      if ( RightRear < MAX_SPEED_B  )
      {
            RightRear = MAX_SPEED_B  ;
      }
      if ( RightRear > MAX_SPEED_F  )
      {
            RightRear = MAX_SPEED_F  ;
      }
      if ( LeftRear < MAX_SPEED_B  )
      {
            LeftRear = MAX_SPEED_B  ;
      }
      if ( LeftRear > MAX_SPEED_F  )
      {
            LeftRear = MAX_SPEED_F  ;
      }
      // Now we add 6 back to the speed 
      // The speed values are stored in an array from 0 - 13 
      // with 7 as neutral 
      // We used 0 as neutral before to allow for seemless addition and  
subtraction 
      // Set the motors and add back 127 for 0-255 
      // If you need to invert a motor replace with"255-(LeftFront+127)" 
      SetPWM ( Left_Rear_PWM , (LeftRear+127) ) ; // Output to Motor
      SetPWM ( Left_Front_PWM , LeftFront+127 ) ; // Output to Motor
      SetPWM ( Right_Rear_PWM , (255-RightRear+127) ) ; // Output to Motor
      SetPWM ( Rigth_Front_PWM , (255-RightFront+127) ) ; // Output to  
Motor
      if ( solonoid_Yaxis > solonoid_gray_area  )
      {
            solonoid_move_up = 1 ;
            solonoid_move_down = -1 ;
      }
      else if (  solonoid_Yaxis < (-1) *  solonoid_gray_area )
      {
            solonoid_move_up= -1 ;
            solonoid_move_down = 1 ;
      }
      else
      {
            solonoid_move_up = 0 ;
            solonoid_move_down = 0 ;
      }
      SetRelay ( 1 , solonoid_move_up , solonoid_move_down ) ;
      if ( Solonoid_push  )
      {
            SetRelay ( 3 , 1 , 0 ) ;
            Wait ( 400 ) ;
            SetRelay ( 3 , 0 , 1 ) ;
            Wait ( 400 ) ;
            SetRelay ( 3 , 0 , 0 ) ;
      }
      if ( greper_inital_open_trig && inital_done  )
      {
            inital_done = 0 ;
            left_greper_motor = 1 ;
            right_greper_motor  = 1 ;
      }
      else if ( greper_open_trig  )
      {
            printf("----------in OPEN trig-----------\n");
            left_greper_motor = 70 ;
            right_greper_motor  = 70 ;
      }
      else if ( grepre_close_trig  )
      {
            printf("----------in CLOSE trig-----------\n");
            left_greper_motor = 200 ;
            right_greper_motor  = 200 ;
      }
      else
      {
            printf("----------in ELSE trig-----------\n");
            left_greper_motor = 127 ;
            right_greper_motor  = 127 ;
      }
      SetPWM ( LEFT_GREPER_PWM , left_greper_motor ) ; // Output to Motor
      SetPWM ( RIGHT_GREPER_PWM , right_greper_motor ) ; // Output to  
Motor
}

Team 1946

[EHaskins]

Your EasyC code is not compatible with the new system, and will have to be rewritten.

I have not had good luck provided holonomic drive function, luckly that is not a difficult function to write.

Following is the code for a Mecanum/Holonomic drive. You need to insert the code piecse into the places noted in a blank copy of the Iterative robot demo project.

//INSERT IN: Variable definitions 
jaguar NEMotor;
jaguar SEMotor;
jaguar NWMotor;
jaguar SWMotor;


//INSERT IN: Constructor
//You can change the port numbers if needed to match your robot.
NWMotor = new Jaguar(1);
NEMotor = new Jaguar(2);
SWMotor = new Jaguar(3);
SEMotor = new Jaguar(4);


//INSERT IN: TeleopPeriodic or main loop
float X = m_leftStick->GetZ();
float Y = m_leftStick->GetY();
float Z = m_leftStick->GetX();
			
Y *= -1;
X *= -1;
float NW = Y + X + Z;
float NE = Y - X - Z;
float SW = Y - X + Z;
float SE = Y + X - Z;
			
NE *= -1;
NWMotor->Set(NW);
NEMotor->Set(NE);
SWMotor->Set(SW);
SEMotor->Set(SE);

Let me know if you have issues.

[yara92]

hello Eric

Thank you for your help we were able to insert the code you sent us into RobotDemo however we are not sure if the mecanum wheels are fully doing all the right moves (it works fine in all direction except to the sides) we think that it could be a problem with not having a gray area specified ?!!!
Let us know what you think!!
the code,

#include "WPILib.h"

/**
 * This is a demo program showing the use of the RobotBase class.
 * The SimpleRobot class is the base of a robot application that will automatically call your
 * Autonomous and OperatorControl methods at the right time as controlled by the switches on
 * the driver station or the field controls.
 */ 
class RobotDemo : public SimpleRobot
{
	RobotDrive myRobot; // robot drive system
	Joystick stick; // only joystick 
	{
	public:
		typedef enum
		{
			kFrontLeftMotor = 0,
			kFrontRightMotor = 1,
			kRearLeftMotor = 2,
			kRearRightMotor = 3
		} MotorType;
public:
	RobotDemo(void):
		myRobot(1, 2, 3, 4),	// these must be initialized in the same order
		stick(1)		// as they are declared above.


	{
		GetWatchdog().SetExpiration(0.1);
	}

	/**
	 * Drive left & right motors for 2 seconds then stop
	 */
	void Autonomous(void)
	{
		GetWatchdog().SetEnabled(false);
		myRobot.Drive(0.5, 0.0); 	// drive forwards half speed
		Wait(2.0); 				//    for 2 seconds
		myRobot.Drive(0.0, 0.0); 	// stop robot
	}

	/**
	 * Runs the motors with arcade steering. 
	 */
	void OperatorControl(void)
	{
		GetWatchdog().SetEnabled(true);
		while (IsOperatorControl())
		{
			GetWatchdog().Feed();
			myRobot.ArcadeDrive(stick); // drive with arcade style (use right stick)
			Wait(0.005);				// wait for a motor update time
		}
		//INSERT IN: TeleopPeriodic or main loop
		float X = m_leftStick->GetZ();
		float Y = m_leftStick->GetY();
		float Z = m_leftStick->GetX();
					
		Y *= -1;
		X *= -1;
		float FrontLeft = Y + X + Z;
		float FrontRight = Y - X - Z;
		float RearLeft = Y - X + Z;
		float RearRight = Y + X - Z;
					
		FrontRight *= -1;
		KFrontLeftMotor->Set(FrontLeft);
		KFrontRightMotor->Set(FrontRight);
		KRearLeftMotor->Set(RearLeft);
		KRearRigtMotor->Set(RearRigt);
	}
};

START_ROBOT_CLASS(RobotDemo);

ps open the attachment
Thank you again
from
1946 programming team

[EHaskins]

Originally Posted by yara92

hello Eric

Thank you for your help we were able to insert the code you sent us into RobotDemo however we are not sure if the mecanum wheels are fully doing all the right moves (it works fine in all direction except to the sides) we think that it could be a problem with not having a gray area specified ?!!!
Let us know what you think!!
the code,

#include "WPILib.h"

/**
 * This is a demo program showing the use of the RobotBase class.
 * The SimpleRobot class is the base of a robot application that will automatically call your
 * Autonomous and OperatorControl methods at the right time as controlled by the switches on
 * the driver station or the field controls.
 */ 
class RobotDemo : public SimpleRobot
{
	RobotDrive myRobot; // robot drive system
	Joystick stick; // only joystick 
	{
	public:
		typedef enum
		{
			kFrontLeftMotor = 0,
			kFrontRightMotor = 1,
			kRearLeftMotor = 2,
			kRearRightMotor = 3
		} MotorType;
public:
	RobotDemo(void):
		myRobot(1, 2, 3, 4),	// these must be initialized in the same order
		stick(1)		// as they are declared above.


	{
		GetWatchdog().SetExpiration(0.1);
	}

	/**
	 * Drive left & right motors for 2 seconds then stop
	 */
	void Autonomous(void)
	{
		GetWatchdog().SetEnabled(false);
		myRobot.Drive(0.5, 0.0); 	// drive forwards half speed
		Wait(2.0); 				//    for 2 seconds
		myRobot.Drive(0.0, 0.0); 	// stop robot
	}

	/**
	 * Runs the motors with arcade steering. 
	 */
	void OperatorControl(void)
	{
		GetWatchdog().SetEnabled(true);
		while (IsOperatorControl())
		{
			GetWatchdog().Feed();
			myRobot.ArcadeDrive(stick); // drive with arcade style (use right stick)
			Wait(0.005);				// wait for a motor update time
		}
		//INSERT IN: TeleopPeriodic or main loop
		float slide = m_leftStick->GetZ(); // was X
		float forward = m_leftStick->GetY(); // was Y
		float spin = m_leftStick->GetX(); // was Z
					
		forward *= -1;
		slide *= -1;
		float FrontLeft = forward + slide + spin;
		float FrontRight = forward - slide - spin;
		float RearLeft = forward - slide + spin;
		float RearRight = forward + slide - spin;
					
		FrontRight *= -1;
		KFrontLeftMotor->Set(FrontLeft);
		KFrontRightMotor->Set(FrontRight);
		KRearLeftMotor->Set(RearLeft);
		KRearRigtMotor->Set(RearRigt);
	}
};

START_ROBOT_CLASS(RobotDemo);

ps open the attachment
Thank you again
from
1946 programming team

When you say it doesn't work sideways what do you mean? Does it do nothing? Does it spin/go forward? Something else?

If you're not sure replace the joystick mapping code with the following, then support the robot with its wheels in the air, and report how each wheel is moving.

Then move the constant to each of the other values in turn, and post the result. This will confirm motor mappings, and wiring to the motors.

	float X = 0.2;
	float Y = 0.0;
	float Z= 0.0;

In any case, see the code above (changes bold) for cliarification of the variable names. Also those joystick mappings are for Logictech PS2 controller clone. If you are using the KOP joysticks or other traditional joysticks you will need to modify the mappings.

I assure you no additional code is necessary for a basic mecanum drive. I've used that piece of code many times.

[BradAMiller]

The RobotDrive class has a HolonomicDrive method that does the calculations for you based on a 4 motor drive base. You supply a magnitude and direction of travel (that's speed, direction), and a rotation value. To use it, you have to create a RobotDrive object with the 4 motors.

Check out the documenation to determine what order to list the motors.