Unofficial Response to: RobotC help XDive

Original Question, @Javapower:
Someone help me with this, I found this off of the internet, but my robot is only going in circles (this is RobotC):


#pragma config(UART_Usage, UART1, uartVEXLCD, baudRate19200, IOPins, None, None)
#pragma config(UART_Usage, UART2, uartNotUsed, baudRate4800, IOPins, None, None)
#pragma config(Motor, port3, frontLeft, tmotorVex269, openLoop)
#pragma config(Motor, port2, rearLeft, tmotorVex269, openLoop)
#pragma config(Motor, port5, rearRight, tmotorVex269, openLoop)
#pragma config(Motor, port4, frontRight, tmotorVex269, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//

/*

Radian to Holonomic Output Testing Code
* Licensend under the Lesser General Public License v3.0. http://goo.gl/fB9rz  (c) 2013
Author: Elliot Berman
/
// In order for the trigonometry functions to work correctly, all drivetrain motors
// should have a +PWM value corresponding to a clockwise rotation on the wheel.

#include "HolonomicRadians.h";

typedef enum {
left = 1,
center = 2,
right = 4
} cLCDButtons;

task main()
{
// Option 1: Full Joystick Control
displayLCDCenteredString(0,"Testing Holonomic");
displayLCDCenteredString(1,"loading...");
wait1Msec(500);
bLCDBacklight = true;
if(nVexRCReceiveState & vrXmit1)
// Jumper clip is plugged into digital port 1, and there is currenty a joystick connected
{
PolarJoystick joystick;
displayLCDCenteredString(0,"Using Joystick");
while(true)
{
clearLCDLine(1);
getPolarJoy(&joystick.radians, &joystick.speed);
HolonomicRadianOutput(joystick.radians, joystick.speed, vexRT[Ch4]);
writeDebugStreamLine("r: %f , s: %f",joystick.radians, joystick.speed);
wait1Msec(10);
}
}
//End of Option 1

else // Jumper clip not plugged in and/or joystick is not connected
{
//Option 2: Use LCD Screen or Ch8 joystick buttons to cycle through all multiples of PI/4 for [0,2PI)
bool leftLCDPressed = false;
bool rightLCDPressed = false;
bool leftJoyPressed = false;
bool rightJoyPressed = false;
bool centerLCDPressed = false;
bool downJoyPressed = false;
bool killSwitch = false;
int radianMultiplier = 0;
bLCDBacklight = true;
while(true)
{
// Check the left LCD Button for changes
if(nLCDButtons == left && !leftLCDPressed)
{
radianMultiplier--;
leftLCDPressed = true;
}
else if(nLCDButtons != left) leftLCDPressed = false;

// Check the right LCD button for changes
if(nLCDButtons == right && !rightLCDPressed)
{
radianMultiplier++;
rightLCDPressed = true;
}
else if(nLCDButtons != right) rightLCDPressed = false;

// Check the left Ch 8 button for changes
if(vexRT[Btn8L] && !leftJoyPressed)
{
radianMultiplier--;
leftJoyPressed = true;
}
else if(!vexRT[Btn8L]) leftJoyPressed = false;

// Check the right Ch 8 button for changes
if((vexRT[Btn8R]) && !rightJoyPressed)
{
radianMultiplier++;
rightJoyPressed = true;
}
else if(!vexRT[Btn8R]) rightJoyPressed = false;

// Check the center LCD button for changes
if(nLCDButtons == center && centerLCDPressed == false) {
killSwitch = killSwitch ? false : true;
centerLCDPressed = true;
}
else if(nLCDButtons != center) centerLCDPressed = false;

// Check the right Ch8 Button for changes
if(vexRT[Btn8D] && !downJoyPressed)
{
killSwitch = killSwitch ? false : true;
downJoyPressed = true;
}
else if(!vexRT[Btn8D]) downJoyPressed = false;

// We want the values to cycle, so when we go beyond bounds, go to the start of the opposite bound
if(radianMultiplier > 7) radianMultiplier = 0;
if(radianMultiplier < 0) radianMultiplier = 7;

displayLCDCenteredString(0,"Cycling RadPI/4");
switch(radianMultiplier)
{
case 0: displayLCDCenteredString(1,"Direction: 0"); break;
case 1: displayLCDCenteredString(1,"Direction: PI/4"); break;
case 2: displayLCDCenteredString(1,"Direction: PI/2"); break;
case 3: displayLCDCenteredString(1,"Direction: 3PI/4"); break;
case 4: displayLCDCenteredString(1,"Direction: PI"); break;
case 5: displayLCDCenteredString(1,"Direction: 5PI/4"); break;
case 6: displayLCDCenteredString(1,"Direction: 3PI/2"); break;
case 7: displayLCDCenteredString(1,"Direction: 7PI/4"); break;
default: displayLCDCenteredString(1,"ERROR");
}
if(!killSwitch) HolonomicRadianOutput(((float)radianMultiplier*(PI/4)),1,0); // the last 1,0 is optional, as it is defaulted to those vals
else if(killSwitch)
{
motor[frontLeft] = 0;
motor[frontRight] = 0;
motor[rearRight] = 0;
motor[rearLeft] = 0;
}
}
}
// End of Option 1

// Debugging Option 3: Runs the "traditional" holonomic code (no trig function). Should be used for determining hardware vs software issues
/*
while(true) {
motor[rearRight] = vexRT[Ch2] - vexRT[Ch4] - vexRT[Ch1];
motor[frontRight] = vexRT[Ch2] - vexRT[Ch4] + vexRT[ch1];
motor[frontLeft] = -vexRT[Ch2] + vexRT[Ch4] - vexRT[Ch1];
motor[rearLeft] = -vexRT[Ch2] + vexRT[Ch4] + vexRT[Ch1];
}
*/
}

