paper: FRC #33 The Killer Bees 2013 Software - BuzzXVIII (Buzz18)

[apalrd]

Thread created automatically to discuss a document in CD-Media.

FRC #33 The Killer Bees 2013 Software - BuzzXVIII (Buzz18) by apalrd

[apalrd]

As promised, a complete release of Buzz18 software.

I am quite busy with school at this time, but I should finish the promised papers within the next week.

For now, the software zip is posted in CD Media.

[z_beeblebrox]

Not having LabVIEW, I can't look at the code. However, a while ago, you mentioned that you used something other than your normal "Cheesy Drive" software for drivetrain control. What did you do?

[apalrd]

Originally Posted by z_beeblebrox

Not having LabVIEW, I can't look at the code. However, a while ago, you mentioned that you used something other than your normal "Cheesy Drive" software for drivetrain control. What did you do?

That would be the 'CulverDrive', which we have a paper and example C code in-progress for.

The basic idea is to use the angle of the steering stick instead of the X axis of the steering stick as the primary steering input, when combined with a joystick with circular boundaries (such as the Logitech F310) this allows you to 'steer' the joystick around the boundary of the stick with finer steering resolution.

[apalrd]

Added new files:
buzz18_pal_lib.pdf
pal_lib_default.zip (LV example code, pal_lib code, and paper pdf)

A little bit about pal_lib:
-'pal_lib' WPIlib replacement for LabVIEW
-Replaces ENTIRE WPIlib LabVIEW with 30 VIs, contained in zip folder and inserted as a subfolder into your project.
-Heavily optimized architecture promotes execution efficienty, runtime performance determinism, and good embedded programming practices. Increases in execution efficiency are primarily done by reducing code size and removing unnecessary code, which significantly decreases compile and download times as an added benefit.
-FAST execution (all code currently executes at 10ms, for small or heavily optimized programs 5ms is possible). We could have run Buzz18 at 7ms iteration time, but ran at 10ms with ~55% CPU usage to leave overhead for LV probes and debugging.
-Supports basic commonly-used inputs, outputs, and control data. Large efficiency and code space gains are made by removing functionality that is not commonly used.
-Season-tested on Buzz18 (including five competitions and Einstein) at 10ms iteration time without any bugs EVER attributed to the library after initial library validation
-Extremely low CPU usage (initial validation tests provided initial functionality to default tank drive program with 1/2 CPU usage % at over 2x execution speed), library CPU usage does not increase with used functionality as the library is already handling all of the data it is capable of (e.g. adding another motor or encoder does not require any expensive VI calls and CPU usage of scaling function is inlined and negligible).
-zip contains example 'default' program which maps several joysticks, buttons, and limit switches to motors, solenoids, and relays, and operates the compressor. See comments in the code for exact mapping and IO pinouts. Entire example code is 4 VI's in addition to pal_lib
-Fully synchronous architecture forces efficient data-passing design, as all data can be passed via connectors. Use of global-visible data structures in Buzz18 further optimizes data-passing as all data is always available to all subsystems, any subsystem can change data without consequence to anything else, and order of operations is guaranteed (simplifies triggers significantly). Data structure architecture also simplifies dashboard programming, as all of the data structures are simply sent to the dashboard using the binary string method ('old-style', not NetworkTables) and the dashboard automatically has access to all key data.
-See buzz18_pal_lib.pdf for complete write-up
-Also see buzz18_sw_postcmp for complete robot usage of library

[apalrd]

OK, finished the 'CulverDrive' paper and attached it with example code.

What is CulverDrive?
-Improvement on previous 'Cheesy' and 'Halo' style drive systems, which are already used by many top teams
-Utilizes Logitech F310's round joystick boundary to provide a virtual steering wheel effect, allowing the driver to gracefully navigate the field
-Utilizes a mathematical algorithm similar to the 'Cheesy Drive', the intent in design was to replicate the functionality of a 'Cheesy Drive' using a real steering wheel on a joystick.
-Award-Winning (Waterford 'Innovation In controls' and Championship 'Quality' specifically noted CulverDrive in judge comments)

What does the zip include?
-LabVIEW source code, direct from Buzz18 software release, plus required BuzzLib interpolation blocks (prelookup and interp_2d)
-C source code, used in simulation and intended for Vex targets and readable without LabVIEW. Unfortunately it does not directly run on Vex because RobotC does support the C language very well.

