Hello all,
I am currently working on controlling a servo motor with an Xbox controller
I am able to control the servo motor by for example a sinusoid signal, and I am able to control the input of the servo motor with the Xbox controller.
However, when I try to combine the two, I can again control the input of the servo motor, however, I cannot control the servo motor itself.
The full code (I have stripped it down, but it's still giving the problem):
#include <Servo.h>
#include <XBOXRECV.h>
#ifdef dobogusinclude
#include <spi4teensy3.h>
#endif
// Control servo variables
Servo myservo;
const int ms_min = 1000; // minimum [ms] sent to servo
const int ms_max = 2100; // maximum [ms] sent to servo
// Timing constants
unsigned long NewTick; // [ms] time start of current loop
unsigned long LastTick; // [ms] time start of previous loop
float dt; // [ms] time between consecutive loops
float interval_constant = 20; // [ms] per signal send
int count_setup = 20; // [s] setup time
// Servo constants
int ms_input = ms_min;
// Xbox controller variables
int Hat_tol = 5000; // Tolerance on when the Xbox controller responds to input LeftHatY
float throttle_analog; // Value received from input LeftHatY
float throttle; // 0-1, gives rate of minimum to maximum input LeftHatY
int k = 0; // Variable to trigger 'Controller connected' message.
USB Usb;
XBOXRECV Xbox(&Usb);
float ms_float = (float) ms_min; // [ms] calculated from Xbox Controller input (float)
int ms_int = ms_min; // [ms] calculated from Xbox Controller input (integer)
void setup() {
Serial.begin(9600);
while (!Serial); // Wait for serial port to connect
// Give user time to connect ESC to Arduino board
Serial.print("\n\n Program started, connect ESC to motor ("); Serial.print(count_setup); Serial.println(" seconds)");
for (uint8_t ii = count_setup; ii > 0; ii--) {
Serial.print(ii); Serial.println(". "); delay(1000);
}
// ESC calibration and attach Servo to pin 9
Serial.println("ESC calibration started");
myservo.attach(9);
esc();
Serial.println("ESC calibration completed");
Serial.println(" ");
// USB initialization
Serial.println("Start USB Init");
Serial.println(" ");
if (Usb.Init() == -1) {
Serial.print(F("\r\nOSC did not start"));
while (1);
}
Serial.println("USB Init complete");
Serial.println("Connect controller (3 seconds)");
delay(3000);
}
void esc()
{
delay(5000);
Serial.println("Sending lowest throttle ms_min");
myservo.writeMicroseconds(ms_min);
delay(2000);
Serial.println("Low throttle sent");
}
void loop() {
NewTick = millis();
dt = NewTick - LastTick;
if (dt > 50) {
ms_input = Define_input(); //Retrieve input servo from XboxController
myservo.writeMicroseconds(ms_input); //Send input to servo
LastTick = NewTick;
}
// Output to Serial to see values
Serial.print(NewTick);
Serial.print(",");
Serial.print(ms_input);
Serial.println("");
}
// Check input of Xbox controller
int Define_input()
{
throttle = 0; //Set throttle to 0, so that if no input is found, no throttle (ms_min) is sent
Usb.Task(); //Polls connected usb devices for updates to their status
if (Xbox.XboxReceiverConnected) {
if (Xbox.Xbox360Connected[0]) {
if (k==0){ //Print to Serial whenever the controller is connected
Serial.println("Controller connected!");
k = 1;
}
if (Xbox.getAnalogHat(LeftHatY, 0) > Hat_tol) { //If value of LeftHatY is larger than tolerance 'Hat_tol'
throttle_analog = Xbox.getAnalogHat(LeftHatY, 0);
throttle = (throttle_analog - Hat_tol) / (32767 - Hat_tol); // Calculate the throttle (0 - 1)
}
}
}
ms_float = ms_min+(ms_max-ms_min)*throttle; // Calculate the input servo [ms] from throttle.
ms_int = (int) ms_float; //Convert input servo from float to integer
return ms_int;
}
Could it have to do with the fact that I am using the USB host shield every loop to read out the Xbox Controller signal, and because of the USB.task() I cannot use the shield to give input to the servo motor?
It would really help me if anybody can help me with this problem.
With kind regards,
Thijs