.h file:

/*

HolonomicRadians.h
* Licensend under the Lesser General Public License v3.0. http://goo.gl/fB9rz  (c) 2013
Author: Elliot Berman
/
#define maxMotorSpeed 127
#define numberOfMotors 4

//**-----------------------SUPPORT FUNCTIONS-----------------------------------**//

float FindMaxFloat(float a, float b, float c = (0), float d = (0), float e = (0),
float f = (0), float g = (0), float h = (0), float i = (0), float j = (0))
{
float values] = {a,b,c,d,e,f,g,h,i,j};
float maxValue = 0;
for(int z = 0; z < 9; z++) {
if(abs(values[z]) > maxValue) maxValue = abs(values[z]);
}
return maxValue;
}

//**-----------------------MAIN FUNCTIONS------------------------------------**//

void HolonomicRadianOutput(float radians, float speed = 1, byte rotation = 0)
{
// Please refer to README.txt for a full explanation of the formulas used.
if(speed > 0)
{
float frontLeftOutput = -maxMotorSpeed * cos(PI/4 - radians),
frontRightOutput = maxMotorSpeed * cos(PI/4 + radians),
rearRightOutput = maxMotorSpeed * cos(PI/4 - radians),
rearLeftOutput	= -maxMotorSpeed * cos(PI/4 + radians);

frontLeftOutput += rotation;
frontRightOutput += rotation;
rearRightOutput += rotation;
rearLeftOutput += rotation;

float maxOutput = FindMaxFloat(frontLeftOutput, frontRightOutput, rearRightOutput, rearLeftOutput);
// The goal is to rescale all values to -127<=out<=127. See README.txt for algebraic explanation.
speed *= (maxMotorSpeed / maxOutput);

frontLeftOutput *= speed;
frontRightOutput *= speed;
rearLeftOutput *= speed;
rearRightOutput *= speed;

motor[frontLeft] = (byte)frontLeftOutput;
motor[frontRight] = (byte)frontRightOutput;
motor[rearLeft] = (byte)rearLeftOutput;
motor[rearRight] = (byte)rearRightOutput;
}
else if (rotation > 20 || rotation < -20)
{
motor[frontLeft] = rotation;
motor[frontRight] = rotation;
motor[rearLeft] = rotation;
motor[rearRight] = rotation;
}
else
{
motor[frontLeft] = 0;
motor[frontRight] = 0;
motor[rearLeft] = 0;
motor[rearRight] = 0;
}
}

#define JOY_Y_DEADBAND 20
#define JOY_X_DEADBAND 20

typedef struct
{
float radians;
float speed;
} PolarJoystick;

void getPolarJoy(float *radians, float *speed, TVexJoysticks joy_x = Ch2, TVexJoysticks joy_y = Ch1) {
byte x_val = vexRT[joy_x];
byte y_val = vexRT[joy_y];
if((abs(x_val) < 20) && (abs(y_val) < 20)) {
*radians = 0;
*speed = 0;
}
else {
*radians = atan2(y_val,x_val);
float tmpSpeed = sqrt((y_val * y_val) + (x_val * x_val));
*speed = tmpSpeed/127;
if(*speed > 1) *speed = 1;
}
}

This is for everyone else to answer. I believe that you shouldn’t use any code that you just found on the internet and want to use but don’t understand You should also avoid using code that you find that you barely understand that also uses an include file. (I’m assuming that you don’t understand most of this code as it is fairly complex and you don’t know why it’s going in circles :slight_smile: ) You should start out with a basic x-drive code. For example:

motor[leftfront] = VexRT[ch3] - VexRT[ch1] - VexRT[ch4];
Motor[rightfront] = VexRT[ch3] - VexRT[ch1] + VexRT[ch4];
Motor[leftrear] = VexRT[ch3] + VexRT[ch1] + VexRT[ch4];
Motor[rightrear]= VexRT[ch3] + VexRT[ch1] - VexRT[ch4];

Learn more about RobotC before trying to use all of it’s features. @Cody, if you want to, you could make another video.

I’m confused as to what the original code was trying to do. Could @Javapower explain exactly what the goal of that code was?

That’s my code taken from here:

https://vexforum.com/t/holonomic-drives-2-0-a-video-tutorial-by-cody/27052/1

I’ve already done videos on this, everything you need is in that thread.

It’s code that lets you control the exact heading, speed, and rotation of a holonomic robot, rather than using the usual joystick channel addition (mentioned by Cody above). This is useful for autonomous routines where you want to go at unusual angles or curves and where you might want to have more complex driver control logic. I wouldn’t recommend it to anyone who just wants simple holonomic control. Joystick channel addition is faster and simpler.

The program that javapower posted was the demo program, where it would be simpler to use joystick channel addition.

I should probably get around to rewriting those tutorials at some point… (they’re still hobbling at blog.elliotjb.com)

It’s actually @edjubuh’s from his GitHub: https://github.com/edjubuh/HolonomicXDrive-ROBOTC/blob/master/src/HolonomicRadianOutputTesting.c

I was specifically talking about the code at the bottom.