I am writing code for my a robot and basically i need to take a usb xbox controller and turn it into a value between 0-180 for servo control. I have written a function Proportio designed to take the values from the controller, -32,767 - 32,768, and convert the negative to 0-90 and the positive to 90-180. However when I run the code these are not the values I am getting and the values I am getting are not consistent I would appritate if anyone could figure out what is wrong and let me know.
#include <Servo.h>
#include <XBOXUSB.h>
#ifdef dobogusinclude
#include <spi4teensy3.h>
#include <SPI.h>
#endif
USB Usb; //declares a Usb device usb
XBOXUSB Xbox(&Usb); //sets a xbox usb device at usbs location
Servo YM; // Servo for Y motion (Forward, Backward)
Servo XM; // Servo for x motion (Left, Right)
Servo ZM; // Servo for z motion (Up, down)
Servo RM; // Servo for R motion (Rotation)
void setup()
{
YM.attach(9); //Pick the pin for Y motion
XM.attach(9); //Pick the pin for X motion
ZM.attach(9); //Pick the pin for Z motion
RM.attach(9); //Pick the pin for R motion
Serial.begin(115200); //Begin serial port out put and set the
// make sure USB usb initialized correctly to make sure libraries are inizializing.
if (Usb.Init() == -1) {
Serial.print(F("\nlibrary Failed to start "));
while (1); //halt
}
Serial.print(" \nXbox USB Library Started. ");
}
long int Prop;
long int input;
long int Proportion(long int input)
{
//range of 65,535 from -32,767 to 32,768
//-32,767/90=-364.0777777777778
//joystick value/364.0777777777778= +- value between 0-90
if (input > 0)
{
Prop = (input / 364.0777777777778) + 90;
}
else if (input < 0)
{
Prop = (input / 364.0777777777778)-90;
}
}
void Joysticks()
{
if (Xbox.getAnalogHat(LeftHatX) > 3500 || Xbox.getAnalogHat(LeftHatX) < -3500) //Sets dead zones for left sticks X
{
Serial.print(F("\nLeftHatX:"));
Serial.print(Xbox.getAnalogHat(LeftHatX)); //Prints the value of the left sticks x location to the serial monitor.
input = Xbox.getAnalogHat(LeftHatX); //set input equal to the x value of the stick
Prop = Proportion(input);
Serial.print("\n");
Serial.print(Prop);
XM.write(Prop);
}
if (Xbox.getAnalogHat(LeftHatY) > 3500 || Xbox.getAnalogHat(LeftHatY) < -3500) //Sets dead zones for left sticks Y
{
Serial.print(F("\nLeftHatY:"));
Serial.print(Xbox.getAnalogHat(LeftHatY));
input = Xbox.getAnalogHat(LeftHatX); //set input equal to the x value of the stick
Prop = Proportion(input);
Serial.print("\n");
Serial.print(Prop);
YM.write(Prop);
}
if (Xbox.getAnalogHat(RightHatX) > 3500 || Xbox.getAnalogHat(RightHatX) < -3500) //sets dead zones for right sticks X
{
Serial.print(F("\nRightHatX: "));
Serial.print(Xbox.getAnalogHat(RightHatX));
input = Xbox.getAnalogHat(LeftHatX); //set input equal to the x value of the stick
Prop = Proportion(input);
Serial.print("\n");
Serial.print(Prop);
RM.write(Prop);
}
if (Xbox.getAnalogHat(RightHatY) > 3500 || Xbox.getAnalogHat(RightHatY) < -3500) //sets dead zones for right stick Y
{
Serial.print(F("\nRightHatY: "));
Serial.print(Xbox.getAnalogHat(RightHatY));
input = Xbox.getAnalogHat(LeftHatX); //set input equal to the x value of the stick
Prop = Proportion(input);
Serial.print("\n");
Serial.print(Prop);
}
}
void loop()
{
Usb.Task();
Joysticks();
}
Update: solved, I was an idiot and did manage to notice when I set prop = to Xbox.get... that they were all pulling the lefthatx value instead of their own.