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.

[Chexposito]

Originally Posted by JamesTerm

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

not to mention you are not using the resources given to you in the wpi library...
which makes what you did into significantly less code for someone new to programming...
it also makes it a lot easier...
and less time consuming...
which gives you time for the programming autonomous, which has factor for winning matches in FIRST for 4 of the past 5 years...

[Michael Blake]

Originally Posted by Chexposito

which gives you time for the programming autonomous, which has factor for winning matches in FIRST for 4 of the past 5 years...

Chexposito, could you expand on this? Help make us smarter about this statement?

THANKS for your help!

[Chexposito]

Originally Posted by Michael Blake

Chexposito, could you expand on this? Help make us smarter about this statement?

THANKS for your help!

For example, last year's autonomous included scoring the ubertubes. the points for scoring the ubertubes was enough of an incentive, but the 2x bonus was another incentive. note all teams on Einstein had an autonomous.

In Breakaway, autonomous was not as key, but was useful.

In lunacy, autonomous' purpose was to get away from they human players. if you didn't drive away, you could count on the human player filling your trailer to the top.

In overdrive, you got points for knocking the off the trackballs and driving around the field.

In rack 'n roll the keepers were scored only during autonomous (similar to ubertube) and over-rode any scoring on that peg at any point in the match (always red/blue)

based on the importance of autonomous this previous season, i don't this first will ease up on the scoring involved with this portion. the autonomous usually could make or break matches. and usually the sensors that make autonomous easier usually make driving the robot easier.

[JamesTerm]

Originally Posted by Chexposito

not to mention you are not using the resources given to you in the wpi library...
which makes what you did into significantly less code for someone new to programming...
it also makes it a lot easier...

What resources are you referring to in particular?
Let us see the significant less code you mention... I've showed my cards... let's see your hand.

We may all learn something from this.

[Chexposito]

i am referring to the wpi library that is given to all frc teams. it includes code for the joysticks, sensors, and so on. pretty much if you put a sensor on your robot, there is code made and debugged for it. which pretty much leaves you to only have to deal with the code in myrobot.cpp and myrobot.h

[JamesTerm]

Originally Posted by Chexposito

i am referring to the wpi library that is given to all frc teams. it includes code for the joysticks, sensors, and so on. pretty much if you put a sensor on your robot, there is code made and debugged for it. which pretty much leaves you to only have to deal with the code in myrobot.cpp and myrobot.h

Ok, I just wanted to make sure that there wasn't something that I missed. Please look at my code a bit more closely... it is indeed using the WPI library. In fact I have looked through the wpi source... submitted bugs, and am contributing to 2012 wpi libs on the beta team. Whew, there for a moment I thought I missed something. Yes my code was not made to present to a new person, but I wanted to get something out here that is thorough. Plus, it would be good for someone to challenge it to make it better.

[Chexposito]

i'm just saying your time would be better used during the season using the library... especially if you want students to learn to program. you'll loose all your new people pretty quickly without the use of the library. off season is fine, but if you don't make programming fun for the new people, they won't stay interested when you get to the headache moments. the best thing is to have them just program the myrobot files and look at the wpi library to try to understand how the function works, if they're interested in how the functions. but writing that stuff during the season will leave you with no time and an arm that moves inefficiently by dead reckoning. now as a mentor you can write this stuff yourself in your free time or working on another computer while they're working on the myrobot files. we created our own pid this past season but that was my mentor's doing, this was to make our pid work better for our uses. just some suggestions... this is after all a program for learning and mentoring... you might be surprised the functions some of these students come up with

[JamesTerm]

Originally Posted by Chexposito

we created our own pid this past season but that was my mentor's doing, this was to make our pid work better for our uses. just some suggestions... this is after all a program for learning and mentoring... you might be surprised the functions some of these students come up with

It is funny that you should bring up PID, this was one of the things I've been wanting to address as well. Check these out:

http://www.termstech.com/articles/Autonomous2.html

http://www.termstech.com/articles/PID_Kalman.html


One thing I regret is that I didn't spend more time writing an article to answer this question that goes more into some of the coding principles like top-down design bottom up implementation, and show a high level design of how the pieces fit together. I still may do this in the future, but there is so much other work I need to tend to (all within the robotics realm).