How to set up a 3rd (non drive) motor in C++ Windriver

[JamesTerm]

Originally Posted by Stonemotmot

I'm relatively new to programming in C++ and i have a question regarding how to set up a third motor for say arm control. I am using the default Simple Robot Template in windriver. I already understand how to set up tank and arcade drive but as this 3rd motor will only be used for arm control and not driving I'm hesitant to reuse those commands. The biggest problem I am having right now is figuring out how to take a joystick input such as a get-Axis or something like that and send the information to a motor or speed controller. Thanks in advance.

bool Driver_Station_Joystick::read_joystick (size_t nr, JoyState &Info)
{
//First weed out numbers not in range
int Number=(int)nr;
Number-=m_StartingPort;
bool ret=false;
nr++; //DOH the number selection is cardinal!
if ((Number>=0) && (Number<m_NoJoysticks))
{
memset(&Info,0,sizeof(JoyState)); //zero the memory
//The axis selection is also ordinal
Info.lX=m_ds->GetStickAxis(nr,1);
Info.lY=m_ds->GetStickAxis(nr,2);
Info.lZ=m_ds->GetStickAxis(nr,3);
Info.lRx=m_ds->GetStickAxis(nr,4);
Info.lRy=m_ds->GetStickAxis(nr,5);
Info.ButtonBank[0]=m_ds->GetStickButtons(nr);
ret=true;
}
return ret;
}

This is the code I use to obtain the Joystick values... The axis are x, y, z position followed by x, y, z rotation (these enumerations match Microsoft's DirectInput sdk), and my info structure is the same as direct input.

Once you get these values you can manipulate them... the range is -1 - 1.0 on the axis ... here are some of my axis params

IsFlipped - Apply a -1 scalar
Multiplier - Apply a scalar
FilterRange - apply a deadzone range
isSquared - square the input (this is used more times than not)



Here is what I do:

if (AnalogEvents)
{
//Now to use the attributes to tweak the value
//First evaluate dead zone range... if out of range subtract out the offset for no loss in precision
//The /(1.0-filter range) will restore the full range

double Temp=fabs(Value); //take out the sign... put it back in the end
Temp=(Temp>=key.FilterRange) ? Temp-key.FilterRange:0.0;

Temp=key.Multiplier*(Temp/(1.0-key.FilterRange)); //apply scale first then
if (key.isSquared) Temp*=Temp; //square it if it is squared

//Now to restore the sign
Value=(Value<0.0)?-Temp:Temp;

std::vector<std::string>::iterator pos;
for (pos = AnalogEvents->begin(); pos != AnalogEvents->end(); ++pos)
m_controlledEventMap->EventValue_Map[*pos].Fire(key.IsFlipped?-Value:Value);
}


You may want to ignore the event firing lines except to say that the event value map will do a map (i.e. log-n) search for the events associated with an input and fire them... I do this because it allows me to easily switch axis assignments etc. Ideally the code is more robust if you do not hard wire the controls directly to object that listens for event changes.


Now then in the main I do the following:

void OperatorControl(void)
{
printf("Starting TeleOp Session\n");
m_Manager.ResetPos(); //This should avoid errors like the arm swinging backwards
m_Manager.GetRobot()->SetUseEncoders(false);
m_Manager.SetAutoPilot(false); //we are driving the robot
double tm = GetTime();
m_Manager.SetSafety(true);
while (IsOperatorControl() && !IsDisabled())
{
double time=GetTime() - tm;
tm=GetTime();
m_Manager.TimeChange(time);
Wait(0.010);
}
}


I make one call to a TimeChange(time) which then dispatches to poll the joystick and fire off the events. The robot class (the class that processes the events) will then call:


void Robot_Control_2011::UpdateVoltage(size_t index,double Voltage)
{
switch (index)
{
case FRC_2011_Robot::eArm:
{
//Note: client code needs to check the levels are correct!
m_ArmMotor.Set(Voltage); //always the same velocity for both!
#ifdef __ShowPotentiometerReadings__
DriverStationLCD * lcd = DriverStationLCD::GetInstance();
lcd->PrintfLine(DriverStationLCD::kUser_Line4, "ArmVolt=%f ", Voltage);
#endif
}
break;
case FRC_2011_Robot::eRollers:
m_RollerMotor.Set(Voltage);
#ifdef __ShowRollerReadings__
DriverStationLCD * lcd = DriverStationLCD::GetInstance();
lcd->PrintfLine(DriverStationLCD::kUser_Line4, "RollerVolt=%f ", Voltage);
#endif
break;
}
}


Where these are defined as such:

Victor m_1,m_2,m_3,m_4; //explicitly specify victor speed controllers for the robot drive
RobotDrive m_RobotDrive;
Victor m_ArmMotor,m_RollerMotor;


We use victor speed controllers! You'll need to know what you use to get the best reading derivative when the floating point gets translated to integer values.

On a side note: the robot drive we use the constructor that allows you to explicity specify what controllers you want to use since the default case assumes you have Jaguar speed controllers.

Here is a snip of how to do that:
Robot_Control::Robot_Control(bool UseSafety) :
m_1(1),m_2(2),m_3(3),m_4(4),
m_RobotDrive(&m_1,&m_2,&m_3,&m_4),
//m_RobotDrive(1,2,3,4), //default Jaguar instantiation
m_ArmMotor(5),m_RollerMotor(6),m_Compress(5,2),

Note: that for the arm Victor, the contructor use shown here assumes the digital side car is hooked up to slot 4 in the cRio. The first parameter shown indicates which channel on the digital side car that you are using.


I realize this presentation could be over-whelming, so feel free to ask more questions.