Does it require a Logitech F310?
-In short, yes. The Logitech F310 has a circular joystick boundary, which is key to the algorithm. It's also smoother to drive than a square of octagon.
-The Xbox 360 joystick has an octagonal boundary, which will not provide the desired effect nearly as cleanly. The Logitech DualAction has a square boundary, which will not work very well at all.

As always, feel free to ask questions regarding any of our software.

[Iaquinto.Joe]

How do you open .lvsc files? Is it a tool included with Labview for FRC?

[apalrd]

LVSC file are LabVIEW Statecharts.

The module is not included in the FRC release of LabVIEW. We arranged this with NI in return for feedback on it's use.

Since I didn't expect anyone to have Statchart, I generated documentation for all Statecharts using the internal documentation tool. Every statechart has a folder called <name>_files which contains a PNG image of the following:
-The statechart itself (Main Diagram0.png)
-The Inputs and Outputs clusters (Inputs0.png and Outputs0.png) - I didn't use the StateData but there is a cluster for that too
-The outer appearance of each block (I tried to name them reasonably, that should be in these images - #.png)
-The entry/exit action of the block (for states - entryaction#.png and exitaction#.png
-The guard action of the block (for transitions - guard#.png)

Statechart was a great tool to work with once we learned all the quirks (e.g. when you copy-paste a block, they share the same diagram and modifying one modifies the other). It definitely helped some of the more complex state-machines we had (the first and second revision gun feeder system was very complicated). It was also helpful when debugging - I could tell the mechanical and integration people working on that system exactly what it was trying to do and how it should react.

I also made some VI's to graph the input/output data which was very helpful in analyzing the strange behavior (if I do this and this and this, it jams with gate A open and gate B closed but the gun isn't on...)... But even with the automation there were still corner cases that weren't handled perfectly. But the bucket (and really simple code) had virtually none of them.

[JamesTerm]

Originally Posted by apalrd

That would be the 'CulverDrive', which we have a paper and example C code in-progress for.

The basic idea is to use the angle of the steering stick instead of the X axis of the steering stick as the primary steering input, when combined with a joystick with circular boundaries (such as the Logitech F310) this allows you to 'steer' the joystick around the boundary of the stick with finer steering resolution.

That is brilliant! I am looking forward to reviewing this C example code!

[apalrd]

Originally Posted by JamesTerm

That is brilliant! I am looking forward to reviewing this C example code!

Example C code is in the zip (culverdrive_sw_release.zip). It isn't great, and isn't tested like the LV implementation, but should illustrate how the logic works.

interp.c and interp.h are C implementations of the LV interpolation functions. I tested these on a desktop GCC build/test harness and they work. We use these in the Culverdrive to define smooth enabling of different terms, and use them elsewhere for nonlinear functions. They're very useful functions. I can explain their use in more detail if anyone is interested.

culverdrive.h is the header, self explanetory.

The logic itself is written and commented in culverdrive.c in the culver_drive() function. The const data is defined in culverdrivedata.c.

There are some things that are a little weird about the C code. I intended to run it on Vex targets, but it won't compile in RobotC because RobotC does not support the C language very well.

[Iaquinto.Joe]

What would you recommend if a team wanted to implement a Finite State Machine approach to their programming? Should we try to talk with NI about the statechart plugin, or implement state machines on our own?

[JamesTerm]

Originally Posted by Iaquinto.Joe

What would you recommend if a team wanted to implement a Finite State Machine approach to their programming? Should we try to talk with NI about the statechart plugin, or implement state machines on our own?

Can you give an example of how you would like to use a finite state machine?

[Iaquinto.Joe]

Originally Posted by JamesTerm

Can you give an example of how you would like to use a finite state machine?

We drew out each subsystem's state machine so we would use a state machine for our shooter, our drivetrain, our pickup, etc. Having each of the systems diagrammed out before we program could help us debug and improve our code. We would have several independent state machines running in parallel (E.G. The drivetrain FSM runs in parallel to the shooter FSM).

[apalrd]

We use a lot of finite state machines for things that we sequence. We also use other control strategies for other things, including feedback controls, and lookup tables.

Our software modules are very large. They usually line up with the physical subsystem teams, although I threw the gun in with Roller because the integration between them was very critical and I wanted them to be designed together (there were 4 mechanical teams and 3 software modules).

We then design up a high-level design of the software (what each module is responsible for, what sensors and actuators it has command of, and what the interactions with other modules including HMI/Auton are). From that, we design each module.

The modules are split up into blocks, usually based on control of specific actuators or related groups of actuators. For example, drivetrain is split between drive motors and shift. Drive motors handles the drive motors, and shift handles the shifting. Drive motors is a passthrough function (control is unique to HMI/Auton, this is the only feature that is not implemented in it's module) and shifting is a lookup table. With servos in 2012, shifting was a finite-state machine that would peak and hold the servos for better shift quality and servo reliability (we melted servo or two before doing that).

So it dosen't make sense to design everything as an FSM. We really use a lot of small FSMs when we need to sequence actions at a high level (like the gun feed state machine or 2011 arm state machine), or deal with intermediate states at a medium to low level (like the peak-and-hold state machine).

All of the code runs in parallel (it all shares a single task, the software design strictly prevents blocking execution). The execution order is specified by how data is used by other modules.

I am not sure what NI's intentions are with the Statechart module.

[JamesTerm]

Originally Posted by apalrd

Example C code is in the zip (culverdrive_sw_release.zip). It isn't great, and isn't tested like the LV implementation, but should illustrate how the logic works.

I have taken the example into one file all in this reply, and made modification to something that can be tested in visual studio (I use 2008). I've made a second version of the culver drive which should be identical functionality from my tests. It does not call interp_2d() and keeps units in radians using atan2() and keeps the sign. There are some subtle bug fixes mentioned within this code too. The testing mechanism simply iterates from -pi to pi incrementing to an arbitrary number of iterations... from this I put macrod out tests like half magnitude or no Y component to see how the numbers turn out. From what I can tell... it linear interpolates between 90-115 degrees... from 1.0 - 0.0... for the filtering mechanism. I wasn't sure if the wheel drive was suppose to go past -90 - 90... but the tables suggest that it should... Under this assumption it appears if the driver accidentally hits slightly down.. the sharper angles will get filtered out... That is very clever how the magnitude of the angle comes into play with the final result.

#include "stdafx.h"
#include <math.h>

#undef __AlternateRawImplementation__

inline double sgn(double x)
{
	return (x>=0)?1:-1;
}

const double PI=3.14159265358979323846;
const double PI2=PI*2.0;
const double PI_2=PI/2.0;

inline double DEG_2_RAD(double x) 
{	
	return	((x)*PI/180.0); 
}
inline double RAD_2_DEG(double x) 
{
	return ((x)*180.0/PI); 
}

//INTERPOLATION CURVES (CONST DATA)

//Since these go past 90 I'm assuming it intended to steer past 90 on the wheel and filter to 115 from 1 to 0
const double c_radius_theta_x[] = {0, 90, 115, 180};
const double c_radius_theta_z[] = {1,  1,   0,   0};
//This table appears to have no effect since z is 1 all across
const double c_r_warp_x[] = {0, 45, 90, 135, 180};
const double c_r_warp_z[] = {1, 1, 1, 1, 1};

//These appear to only yield 1.0 from 115-170 degrees... I have not tested this alternate implementation
#ifdef __AlternateRawImplementation__
const double c_raw_theta_x[] = {0, 90, 115, 170, 180};
const double c_raw_theta_z[] = {0,  0,   1,   1, 0};
#endif

//CALIBRATIONS (CAL DATA)
double c_gain_radius = 1.2;
double c_gain_raw = 1.5;

//Note for Palardy size is now cardinal ;)
double interp_2d(double x,const double *x_tbl,const double *z_tbl, size_t size)
{
	size_t num_cur = 0;
	//num_cur index will point to the element that is closest to number in table without going over
	while((x_tbl[num_cur] <= x) && (num_cur < (size-1)))
		num_cur++;

	//we assume x fits within range of table
	assert(num_cur);

	int num_last = num_cur - 1;
	//Find fraction between num_cur and num_cur--
	double x_frac;
	{
		const double lt = x_tbl[num_last];
		const double gt = x_tbl[num_cur];
		const double tmp = x - lt;
		const double range = gt - lt;
		x_frac = tmp / range;

		//printf("Interpolation: Found X:%f between lt:%f and gt:%f (pts %d and %d) frac=%f",x,lt,gt,num_last,num_cur,x_frac);
	}

	//Find the two points on the z table
	double z_out;
	{
		const double lt = z_tbl[num_last];
		const double gt = z_tbl[num_cur];
		const double range = gt - lt;
		z_out = (range * x_frac) + lt;
		//printf(" z: lt:%f gt:%f zout:%f\n",lt,gt,z_out);
	}

	return z_out;
}

//This version of it clamps the range... but also will change any NAN cases to zero (This is a generic solution over _isnan() )
inline double limit_motor(double x)
{
	//Clamp range
	if (x>0.0)
	{
		if (x>1.0)
			x=1.0;
	}
	else if (x<0.0)
	{
		if (x<-1.0)
			x=-1.0;
	}
	else
	{
		x=0.0;  //is nan case
		assert(false);  //Sanity check, This shouldn't happen with the simple math in this example
	}
	return x;
}

//Math function for Culver Drive
void culver_drive(double throttle, double wheelx, double wheely, bool quickturn, double &left, double &right)
{
	//double theta = fabs(atan((wheely/wheelx)));

	//I am confused as part of the code appears that we work only from a range of -90 - 90... while the tables go past 90
	//The original implementation shows any down stick movement is the same as if it were up
	//For this new replacement if we want to support values past 90 we can remove the fabs()... while this idea would support the real model 
	//of a steering wheel. I can see it being an undesirable effect (hence the filtering)... especially since there is the quick turn option

	//Find arctan of wheel stick relative to vertical... note relative to vertical suggests that we swap the x and y components!
	#if 0
	const double theta_native = atan2(wheelx,fabs(wheely));
	#else
	const double theta_native = atan2(wheelx,-wheely);  //I suspect we wanted to go past 90 based from the tables
	#endif

	//Test atan2 against atan... this test only succeeds if we use the fabs(wheely) version
	#if 0
	//note Palardy... there was a bug 59.29577... should be 57.29577
	double test = fabs(atan((wheelx/wheely)));  //if it is relative to vertical the x and y components need to be swapped
	double sign_test = sgn(wheelx);
	assert (fabs(theta - (test * sign_test))<1e-5);  //check for equality within 5 decimal places
	printf("%.2f %.2f %.2f %.2f\n",RAD_2_DEG(theta),RAD_2_DEG(test * sign_test),wheelx,wheely);
	#endif

	// and turn it into degrees
	//convert to absolute value and in degrees (grrrr should never solve code in degrees)  ;)
	const double sign = sgn(theta_native);  //keep the sign to restore it later
	const double theta = fabs(RAD_2_DEG(theta_native));  

	//as it stands this statement has no effect if c_r_warp_z has all 1's
	assert(interp_2d(theta,c_r_warp_x,c_r_warp_z,_countof(c_r_warp_x)) == 1.0);

	//Find the magnitude of the wheel stick
	double r = sqrt(((wheely * wheely) + (wheelx * wheelx))) * interp_2d(theta,c_r_warp_x,c_r_warp_z,_countof(c_r_warp_x));

	//Find the Radius Enablement curve
	double radius_en = interp_2d(theta,c_radius_theta_x,c_radius_theta_z,_countof(c_radius_theta_x));

	printf("%.2f, %.2f, %.2f, %.2f, %.2f\n",theta * sign,r,radius_en,wheelx,wheely);


	//Find the radius component based on r, theta, enablement curve, gain, throttle, sign
	double radius = r * theta/90 * radius_en * c_gain_radius * throttle * sign;

	//Find the raw component based on r, radius enablement curve, gain, sign
	#ifndef __AlternateRawImplementation__
	//double raw = r * radius_en * c_gain_raw * sign;
	double raw = r * theta/90 * radius_en * c_gain_raw * sign;  //this needs theta
	#else
	//Alternate raw implementation
	double raw = r * interp_2d(theta, c_raw_theta_x, c_raw_theta_z, _countof(c_raw_theta_x)) * c_gain_raw * sign;
	#endif

	//Find Left and Right powers as sums
	double tmpleft = throttle;
	double tmpright = throttle;

	//Comment out the entire IF/Else block for alternate raw mode
	if(quickturn)
	{
		#if 1
		tmpleft += raw;
		tmpright -= raw;
		#else
		//Find the left and right powers as the sums - Use for Alternate Raw
		tmpleft += radius + raw;
		tmpright -= radius + raw;
		#endif
	}
	else
	{
		tmpleft += radius;
		tmpright -= radius;
	}

	//clamp value range to +-1
	left = limit_motor(tmpleft);
	right = limit_motor(tmpright);
}

//reciprocal
const double c_half_pi_reciprocal=1.0 / (PI_2);  //its more efficient to multiply than to divide... this is around 0.6366...
const double c_taper_limit=DEG_2_RAD(115.0);
const double c_taper_length_recip=1.0 / (c_taper_limit-PI_2);
void culver_drive2(double throttle, double wheelx, double wheely, bool quickturn, double &left, double &right)
{
	//Find arctan of wheel stick relative to vertical... note relative to vertical suggests that we swap the x and y components!
	const double theta = atan2(wheelx,-wheely);

	//Find the magnitude of the wheel stick
	double r = sqrt(((wheely * wheely) + (wheelx * wheelx)));

	//taper off past 90 - 115 using simple linear interpolation 
	double radius_filter=1.0;
	if (fabs(theta)>PI_2)
	{
		const double abs_theta=fabs(theta);
		if (abs_theta<c_taper_limit)
			radius_filter=(1.0-((abs_theta-PI_2) * c_taper_length_recip));
		else
			radius_filter=0;
	}

	//convert to degrees for UI viewing
	//printf("%.2f, %.2f, %.2f, %.2f, %.2f\n",RAD_2_DEG(theta),r,radius_filter,wheelx,wheely);


	//Find Left and Right powers as sums
	double tmpleft = throttle;
	double tmpright = throttle;

	if(quickturn)
	{
		//Find the raw component based on r, radius filter curve, gain, sign
		#ifndef __AlternateRawImplementation__
		const double raw = r * theta*c_half_pi_reciprocal * radius_filter * c_gain_raw; 
		#else
		//Alternate raw implementation
		const double raw = r * interp_2d(theta, c_raw_theta_x, c_raw_theta_z, _countof(c_raw_theta_x)) * c_gain_raw);
		#endif

		printf("%.2f, %.2f, %.2f  -->  X(%.2f) Y(%.2f)\n",r,RAD_2_DEG(theta),raw,wheelx,wheely);

		//here is a separate alternate raw case
		#if 1
		tmpleft += raw;
		tmpright -= raw;
		#else
		const double radius = r * theta*c_half_pi_reciprocal * radius_filter * c_gain_radius * throttle;
		//Find the left and right powers as the sums - Use for Alternate Raw
		tmpleft += radius + raw;
		tmpright -= radius + raw;
		#endif
	}
	else
	{
		//Find the radius component based on r, theta, filter curve, gain, throttle, sign
		const double radius = r * theta*c_half_pi_reciprocal * radius_filter * c_gain_radius * throttle;

		printf("%.2f, %.2f, %.2f  -->  X(%.2f) Y(%.2f)\n",r,RAD_2_DEG(theta),radius,wheelx,wheely);
		tmpleft += radius;
		tmpright -= radius;
	}

	//clamp value range to +-1
	left = limit_motor(tmpleft);
	right = limit_motor(tmpright);
}

void main()
{
	double theta=-PI;
	const double NoIterations=100.0;
	const double rho=PI2 / NoIterations;  //compute step size... by dividing the interval by number of iterations
	const bool DoRaw=false;
	//do a sweep test full forward throttle non quick turn
	while (theta<PI)
	{
		double Left,Right;
		double x=sin(theta);
		double y=-cos(theta);
		//Test with weaker magnitude
		#if 0
		y*=0.5;
		x*=0.5;
		#endif
		//Test with no Y
		#if 0
		y=0;
		#endif
		//swapped as 0 degrees if vertical alignment
		//culver_drive(1.0,x,y,false,Left,Right);
		culver_drive2(1.0,x,y,DoRaw,Left,Right);
		theta+=rho;
	}
	printf("\n");
}