remove old stuff
This commit is contained in:
Thefeli73
parent
16647bc5ff
commit
8b662ea652
@ -1,505 +0,0 @@
|
|||||||
/* Copyright (C) 2013-2015 Kristian Lauszus, TKJ Electronics. All rights reserved.
|
|
||||||
This software may be distributed and modified under the terms of the GNU
|
|
||||||
General Public License version 2 (GPL2) as published by the Free Software
|
|
||||||
Foundation and appearing in the file GPL2.TXT included in the packaging of
|
|
||||||
this file. Please note that GPL2 Section 2[b] requires that all works based
|
|
||||||
on this software must also be made publicly available under the terms of
|
|
||||||
the GPL2 ("Copyleft").
|
|
||||||
Contact information
|
|
||||||
-------------------
|
|
||||||
Kristian Lauszus, TKJ Electronics
|
|
||||||
Web : http://www.tkjelectronics.dk
|
|
||||||
e-mail : kristianl@tkjelectronics.dk
|
|
||||||
*/
|
|
||||||
|
|
||||||
#if defined(ENABLE_USB) || defined(ENABLE_SPEKTRUM)
|
|
||||||
|
|
||||||
static uint8_t ps3OldLed, wiiOldLed, ps4OldBatteryLevel;
|
|
||||||
|
|
||||||
#ifdef ENABLE_XBOX
|
|
||||||
static LEDEnum xboxOldLed;
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifdef ENABLE_SPP
|
|
||||||
void readSPPData() {
|
|
||||||
if (SerialBT.connected) { // The SPP connection won't return data as fast as the controllers, so we will handle it separately
|
|
||||||
if (SerialBT.available()) {
|
|
||||||
uint8_t i = 0;
|
|
||||||
while (1) {
|
|
||||||
dataInput[i] = SerialBT.read();
|
|
||||||
if (dataInput[i] == -1) // Error while reading the string
|
|
||||||
return;
|
|
||||||
if (dataInput[i] == ';') // Keep reading until it reads a semicolon
|
|
||||||
break;
|
|
||||||
if (++i >= sizeof(dataInput) / sizeof(dataInput[0])) // String is too long
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
bluetoothData = true;
|
|
||||||
setValues(dataInput);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
#endif // ENABLE_SPP
|
|
||||||
|
|
||||||
void readUsb() {
|
|
||||||
#ifdef ENABLE_USB
|
|
||||||
Usb.Task(); // The SPP data is actually not send until this is called, one could call SerialBT.send() directly as well
|
|
||||||
|
|
||||||
if (Usb.getUsbTaskState() == USB_STATE_ERROR && layingDown) { // Check if the USB state machine is in an error state, but also make sure the robot is laying down
|
|
||||||
Serial.println(F("USB fail"));
|
|
||||||
Usb.vbusPower(vbus_off);
|
|
||||||
delay(1000);
|
|
||||||
Usb.vbusPower(vbus_on);
|
|
||||||
Usb.setUsbTaskState(USB_DETACHED_SUBSTATE_WAIT_FOR_DEVICE); // Reset state machine
|
|
||||||
}
|
|
||||||
#endif // ENABLE_USB
|
|
||||||
|
|
||||||
#ifdef ENABLE_SPP
|
|
||||||
readSPPData();
|
|
||||||
#endif // ENABLE_SPP
|
|
||||||
|
|
||||||
if (millis() > (receiveControlTimer + receiveControlTimeout)) {
|
|
||||||
commandSent = false; // We use this to detect when there has already been sent a command by one of the controllers
|
|
||||||
#ifdef ENABLE_PS3
|
|
||||||
if (PS3.PS3Connected) {
|
|
||||||
if (PS3.getButtonPress(SELECT)) {
|
|
||||||
stopAndReset();
|
|
||||||
while (!PS3.getButtonPress(START))
|
|
||||||
Usb.Task();
|
|
||||||
}
|
|
||||||
else if (PS3.getButtonPress(CROSS) || (PS3.getAnalogHat(LeftHatY) < 117 || PS3.getAnalogHat(RightHatY) < 117 || PS3.getAnalogHat(LeftHatY) > 137 || PS3.getAnalogHat(RightHatY) > 137))
|
|
||||||
steer(updatePS3);
|
|
||||||
} else if (PS3.PS3NavigationConnected && (PS3.getAnalogHat(LeftHatX) > 200 || PS3.getAnalogHat(LeftHatX) < 55 || PS3.getAnalogHat(LeftHatY) > 137 || PS3.getAnalogHat(LeftHatY) < 117))
|
|
||||||
steer(updatePS3);
|
|
||||||
#endif // ENABLE_PS3
|
|
||||||
#ifdef ENABLE_PS4
|
|
||||||
if (PS4.connected() && !commandSent) {
|
|
||||||
if (PS4.getButtonPress(SHARE)) {
|
|
||||||
stopAndReset();
|
|
||||||
while (!PS4.getButtonPress(OPTIONS))
|
|
||||||
Usb.Task();
|
|
||||||
}
|
|
||||||
else if (PS4.getButtonPress(CROSS) || PS4.getAnalogHat(LeftHatY) < 117 || PS4.getAnalogHat(RightHatY) < 117 || PS4.getAnalogHat(LeftHatY) > 137 || PS4.getAnalogHat(RightHatY) > 137)
|
|
||||||
steer(updatePS4);
|
|
||||||
}
|
|
||||||
#endif // ENABLE_PS4
|
|
||||||
#ifdef ENABLE_WII
|
|
||||||
if (Wii.wiimoteConnected && !Wii.wiiUProControllerConnected && !commandSent) {
|
|
||||||
if (Wii.getButtonPress(B) || (Wii.nunchuckConnected && (Wii.getAnalogHat(HatX) > 137 || Wii.getAnalogHat(HatX) < 117 || Wii.getAnalogHat(HatY) > 137 || Wii.getAnalogHat(HatY) < 117)))
|
|
||||||
steer(updateWii);
|
|
||||||
} else if (Wii.wiiUProControllerConnected && !commandSent) { // The Wii U Pro Controller Joysticks has an range from approximately 800-3200
|
|
||||||
if (Wii.getButtonPress(MINUS)) {
|
|
||||||
stopAndReset();
|
|
||||||
while (!Wii.getButtonPress(PLUS))
|
|
||||||
Usb.Task();
|
|
||||||
}
|
|
||||||
else if (Wii.getAnalogHat(LeftHatY) > 2200 || Wii.getAnalogHat(LeftHatY) < 1800 || Wii.getAnalogHat(RightHatY) > 2200 || Wii.getAnalogHat(RightHatY) < 1800)
|
|
||||||
steer(updateWii);
|
|
||||||
}
|
|
||||||
#endif // ENABLE_WII
|
|
||||||
#ifdef ENABLE_XBOX
|
|
||||||
if (Xbox.Xbox360Connected[0] && !commandSent) { // We will only read from the first controller, up to four is supported by one receiver
|
|
||||||
if (Xbox.getButtonPress(BACK)) {
|
|
||||||
stopAndReset();
|
|
||||||
while (!Xbox.getButtonPress(START))
|
|
||||||
Usb.Task();
|
|
||||||
}
|
|
||||||
else if (Xbox.getAnalogHat(LeftHatY) < -7500 || Xbox.getAnalogHat(RightHatY) < -7500 || Xbox.getAnalogHat(LeftHatY) > 7500 || Xbox.getAnalogHat(RightHatY) > 7500)
|
|
||||||
steer(updateXbox);
|
|
||||||
}
|
|
||||||
#endif // ENABLE_XBOX
|
|
||||||
#ifdef ENABLE_SPEKTRUM
|
|
||||||
if (spekConnected) {
|
|
||||||
if (millis() - spekConnectedTimer > 100) // If it has been more than 100ms since last data, then it must be disconnected
|
|
||||||
spekConnected = false;
|
|
||||||
else {
|
|
||||||
if (!commandSent && (rcValue[RC_CHAN_ROLL] < 1490 || rcValue[RC_CHAN_ROLL] > 1510 || rcValue[RC_CHAN_PITCH] < 1490 || rcValue[RC_CHAN_PITCH] > 1510))
|
|
||||||
steer(updateSpektrum);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
#endif // ENABLE_SPEKTRUM
|
|
||||||
if (!commandSent) // If there hasn't been send a command by now, then send stop
|
|
||||||
steer(stop);
|
|
||||||
}
|
|
||||||
|
|
||||||
#ifdef ENABLE_PS3
|
|
||||||
if (PS3.PS3Connected || PS3.PS3NavigationConnected) {
|
|
||||||
if (PS3.getButtonClick(PS)) {
|
|
||||||
PS3.disconnect();
|
|
||||||
ps3OldLed = 0; // Reset value
|
|
||||||
}
|
|
||||||
}
|
|
||||||
#endif // ENABLE_PS3
|
|
||||||
#ifdef ENABLE_PS4
|
|
||||||
if (PS4.connected()) {
|
|
||||||
if (PS4.getButtonClick(PS)) {
|
|
||||||
PS4.disconnect();
|
|
||||||
ps4OldBatteryLevel = 0; // Reset value
|
|
||||||
}
|
|
||||||
}
|
|
||||||
#endif // ENABLE_PS4
|
|
||||||
#ifdef ENABLE_WII
|
|
||||||
if (Wii.wiimoteConnected || Wii.wiiUProControllerConnected) {
|
|
||||||
if (Wii.getButtonClick(HOME)) {
|
|
||||||
Wii.disconnect();
|
|
||||||
wiiOldLed = 0; // Reset value
|
|
||||||
}
|
|
||||||
}
|
|
||||||
#endif // ENABLE_WII
|
|
||||||
#ifdef ENABLE_XBOX
|
|
||||||
if (Xbox.Xbox360Connected[0]) {
|
|
||||||
if (Xbox.getButtonClick(XBOX)) {
|
|
||||||
Xbox.disconnect();
|
|
||||||
xboxOldLed = OFF; // Reset value
|
|
||||||
}
|
|
||||||
}
|
|
||||||
#endif // ENABLE_XBOX
|
|
||||||
|
|
||||||
#if defined(ENABLE_PS3) || defined(ENABLE_PS4) || defined(ENABLE_WII) || defined(ENABLE_XBOX)
|
|
||||||
if (millis() - ledTimer > 1000) { // Update every 1s
|
|
||||||
ledTimer = millis();
|
|
||||||
updateLEDs();
|
|
||||||
}
|
|
||||||
#endif // defined(ENABLE_PS3) || defined(ENABLE_PS4) || defined(ENABLE_WII) || defined(ENABLE_XBOX)
|
|
||||||
}
|
|
||||||
|
|
||||||
#if defined(ENABLE_PS3) || defined(ENABLE_PS4) || defined(ENABLE_WII) || defined(ENABLE_XBOX)
|
|
||||||
void updateLEDs() {
|
|
||||||
uint8_t Led;
|
|
||||||
#ifdef ENABLE_PS3
|
|
||||||
if (PS3.PS3Connected) {
|
|
||||||
if (ps3RumbleEnable) {
|
|
||||||
ps3RumbleEnable = false;
|
|
||||||
PS3.setRumbleOn(RumbleLow);
|
|
||||||
ps3RumbleDisable = true;
|
|
||||||
ledTimer -= 500; // Turn off again in 500ms
|
|
||||||
} else if (ps3RumbleDisable) {
|
|
||||||
ps3RumbleDisable = false;
|
|
||||||
PS3.setRumbleOff();
|
|
||||||
ps3OldLed = 0; // Reset value
|
|
||||||
} else {
|
|
||||||
if (PS3.getStatus(Shutdown)) { // Blink all LEDs
|
|
||||||
if (ps3OldLed)
|
|
||||||
Led = 0x00; // All off
|
|
||||||
else
|
|
||||||
Led = 0x0F; // All on
|
|
||||||
}
|
|
||||||
else if (PS3.getStatus(Dying))
|
|
||||||
Led = 0x01; // LED1 on
|
|
||||||
else if (PS3.getStatus(Low))
|
|
||||||
Led = 0x03; // LED1 and LED2 on
|
|
||||||
else if (PS3.getStatus(High))
|
|
||||||
Led = 0x07; // LED1, LED2 and LED3 on
|
|
||||||
else if (PS3.getStatus(Full))
|
|
||||||
Led = 0x0F; // LED1, LED2, LED3 and LED4 on
|
|
||||||
else if (PS3.getStatus(Charging))
|
|
||||||
Led = (ps3OldLed == 0x0F ? 0x01 : (ps3OldLed << 1 | 1) & 0x0F); // Indicate charging using the LEDs
|
|
||||||
else
|
|
||||||
Led = ps3OldLed;
|
|
||||||
|
|
||||||
if (Led != ps3OldLed) {
|
|
||||||
ps3OldLed = Led;
|
|
||||||
PS3.setLedRaw(Led);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
} else if (PS3.PS3NavigationConnected) {
|
|
||||||
if (PS3.getStatus(Shutdown))
|
|
||||||
PS3.setLedToggle(LED1); // Blink LED
|
|
||||||
else {
|
|
||||||
Led = 0x01; // LED on
|
|
||||||
if (Led != ps3OldLed) {
|
|
||||||
ps3OldLed = Led;
|
|
||||||
PS3.setLedRaw(Led);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
#endif // ENABLE_PS3
|
|
||||||
#ifdef ENABLE_PS4
|
|
||||||
if (PS4.connected()) {
|
|
||||||
if (ps4RumbleEnabled) {
|
|
||||||
ps4RumbleEnabled = false;
|
|
||||||
PS4.setRumbleOff();
|
|
||||||
ps4OldBatteryLevel = 0; // Reset value
|
|
||||||
} else {
|
|
||||||
uint8_t ps4BatteryLevel = PS4.getBatteryLevel();
|
|
||||||
if (ps4BatteryLevel != ps4OldBatteryLevel) {
|
|
||||||
ps4OldBatteryLevel = ps4BatteryLevel;
|
|
||||||
PS4.setLed(0, 0, ps4BatteryLevel * 17); // The battery status is in the range from 0-15, by multiplying we get it in the range 0-255
|
|
||||||
if (ps4BatteryLevel < 2)
|
|
||||||
PS4.setLedFlash(10, 10); // Blink rapidly
|
|
||||||
else
|
|
||||||
PS4.setLedFlash(0xFF, 0); // Turn on
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
#endif // ENABLE_PS4
|
|
||||||
#ifdef ENABLE_WII
|
|
||||||
if (Wii.wiimoteConnected || Wii.wiiUProControllerConnected) {
|
|
||||||
if (wiiRumbleEnabled) {
|
|
||||||
wiiRumbleEnabled = false;
|
|
||||||
Wii.setRumbleOff();
|
|
||||||
wiiOldLed = 0; // Reset value
|
|
||||||
} else {
|
|
||||||
uint8_t batteryLevel = Wii.getBatteryLevel();
|
|
||||||
if (batteryLevel < 60) { // Blink all LEDs
|
|
||||||
if (wiiOldLed)
|
|
||||||
Led = 0x00; // All off
|
|
||||||
else
|
|
||||||
Led = 0xF0; // All on
|
|
||||||
}
|
|
||||||
else if (batteryLevel < 110)
|
|
||||||
Led = 0x10; // LED1 on
|
|
||||||
else if (batteryLevel < 160)
|
|
||||||
Led = 0x30; // LED1 and LED2 on
|
|
||||||
else if (batteryLevel < 210)
|
|
||||||
Led = 0x70; // LED1, LED2 and LED3 on
|
|
||||||
else
|
|
||||||
Led = 0xF0; // LED1, LED2, LED3 and LED4 on
|
|
||||||
if (Led != wiiOldLed) {
|
|
||||||
wiiOldLed = Led;
|
|
||||||
Wii.setLedRaw(Led);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
#endif // ENABLE_WII
|
|
||||||
#ifdef ENABLE_XBOX
|
|
||||||
if (Xbox.Xbox360Connected[0]) {
|
|
||||||
if (xboxRumbleDisable) {
|
|
||||||
xboxRumbleDisable = false;
|
|
||||||
Xbox.setRumbleOff();
|
|
||||||
xboxOldLed = OFF; // Reset value
|
|
||||||
} else {
|
|
||||||
uint8_t batteryLevel = Xbox.getBatteryLevel();
|
|
||||||
LEDEnum xboxLed;
|
|
||||||
if (batteryLevel == 0)
|
|
||||||
xboxLed = LED1;
|
|
||||||
else if (batteryLevel == 1)
|
|
||||||
xboxLed = LED2;
|
|
||||||
else if (batteryLevel == 2)
|
|
||||||
xboxLed = LED3;
|
|
||||||
else
|
|
||||||
xboxLed = LED4;
|
|
||||||
|
|
||||||
if (xboxLed != xboxOldLed) {
|
|
||||||
xboxOldLed = xboxLed;
|
|
||||||
Xbox.setLedOn(xboxLed);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
#endif // ENABLE_XBOX
|
|
||||||
}
|
|
||||||
|
|
||||||
#ifdef ENABLE_PS3
|
|
||||||
void onInitPS3() { // This function is called when the controller is first initialized
|
|
||||||
if (PS3.PS3Connected) { // We only check the normal PS3 controller as the Navigation controller doesn't have rumble support
|
|
||||||
updateLEDs(); // Turn the LEDs on according to the voltage level
|
|
||||||
ledTimer = millis() - 500; // Wait 500ms before turning rumble on
|
|
||||||
ps3RumbleEnable = true; // We can't sent commands to the PS3 controller that often, so we don't send the command here
|
|
||||||
}
|
|
||||||
}
|
|
||||||
#endif // ENABLE_PS3
|
|
||||||
|
|
||||||
#ifdef ENABLE_PS4
|
|
||||||
void onInitPS4() { // This function is called when the controller is first initialized
|
|
||||||
if (PS4.connected()) { // Check if the PS4 controller is connected
|
|
||||||
updateLEDs(); // Turn the LEDs on according to the voltage level
|
|
||||||
ledTimer = millis() - 500; // This will turn the rumble off again after 500ms
|
|
||||||
PS4.setRumbleOn(RumbleLow);
|
|
||||||
ps4RumbleEnabled = true;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
#endif // ENABLE_PS4
|
|
||||||
|
|
||||||
#ifdef ENABLE_WII
|
|
||||||
void onInitWii() { // This function is called when the controller is first initialized
|
|
||||||
if (Wii.wiimoteConnected) { // Both the Wiimote and the Wii U Pro Controller
|
|
||||||
updateLEDs(); // Turn the LEDs on according to the voltage level
|
|
||||||
ledTimer = millis() - 500; // This will turn the rumble off again after 500ms
|
|
||||||
Wii.setRumbleOn();
|
|
||||||
wiiRumbleEnabled = true;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
#endif // ENABLE_WII
|
|
||||||
|
|
||||||
#ifdef ENABLE_XBOX
|
|
||||||
void onInitXbox() { // This function is called when the controller is first initialized
|
|
||||||
if (Xbox.Xbox360Connected[0]) { // Xbox wireless controller
|
|
||||||
ledTimer = millis() - 500; // This will turn the rumble off again after 500ms
|
|
||||||
Xbox.setRumbleOn(0x00, 0xFF);
|
|
||||||
xboxRumbleDisable = true;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
#endif // ENABLE_XBOX
|
|
||||||
|
|
||||||
#endif // defined(ENABLE_PS3) || defined(ENABLE_PS4) || defined(ENABLE_WII) || defined(ENABLE_XBOX)
|
|
||||||
|
|
||||||
#endif // defined(ENABLE_USB) || defined(ENABLE_SPEKTRUM)
|
|
||||||
|
|
||||||
#if defined(ENABLE_SPP) || defined(ENABLE_PS3) || defined(ENABLE_PS4) || defined(ENABLE_WII) || defined(ENABLE_XBOX) || defined(ENABLE_TOOLS) || defined(ENABLE_SPEKTRUM)
|
|
||||||
void steer(Command command) {
|
|
||||||
commandSent = true; // Used to see if there has already been send a command or not
|
|
||||||
|
|
||||||
steerStop = false;
|
|
||||||
targetOffset = turningOffset = 0; // Set both to 0
|
|
||||||
|
|
||||||
#if defined(ENABLE_SPP) || defined(ENABLE_TOOLS)
|
|
||||||
if (command == joystick) {
|
|
||||||
if (sppData2 > 0) // Forward
|
|
||||||
targetOffset = scale(sppData2, 0, 1, 0, cfg.controlAngleLimit);
|
|
||||||
else if (sppData2 < 0) // Backward
|
|
||||||
targetOffset = -scale(sppData2, 0, -1, 0, cfg.controlAngleLimit);
|
|
||||||
if (sppData1 > 0) // Right
|
|
||||||
turningOffset = scale(sppData1, 0, 1, 0, cfg.turningLimit);
|
|
||||||
else if (sppData1 < 0) // Left
|
|
||||||
turningOffset = -scale(sppData1, 0, -1, 0, cfg.turningLimit);
|
|
||||||
} else if (command == imu) {
|
|
||||||
if (sppData1 > 0) // Forward
|
|
||||||
targetOffset = scale(sppData1, 0, 36, 0, cfg.controlAngleLimit);
|
|
||||||
else if (sppData1 < 0) // Backward
|
|
||||||
targetOffset = -scale(sppData1, 0, -36, 0, cfg.controlAngleLimit);
|
|
||||||
if (sppData2 > 0) // Right
|
|
||||||
turningOffset = scale(sppData2, 0, 45, 0, cfg.turningLimit);
|
|
||||||
else if (sppData2 < 0) // Left
|
|
||||||
turningOffset = -scale(sppData2, 0, -45, 0, cfg.turningLimit);
|
|
||||||
}
|
|
||||||
#endif // ENABLE_SPP or ENABLE_TOOLS
|
|
||||||
#ifdef ENABLE_PS3
|
|
||||||
if (command == updatePS3) {
|
|
||||||
if (PS3.PS3Connected) {
|
|
||||||
if (PS3.getButtonPress(CROSS)) {
|
|
||||||
if (PS3.getAngle(Pitch) > 180) // Forward
|
|
||||||
targetOffset = scale(PS3.getAngle(Pitch), 180, 216, 0, cfg.controlAngleLimit);
|
|
||||||
else if (PS3.getAngle(Pitch) < 180) // Backward
|
|
||||||
targetOffset = -scale(PS3.getAngle(Pitch), 180, 144, 0, cfg.controlAngleLimit);
|
|
||||||
if (PS3.getAngle(Roll) > 180) // Right
|
|
||||||
turningOffset = scale(PS3.getAngle(Roll), 180, 225, 0, cfg.turningLimit);
|
|
||||||
else if (PS3.getAngle(Roll) < 180) // Left
|
|
||||||
turningOffset = -scale(PS3.getAngle(Roll), 180, 135, 0, cfg.turningLimit);
|
|
||||||
} else {
|
|
||||||
if (PS3.getAnalogHat(LeftHatY) < 117 && PS3.getAnalogHat(RightHatY) < 117) // Forward
|
|
||||||
targetOffset = scale(PS3.getAnalogHat(LeftHatY) + PS3.getAnalogHat(RightHatY), 232, 0, 0, cfg.controlAngleLimit); // Scale from 232-0 to 0-controlAngleLimit
|
|
||||||
else if (PS3.getAnalogHat(LeftHatY) > 137 && PS3.getAnalogHat(RightHatY) > 137) // Backward
|
|
||||||
targetOffset = -scale(PS3.getAnalogHat(LeftHatY) + PS3.getAnalogHat(RightHatY), 276, 510, 0, cfg.controlAngleLimit); // Scale from 276-510 to 0-controlAngleLimit
|
|
||||||
if (((int16_t)PS3.getAnalogHat(LeftHatY) - (int16_t)PS3.getAnalogHat(RightHatY)) > 15) // Left
|
|
||||||
turningOffset = -scale(abs((int16_t)PS3.getAnalogHat(LeftHatY) - (int16_t)PS3.getAnalogHat(RightHatY)), 0, 255, 0, cfg.turningLimit); // Scale from 0-255 to 0-turningLimit
|
|
||||||
else if (((int16_t)PS3.getAnalogHat(RightHatY) - (int16_t)PS3.getAnalogHat(LeftHatY)) > 15) // Right
|
|
||||||
turningOffset = scale(abs((int16_t)PS3.getAnalogHat(LeftHatY) - (int16_t)PS3.getAnalogHat(RightHatY)), 0, 255, 0, cfg.turningLimit); // Scale from 0-255 to 0-turningLimit
|
|
||||||
}
|
|
||||||
} else { // It must be a Navigation controller then
|
|
||||||
if (PS3.getAnalogHat(LeftHatY) < 117) // Forward
|
|
||||||
targetOffset = scale(PS3.getAnalogHat(LeftHatY), 116, 0, 0, cfg.controlAngleLimit); // Scale from 116-0 to 0-controlAngleLimit
|
|
||||||
else if (PS3.getAnalogHat(LeftHatY) > 137) // Backward
|
|
||||||
targetOffset = -scale(PS3.getAnalogHat(LeftHatY), 138, 255, 0, cfg.controlAngleLimit); // Scale from 138-255 to 0-controlAngleLimit
|
|
||||||
if (PS3.getAnalogHat(LeftHatX) < 55) // Left
|
|
||||||
turningOffset = -scale(PS3.getAnalogHat(LeftHatX), 54, 0, 0, cfg.turningLimit); // Scale from 54-0 to 0-turningLimit
|
|
||||||
else if (PS3.getAnalogHat(LeftHatX) > 200) // Right
|
|
||||||
turningOffset = scale(PS3.getAnalogHat(LeftHatX), 201, 255, 0, cfg.turningLimit); // Scale from 201-255 to 0-turningLimit
|
|
||||||
}
|
|
||||||
}
|
|
||||||
#endif // ENABLE_PS3
|
|
||||||
#ifdef ENABLE_PS4
|
|
||||||
if (command == updatePS4) {
|
|
||||||
if (PS4.getButtonPress(CROSS)) {
|
|
||||||
if (PS4.getAngle(Pitch) > 180) // Forward
|
|
||||||
targetOffset = scale(PS4.getAngle(Pitch), 180, 216, 0, cfg.controlAngleLimit);
|
|
||||||
else if (PS4.getAngle(Pitch) < 180) // Backward
|
|
||||||
targetOffset = -scale(PS4.getAngle(Pitch), 180, 144, 0, cfg.controlAngleLimit);
|
|
||||||
if (PS4.getAngle(Roll) < 180) // Right
|
|
||||||
turningOffset = scale(PS4.getAngle(Roll), 180, 135, 0, cfg.turningLimit);
|
|
||||||
else if (PS4.getAngle(Roll) > 180) // Left
|
|
||||||
turningOffset = -scale(PS4.getAngle(Roll), 180, 225, 0, cfg.turningLimit);
|
|
||||||
} else {
|
|
||||||
if (PS4.getAnalogHat(LeftHatY) < 117 && PS4.getAnalogHat(RightHatY) < 117) // Forward
|
|
||||||
targetOffset = scale(PS4.getAnalogHat(LeftHatY) + PS4.getAnalogHat(RightHatY), 232, 0, 0, cfg.controlAngleLimit); // Scale from 232-0 to 0-controlAngleLimit
|
|
||||||
else if (PS4.getAnalogHat(LeftHatY) > 137 && PS4.getAnalogHat(RightHatY) > 137) // Backward
|
|
||||||
targetOffset = -scale(PS4.getAnalogHat(LeftHatY) + PS4.getAnalogHat(RightHatY), 276, 510, 0, cfg.controlAngleLimit); // Scale from 276-510 to 0-controlAngleLimit
|
|
||||||
if (((int16_t)PS4.getAnalogHat(LeftHatY) - (int16_t)PS4.getAnalogHat(RightHatY)) > 15) // Left
|
|
||||||
turningOffset = -scale(abs((int16_t)PS4.getAnalogHat(LeftHatY) - (int16_t)PS4.getAnalogHat(RightHatY)), 0, 255, 0, cfg.turningLimit); // Scale from 0-255 to 0-turningLimit
|
|
||||||
else if (((int16_t)PS4.getAnalogHat(RightHatY) - (int16_t)PS4.getAnalogHat(LeftHatY)) > 15) // Right
|
|
||||||
turningOffset = scale(abs((int16_t)PS4.getAnalogHat(LeftHatY) - (int16_t)PS4.getAnalogHat(RightHatY)), 0, 255, 0, cfg.turningLimit); // Scale from 0-255 to 0-turningLimit
|
|
||||||
}
|
|
||||||
}
|
|
||||||
#endif // ENABLE_PS4
|
|
||||||
#ifdef ENABLE_WII
|
|
||||||
if (command == updateWii) {
|
|
||||||
if (!Wii.wiiUProControllerConnected) {
|
|
||||||
if (Wii.getButtonPress(B)) {
|
|
||||||
if (Wii.getPitch() > 180) // Forward
|
|
||||||
targetOffset = scale(Wii.getPitch(), 180, 216, 0, cfg.controlAngleLimit);
|
|
||||||
else if (Wii.getPitch() < 180) // Backward
|
|
||||||
targetOffset = -scale(Wii.getPitch(), 180, 144, 0, cfg.controlAngleLimit);
|
|
||||||
if (Wii.getRoll() > 180) // Right
|
|
||||||
turningOffset = scale(Wii.getRoll(), 180, 225, 0, cfg.turningLimit);
|
|
||||||
else if (Wii.getRoll() < 180) // Left
|
|
||||||
turningOffset = -scale(Wii.getRoll(), 180, 135, 0, cfg.turningLimit);
|
|
||||||
}
|
|
||||||
else { // Read the Nunchuck controller
|
|
||||||
if (Wii.getAnalogHat(HatY) > 137) // Forward
|
|
||||||
targetOffset = scale(Wii.getAnalogHat(HatY), 138, 230, 0, cfg.controlAngleLimit);
|
|
||||||
else if (Wii.getAnalogHat(HatY) < 117) // Backward
|
|
||||||
targetOffset = -scale(Wii.getAnalogHat(HatY), 116, 25, 0, cfg.controlAngleLimit);
|
|
||||||
if (Wii.getAnalogHat(HatX) > 137) // Right
|
|
||||||
turningOffset = scale(Wii.getAnalogHat(HatX), 138, 230, 0, cfg.turningLimit);
|
|
||||||
else if (Wii.getAnalogHat(HatX) < 117) // Left
|
|
||||||
turningOffset = -scale(Wii.getAnalogHat(HatX), 116, 25, 0, cfg.turningLimit);
|
|
||||||
}
|
|
||||||
} else { // It must be a Wii U Pro Controller then
|
|
||||||
if (Wii.getAnalogHat(LeftHatY) > 2200 && Wii.getAnalogHat(RightHatY) > 2200) // Forward
|
|
||||||
targetOffset = scale(Wii.getAnalogHat(LeftHatY) + Wii.getAnalogHat(RightHatY), 4402, 6400, 0, cfg.controlAngleLimit); // Scale from 4402-6400 to 0-controlAngleLimit
|
|
||||||
else if (Wii.getAnalogHat(LeftHatY) < 1800 && Wii.getAnalogHat(RightHatY) < 1800) // Backward
|
|
||||||
targetOffset = -scale(Wii.getAnalogHat(LeftHatY) + Wii.getAnalogHat(RightHatY), 3598, 1600, 0, cfg.controlAngleLimit); // Scale from 3598-1600 to 0-controlAngleLimit
|
|
||||||
if (((int16_t)Wii.getAnalogHat(RightHatY) - (int16_t)Wii.getAnalogHat(LeftHatY)) > 200) // Left
|
|
||||||
turningOffset = -scale(abs((int16_t)Wii.getAnalogHat(LeftHatY) - (int16_t)Wii.getAnalogHat(RightHatY)), 0, 2400, 0, cfg.turningLimit); // Scale from 0-2400 to 0-turningLimit
|
|
||||||
else if (((int16_t)Wii.getAnalogHat(LeftHatY) - (int16_t)Wii.getAnalogHat(RightHatY)) > 200) // Right
|
|
||||||
turningOffset = scale(abs((int16_t)Wii.getAnalogHat(LeftHatY) - (int16_t)Wii.getAnalogHat(RightHatY)), 0, 2400, 0, cfg.turningLimit); // Scale from 0-2400 to 0-turningLimit
|
|
||||||
}
|
|
||||||
}
|
|
||||||
#endif // ENABLE_WII
|
|
||||||
#ifdef ENABLE_XBOX
|
|
||||||
if (command == updateXbox) {
|
|
||||||
if (Xbox.getAnalogHat(LeftHatY) > 7500 && Xbox.getAnalogHat(RightHatY) > 7500) // Forward
|
|
||||||
targetOffset = scale((int32_t)Xbox.getAnalogHat(LeftHatY) + (int32_t)Xbox.getAnalogHat(RightHatY), 15002, 65534, 0, cfg.controlAngleLimit); // Scale from 15002-65534 to 0-controlAngleLimit
|
|
||||||
else if (Xbox.getAnalogHat(LeftHatY) < -7500 && Xbox.getAnalogHat(RightHatY) < -7500) // Backward
|
|
||||||
targetOffset = -scale((int32_t)Xbox.getAnalogHat(LeftHatY) + (int32_t)Xbox.getAnalogHat(RightHatY), -15002, -65536, 0, cfg.controlAngleLimit); // Scale from -15002-(-65536) to 0-controlAngleLimit
|
|
||||||
if (((int32_t)Xbox.getAnalogHat(RightHatY) - (int32_t)Xbox.getAnalogHat(LeftHatY)) > 7500) // Left
|
|
||||||
turningOffset = -scale(abs((int32_t)Xbox.getAnalogHat(LeftHatY) - (int32_t)Xbox.getAnalogHat(RightHatY)), 0, 65535, 0, cfg.turningLimit); // Scale from 0-65535 to 0-turningLimit
|
|
||||||
else if (((int32_t)Xbox.getAnalogHat(LeftHatY) - (int32_t)Xbox.getAnalogHat(RightHatY)) > 7500) // Right
|
|
||||||
turningOffset = scale(abs((int32_t)Xbox.getAnalogHat(LeftHatY) - (int32_t)Xbox.getAnalogHat(RightHatY)), 0, 65535, 0, cfg.turningLimit); // Scale from 0-65535 to 0-turningLimit
|
|
||||||
}
|
|
||||||
#endif // ENABLE_XBOX
|
|
||||||
#ifdef ENABLE_SPEKTRUM
|
|
||||||
if (command == updateSpektrum) {
|
|
||||||
if (rcValue[RC_CHAN_PITCH] > 1500) // Forward
|
|
||||||
targetOffset = scale(rcValue[RC_CHAN_PITCH], 1500, 2000, 0, cfg.controlAngleLimit);
|
|
||||||
else if (rcValue[RC_CHAN_PITCH] < 1500) // Backward
|
|
||||||
targetOffset = -scale(rcValue[RC_CHAN_PITCH], 1500, 1000, 0, cfg.controlAngleLimit);
|
|
||||||
if (rcValue[RC_CHAN_ROLL] < 1500) // Left
|
|
||||||
turningOffset = -scale(rcValue[RC_CHAN_ROLL], 1500, 1000, 0, cfg.turningLimit);
|
|
||||||
else if (rcValue[RC_CHAN_ROLL] > 1500) // Right
|
|
||||||
turningOffset = scale(rcValue[RC_CHAN_ROLL], 1500, 2000, 0, cfg.turningLimit);
|
|
||||||
}
|
|
||||||
#endif // ENABLE_SPEKTRUM
|
|
||||||
|
|
||||||
if (command == stop) {
|
|
||||||
steerStop = true;
|
|
||||||
if (lastCommand != stop) { // Set new stop position
|
|
||||||
targetPosition = getWheelsPosition();
|
|
||||||
stopped = false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
lastCommand = command;
|
|
||||||
}
|
|
||||||
|
|
||||||
float scale(float input, float inputMin, float inputMax, float outputMin, float outputMax) { // Like map() just returns a float
|
|
||||||
float output;
|
|
||||||
if (inputMin < inputMax)
|
|
||||||
output = (input - inputMin) / ((inputMax - inputMin) / (outputMax - outputMin));
|
|
||||||
else
|
|
||||||
output = (inputMin - input) / ((inputMin - inputMax) / (outputMax - outputMin));
|
|
||||||
if (output > outputMax)
|
|
||||||
output = outputMax;
|
|
||||||
else if (output < outputMin)
|
|
||||||
output = outputMin;
|
|
||||||
return output;
|
|
||||||
}
|
|
||||||
#endif // defined(ENABLE_SPP) || defined(ENABLE_PS3) || defined(ENABLE_PS4) || defined(ENABLE_WII) || defined(ENABLE_XBOX) || defined(ENABLE_TOOLS) || defined(ENABLE_SPEKTRUM)
|
|
||||||
400
MasterFile.ino
400
MasterFile.ino
@ -1,400 +0,0 @@
|
|||||||
/* Copyright (C) 2013-2015 Kristian Lauszus, TKJ Electronics. All rights reserved.
|
|
||||||
This software may be distributed and modified under the terms of the GNU
|
|
||||||
General Public License version 2 (GPL2) as published by the Free Software
|
|
||||||
Foundation and appearing in the file GPL2.TXT included in the packaging of
|
|
||||||
this file. Please note that GPL2 Section 2[b] requires that all works based
|
|
||||||
on this software must also be made publicly available under the terms of
|
|
||||||
the GPL2 ("Copyleft").
|
|
||||||
Contact information
|
|
||||||
-------------------
|
|
||||||
Kristian Lauszus, TKJ Electronics
|
|
||||||
Web : http://www.tkjelectronics.dk
|
|
||||||
e-mail : kristianl@tkjelectronics.dk
|
|
||||||
This is the algorithm for the Balanduino balancing robot.
|
|
||||||
It can be controlled by either an Android app or a computer application via Bluetooth.
|
|
||||||
The Android app can be found at the following link: https://github.com/TKJElectronics/BalanduinoAndroidApp
|
|
||||||
The Processing application can be found here: https://github.com/TKJElectronics/BalanduinoProcessingApp
|
|
||||||
A dedicated Windows application can be found here: https://github.com/TKJElectronics/BalanduinoWindowsApp
|
|
||||||
It can also be controlled by a PS3, PS4, Wii or a Xbox controller.
|
|
||||||
Furthermore it supports the Spektrum serial protocol used for RC receivers.
|
|
||||||
For details, see: http://balanduino.net/
|
|
||||||
*/
|
|
||||||
|
|
||||||
/* Use this to enable and disable the different options */
|
|
||||||
#define ENABLE_TOOLS
|
|
||||||
#define ENABLE_SPP
|
|
||||||
#define ENABLE_PS3
|
|
||||||
#define ENABLE_PS4
|
|
||||||
#define ENABLE_WII
|
|
||||||
#define ENABLE_XBOX
|
|
||||||
#define ENABLE_ADK
|
|
||||||
#define ENABLE_SPEKTRUM
|
|
||||||
|
|
||||||
#include "Balanduino.h"
|
|
||||||
#include <Arduino.h> // Standard Arduino header
|
|
||||||
#include <Wire.h> // Official Arduino Wire library
|
|
||||||
#include <SPI.h> // Official Arduino SPI library
|
|
||||||
|
|
||||||
#ifdef ENABLE_ADK
|
|
||||||
#include <adk.h>
|
|
||||||
#endif
|
|
||||||
|
|
||||||
// These are all open source libraries written by Kristian Lauszus, TKJ Electronics
|
|
||||||
// The USB libraries are located at the following link: https://github.com/felis/USB_Host_Shield_2.0
|
|
||||||
#include <Kalman.h> // Kalman filter library - see: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/
|
|
||||||
|
|
||||||
#ifdef ENABLE_XBOX
|
|
||||||
#include <XBOXRECV.h>
|
|
||||||
#endif
|
|
||||||
#ifdef ENABLE_SPP
|
|
||||||
#include <SPP.h>
|
|
||||||
#endif
|
|
||||||
#ifdef ENABLE_PS3
|
|
||||||
#include <PS3BT.h>
|
|
||||||
#endif
|
|
||||||
#ifdef ENABLE_PS4
|
|
||||||
#include <PS4BT.h>
|
|
||||||
#endif
|
|
||||||
#ifdef ENABLE_WII
|
|
||||||
#include <Wii.h>
|
|
||||||
#endif
|
|
||||||
|
|
||||||
// Create the Kalman library instance
|
|
||||||
Kalman kalman; // See https://github.com/TKJElectronics/KalmanFilter for source code
|
|
||||||
|
|
||||||
#if defined(ENABLE_SPP) || defined(ENABLE_PS3) || defined(ENABLE_PS4) || defined(ENABLE_WII) || defined(ENABLE_XBOX) || defined(ENABLE_ADK)
|
|
||||||
#define ENABLE_USB
|
|
||||||
USB Usb; // This will take care of all USB communication
|
|
||||||
#else
|
|
||||||
#define _usb_h_ // Workaround include trap in the USB Host library
|
|
||||||
#include <avrpins.h> // Include this from the USB Host library
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifdef ENABLE_ADK
|
|
||||||
// Implementation for the Android Open Accessory Protocol. Simply connect your phone to get redirected to the Play Store
|
|
||||||
ADK adk(&Usb, "TKJ Electronics", // Manufacturer Name
|
|
||||||
"Balanduino", // Model Name
|
|
||||||
"Android App for Balanduino", // Description - user visible string
|
|
||||||
"0.6.3", // Version of the Android app
|
|
||||||
"https://play.google.com/store/apps/details?id=com.tkjelectronics.balanduino", // URL - web page to visit if no installed apps support the accessory
|
|
||||||
"1234"); // Serial Number - this is not used
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifdef ENABLE_XBOX
|
|
||||||
XBOXRECV Xbox(&Usb); // You have to connect a Xbox wireless receiver to the Arduino to control it with a wireless Xbox controller
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#if defined(ENABLE_SPP) || defined(ENABLE_PS3) || defined(ENABLE_PS4) || defined(ENABLE_WII)
|
|
||||||
#define ENABLE_BTD
|
|
||||||
#include <usbhub.h> // Some dongles can have a hub inside
|
|
||||||
USBHub Hub(&Usb); // Some dongles have a hub inside
|
|
||||||
BTD Btd(&Usb); // This is the main Bluetooth library, it will take care of all the USB and HCI communication with the Bluetooth dongle
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifdef ENABLE_SPP
|
|
||||||
SPP SerialBT(&Btd, "Balanduino", "0000"); // The SPP (Serial Port Protocol) emulates a virtual Serial port, which is supported by most computers and mobile phones
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifdef ENABLE_PS3
|
|
||||||
PS3BT PS3(&Btd); // The PS3 library supports all three official controllers: the Dualshock 3, Navigation and Move controller
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifdef ENABLE_PS4
|
|
||||||
//PS4BT PS4(&Btd, PAIR); // You should create the instance like this if you want to pair with a PS4 controller, then hold PS and Share on the PS4 controller
|
|
||||||
// Or you can simply send "CPP;" to the robot to start the pairing sequence
|
|
||||||
// This can also be done using the Android or via the serial port
|
|
||||||
PS4BT PS4(&Btd); // The PS4BT library supports the PS4 controller via Bluetooth
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifdef ENABLE_WII
|
|
||||||
WII Wii(&Btd); // The Wii library can communicate with Wiimotes and the Nunchuck and Motion Plus extension and finally the Wii U Pro Controller
|
|
||||||
//WII Wii(&Btd,PAIR); // You will have to pair with your Wiimote first by creating the instance like this and the press 1+2 on the Wiimote or press sync if you are using a Wii U Pro Controller
|
|
||||||
// Or you can simply send "CPW;" to the robot to start the pairing sequence
|
|
||||||
// This can also be done using the Android or via the serial port
|
|
||||||
#endif
|
|
||||||
|
|
||||||
void setup() {
|
|
||||||
/* Setup buzzer pin */
|
|
||||||
buzzer::SetDirWrite();
|
|
||||||
|
|
||||||
/* Read the PID values, target angle and other saved values in the EEPROM */
|
|
||||||
if (!checkInitializationFlags()) {
|
|
||||||
readEEPROMValues(); // Only read the EEPROM values if they have not been restored
|
|
||||||
#ifdef ENABLE_SPEKTRUM
|
|
||||||
if (cfg.bindSpektrum) // If flag is set, then bind with Spektrum satellite receiver
|
|
||||||
bindSpektrum();
|
|
||||||
#endif
|
|
||||||
} else { // Indicate that the EEPROM values have been reset by turning on the buzzer
|
|
||||||
buzzer::Set();
|
|
||||||
delay(1000);
|
|
||||||
buzzer::Clear();
|
|
||||||
delay(100); // Wait a little after the pin is cleared
|
|
||||||
}
|
|
||||||
|
|
||||||
/* Initialize UART */
|
|
||||||
Serial.begin(115200);
|
|
||||||
|
|
||||||
/* Setup encoders */
|
|
||||||
leftEncoder1::SetDirRead();
|
|
||||||
leftEncoder2::SetDirRead();
|
|
||||||
rightEncoder1::SetDirRead();
|
|
||||||
rightEncoder2::SetDirRead();
|
|
||||||
leftEncoder1::Set(); // Enable pull-ups
|
|
||||||
leftEncoder2::Set();
|
|
||||||
rightEncoder1::Set();
|
|
||||||
rightEncoder2::Set();
|
|
||||||
|
|
||||||
#if BALANDUINO_REVISION < 13 // On the new revisions pin change interrupt is used for all pins
|
|
||||||
attachInterrupt(digitalPinToInterrupt(leftEncoder1Pin), leftEncoder, CHANGE);
|
|
||||||
attachInterrupt(digitalPinToInterrupt(rightEncoder1Pin), rightEncoder, CHANGE);
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#if defined(PIN_CHANGE_INTERRUPT_VECTOR_LEFT) && defined(PIN_CHANGE_INTERRUPT_VECTOR_RIGHT)
|
|
||||||
/* Enable encoder pins interrupt sources */
|
|
||||||
#if BALANDUINO_REVISION >= 13
|
|
||||||
*digitalPinToPCMSK(leftEncoder1Pin) |= (1 << digitalPinToPCMSKbit(leftEncoder1Pin));
|
|
||||||
*digitalPinToPCMSK(rightEncoder1Pin) |= (1 << digitalPinToPCMSKbit(rightEncoder1Pin));
|
|
||||||
#endif
|
|
||||||
*digitalPinToPCMSK(leftEncoder2Pin) |= (1 << digitalPinToPCMSKbit(leftEncoder2Pin));
|
|
||||||
*digitalPinToPCMSK(rightEncoder2Pin) |= (1 << digitalPinToPCMSKbit(rightEncoder2Pin));
|
|
||||||
|
|
||||||
/* Enable pin change interrupts */
|
|
||||||
#if BALANDUINO_REVISION >= 13
|
|
||||||
*digitalPinToPCICR(leftEncoder1Pin) |= (1 << digitalPinToPCICRbit(leftEncoder1Pin));
|
|
||||||
*digitalPinToPCICR(rightEncoder1Pin) |= (1 << digitalPinToPCICRbit(rightEncoder1Pin));
|
|
||||||
#endif
|
|
||||||
*digitalPinToPCICR(leftEncoder2Pin) |= (1 << digitalPinToPCICRbit(leftEncoder2Pin));
|
|
||||||
*digitalPinToPCICR(rightEncoder2Pin) |= (1 << digitalPinToPCICRbit(rightEncoder2Pin));
|
|
||||||
#elif BALANDUINO_REVISION >= 13
|
|
||||||
#error "Please define "PIN_CHANGE_INTERRUPT_VECTOR_LEFT" and "PIN_CHANGE_INTERRUPT_VECTOR_RIGHT" in Balanduino.h"
|
|
||||||
#endif
|
|
||||||
|
|
||||||
/* Set the motordriver diagnostic pins to inputs */
|
|
||||||
leftDiag::SetDirRead();
|
|
||||||
rightDiag::SetDirRead();
|
|
||||||
|
|
||||||
/* Setup motor pins to output */
|
|
||||||
leftPWM::SetDirWrite();
|
|
||||||
leftA::SetDirWrite();
|
|
||||||
leftB::SetDirWrite();
|
|
||||||
rightPWM::SetDirWrite();
|
|
||||||
rightA::SetDirWrite();
|
|
||||||
rightB::SetDirWrite();
|
|
||||||
|
|
||||||
/* Set PWM frequency to 20kHz - see the datasheet http://www.atmel.com/Images/Atmel-8272-8-bit-AVR-microcontroller-ATmega164A_PA-324A_PA-644A_PA-1284_P_datasheet.pdf page 129-139 */
|
|
||||||
// Set up PWM, Phase and Frequency Correct on pin 18 (OC1A) & pin 17 (OC1B) with ICR1 as TOP using Timer1
|
|
||||||
TCCR1B = (1 << WGM13) | (1 << CS10); // Set PWM Phase and Frequency Correct with ICR1 as TOP and no prescaling
|
|
||||||
ICR1 = PWMVALUE; // ICR1 is the TOP value - this is set so the frequency is equal to 20kHz
|
|
||||||
|
|
||||||
/* Enable PWM on pin 18 (OC1A) & pin 17 (OC1B) */
|
|
||||||
// Clear OC1A/OC1B on compare match when up-counting
|
|
||||||
// Set OC1A/OC1B on compare match when down-counting
|
|
||||||
TCCR1A = (1 << COM1A1) | (1 << COM1B1);
|
|
||||||
|
|
||||||
#ifdef ENABLE_USB
|
|
||||||
if (Usb.Init() == -1) { // Check if USB Host is working
|
|
||||||
Serial.print(F("OSC did not start"));
|
|
||||||
buzzer::Set();
|
|
||||||
while (1); // Halt
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
/* Attach onInit function */
|
|
||||||
// This is used to set the LEDs according to the voltage level and vibrate the controller to indicate the new connection
|
|
||||||
#ifdef ENABLE_PS3
|
|
||||||
PS3.attachOnInit(onInitPS3);
|
|
||||||
#endif
|
|
||||||
#ifdef ENABLE_PS4
|
|
||||||
PS4.attachOnInit(onInitPS4);
|
|
||||||
#endif
|
|
||||||
#ifdef ENABLE_WII
|
|
||||||
Wii.attachOnInit(onInitWii);
|
|
||||||
#endif
|
|
||||||
#ifdef ENABLE_XBOX
|
|
||||||
Xbox.attachOnInit(onInitXbox);
|
|
||||||
#endif
|
|
||||||
|
|
||||||
/* Setup IMU */
|
|
||||||
Wire.begin();
|
|
||||||
#if ARDUINO >= 157
|
|
||||||
Wire.setClock(400000UL); // Set I2C frequency to 400kHz
|
|
||||||
#else
|
|
||||||
TWBR = ((F_CPU / 400000UL) - 16) / 2; // Set I2C frequency to 400kHz
|
|
||||||
#endif
|
|
||||||
|
|
||||||
while (i2cRead(0x75, i2cBuffer, 1));
|
|
||||||
if (i2cBuffer[0] != 0x68) { // Read "WHO_AM_I" register
|
|
||||||
Serial.print(F("Error reading sensor"));
|
|
||||||
buzzer::Set();
|
|
||||||
while (1); // Halt
|
|
||||||
}
|
|
||||||
|
|
||||||
while (i2cWrite(0x6B, 0x80, true)); // Reset device, this resets all internal registers to their default values
|
|
||||||
do {
|
|
||||||
while (i2cRead(0x6B, i2cBuffer, 1));
|
|
||||||
} while (i2cBuffer[0] & 0x80); // Wait for the bit to clear
|
|
||||||
delay(5);
|
|
||||||
while (i2cWrite(0x6B, 0x09, true)); // PLL with X axis gyroscope reference, disable temperature sensor and disable sleep mode
|
|
||||||
#if 1
|
|
||||||
i2cBuffer[0] = 1; // Set the sample rate to 500Hz - 1kHz/(1+1) = 500Hz
|
|
||||||
i2cBuffer[1] = 0x03; // Disable FSYNC and set 44 Hz Acc filtering, 42 Hz Gyro filtering, 1 KHz sampling
|
|
||||||
#else
|
|
||||||
i2cBuffer[0] = 15; // Set the sample rate to 500Hz - 8kHz/(15+1) = 500Hz
|
|
||||||
i2cBuffer[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
|
|
||||||
#endif
|
|
||||||
i2cBuffer[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
|
|
||||||
i2cBuffer[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
|
|
||||||
while (i2cWrite(0x19, i2cBuffer, 4, true)); // Write to all four registers at once
|
|
||||||
|
|
||||||
delay(100); // Wait for the sensor to get ready
|
|
||||||
|
|
||||||
/* Set Kalman and gyro starting angle */
|
|
||||||
while (i2cRead(0x3D, i2cBuffer, 4));
|
|
||||||
int16_t accY = ((i2cBuffer[0] << 8) | i2cBuffer[1]);
|
|
||||||
int16_t accZ = ((i2cBuffer[2] << 8) | i2cBuffer[3]);
|
|
||||||
// atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
|
|
||||||
// We then convert it to 0 to 2π and then from radians to degrees
|
|
||||||
accAngle = (atan2((float)accY - cfg.accYzero, (float)accZ - cfg.accZzero) + PI) * RAD_TO_DEG;
|
|
||||||
|
|
||||||
kalman.setAngle(accAngle); // Set starting angle
|
|
||||||
pitch = accAngle;
|
|
||||||
gyroAngle = accAngle;
|
|
||||||
|
|
||||||
/* Calibrate gyro zero value */
|
|
||||||
while (calibrateGyro()); // Run again if the robot is moved while calibrating
|
|
||||||
|
|
||||||
LED::SetDirWrite(); // Set LED pin to output
|
|
||||||
stopAndReset(); // Turn off motors and reset different values
|
|
||||||
|
|
||||||
#ifdef ENABLE_TOOLS
|
|
||||||
printMenu();
|
|
||||||
#endif
|
|
||||||
|
|
||||||
/* Beep to indicate that it is now ready */
|
|
||||||
buzzer::Set();
|
|
||||||
delay(100);
|
|
||||||
buzzer::Clear();
|
|
||||||
|
|
||||||
/* Setup timing */
|
|
||||||
kalmanTimer = micros();
|
|
||||||
pidTimer = kalmanTimer;
|
|
||||||
imuTimer = millis();
|
|
||||||
encoderTimer = imuTimer;
|
|
||||||
reportTimer = imuTimer;
|
|
||||||
ledTimer = imuTimer;
|
|
||||||
blinkTimer = imuTimer;
|
|
||||||
}
|
|
||||||
|
|
||||||
void loop() {
|
|
||||||
if (!leftDiag::IsSet() || !rightDiag::IsSet()) { // Motor driver will pull these low on error
|
|
||||||
buzzer::Set();
|
|
||||||
stopMotor(left);
|
|
||||||
stopMotor(right);
|
|
||||||
while (1);
|
|
||||||
}
|
|
||||||
|
|
||||||
#if defined(ENABLE_WII) || defined(ENABLE_PS4) // We have to read much more often from the Wiimote and PS4 controller to decrease latency
|
|
||||||
bool readUSB = false;
|
|
||||||
#ifdef ENABLE_WII
|
|
||||||
if (Wii.wiimoteConnected)
|
|
||||||
readUSB = true;
|
|
||||||
#endif
|
|
||||||
#ifdef ENABLE_PS4
|
|
||||||
if (PS4.connected())
|
|
||||||
readUSB = true;
|
|
||||||
#endif
|
|
||||||
if (readUSB)
|
|
||||||
Usb.Task();
|
|
||||||
#endif
|
|
||||||
|
|
||||||
/* Calculate pitch */
|
|
||||||
while (i2cRead(0x3D, i2cBuffer, 8));
|
|
||||||
int16_t accY = ((i2cBuffer[0] << 8) | i2cBuffer[1]);
|
|
||||||
int16_t accZ = ((i2cBuffer[2] << 8) | i2cBuffer[3]);
|
|
||||||
int16_t gyroX = ((i2cBuffer[6] << 8) | i2cBuffer[7]);
|
|
||||||
|
|
||||||
// atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
|
|
||||||
// We then convert it to 0 to 2π and then from radians to degrees
|
|
||||||
accAngle = (atan2((float)accY - cfg.accYzero, (float)accZ - cfg.accZzero) + PI) * RAD_TO_DEG;
|
|
||||||
|
|
||||||
uint32_t timer = micros();
|
|
||||||
// This fixes the 0-360 transition problem when the accelerometer angle jumps between 0 and 360 degrees
|
|
||||||
if ((accAngle < 90 && pitch > 270) || (accAngle > 270 && pitch < 90)) {
|
|
||||||
kalman.setAngle(accAngle);
|
|
||||||
pitch = accAngle;
|
|
||||||
gyroAngle = accAngle;
|
|
||||||
} else {
|
|
||||||
float gyroRate = ((float)gyroX - gyroXzero) / 131.0f; // Convert to deg/s
|
|
||||||
float dt = (float)(timer - kalmanTimer) / 1000000.0f;
|
|
||||||
gyroAngle += gyroRate * dt; // Gyro angle is only used for debugging
|
|
||||||
if (gyroAngle < 0 || gyroAngle > 360)
|
|
||||||
gyroAngle = pitch; // Reset the gyro angle when it has drifted too much
|
|
||||||
pitch = kalman.getAngle(accAngle, gyroRate, dt); // Calculate the angle using a Kalman filter
|
|
||||||
}
|
|
||||||
kalmanTimer = timer;
|
|
||||||
//Serial.print(accAngle);Serial.print('\t');Serial.print(gyroAngle);Serial.print('\t');Serial.println(pitch);
|
|
||||||
|
|
||||||
#if defined(ENABLE_WII) || defined(ENABLE_PS4) // We have to read much more often from the Wiimote and PS4 controller to decrease latency
|
|
||||||
if (readUSB)
|
|
||||||
Usb.Task();
|
|
||||||
#endif
|
|
||||||
|
|
||||||
/* Drive motors */
|
|
||||||
timer = micros();
|
|
||||||
// If the robot is laying down, it has to be put in a vertical position before it starts balancing
|
|
||||||
// If it's already balancing it has to be ±45 degrees before it stops trying to balance
|
|
||||||
if ((layingDown && (pitch < cfg.targetAngle - 10 || pitch > cfg.targetAngle + 10)) || (!layingDown && (pitch < cfg.targetAngle - 45 || pitch > cfg.targetAngle + 45))) {
|
|
||||||
layingDown = true; // The robot is in a unsolvable position, so turn off both motors and wait until it's vertical again
|
|
||||||
stopAndReset();
|
|
||||||
} else {
|
|
||||||
layingDown = false; // It's no longer laying down
|
|
||||||
updatePID(cfg.targetAngle, targetOffset, turningOffset, (float)(timer - pidTimer) / 1000000.0f);
|
|
||||||
}
|
|
||||||
pidTimer = timer;
|
|
||||||
|
|
||||||
/* Update encoders */
|
|
||||||
timer = millis();
|
|
||||||
if (timer - encoderTimer >= 100) { // Update encoder values every 100ms
|
|
||||||
encoderTimer = timer;
|
|
||||||
int32_t wheelPosition = getWheelsPosition();
|
|
||||||
wheelVelocity = wheelPosition - lastWheelPosition;
|
|
||||||
lastWheelPosition = wheelPosition;
|
|
||||||
//Serial.print(wheelPosition);Serial.print('\t');Serial.print(targetPosition);Serial.print('\t');Serial.println(wheelVelocity);
|
|
||||||
if (abs(wheelVelocity) <= 40 && !stopped) { // Set new targetPosition if braking
|
|
||||||
targetPosition = wheelPosition;
|
|
||||||
stopped = true;
|
|
||||||
}
|
|
||||||
|
|
||||||
batteryCounter++;
|
|
||||||
if (batteryCounter >= 10) { // Measure battery every 1s
|
|
||||||
batteryCounter = 0;
|
|
||||||
batteryVoltage = (float)analogRead(VBAT) / 63.050847458f; // VBAT is connected to analog input 5 which is not broken out. This is then connected to a 47k-12k voltage divider - 1023.0/(3.3/(12.0/(12.0+47.0))) = 63.050847458
|
|
||||||
if (batteryVoltage < 10.2 && batteryVoltage > 5) // Equal to 3.4V per cell - don't turn on if it's below 5V, this means that no battery is connected
|
|
||||||
buzzer::Set();
|
|
||||||
else
|
|
||||||
buzzer::Clear();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/* Read the Bluetooth dongle and send PID and IMU values */
|
|
||||||
#if defined(ENABLE_TOOLS) || defined(ENABLE_SPEKTRUM)
|
|
||||||
checkSerialData();
|
|
||||||
#endif
|
|
||||||
#if defined(ENABLE_USB) || defined(ENABLE_SPEKTRUM)
|
|
||||||
readUsb();
|
|
||||||
#endif
|
|
||||||
#if defined(ENABLE_TOOLS) || defined(ENABLE_SPP)
|
|
||||||
printValues();
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifdef ENABLE_BTD
|
|
||||||
if (Btd.isReady()) {
|
|
||||||
timer = millis();
|
|
||||||
if ((Btd.watingForConnection && timer - blinkTimer > 1000) || (!Btd.watingForConnection && timer - blinkTimer > 100)) {
|
|
||||||
blinkTimer = timer;
|
|
||||||
LED::Toggle(); // Used to blink the built in LED, starts blinking faster upon an incoming Bluetooth request
|
|
||||||
}
|
|
||||||
} else
|
|
||||||
LED::Clear(); // This will turn it off
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
184
Motors.ino
184
Motors.ino
@ -1,184 +0,0 @@
|
|||||||
/* Copyright (C) 2013-2015 Kristian Lauszus, TKJ Electronics. All rights reserved.
|
|
||||||
This software may be distributed and modified under the terms of the GNU
|
|
||||||
General Public License version 2 (GPL2) as published by the Free Software
|
|
||||||
Foundation and appearing in the file GPL2.TXT included in the packaging of
|
|
||||||
this file. Please note that GPL2 Section 2[b] requires that all works based
|
|
||||||
on this software must also be made publicly available under the terms of
|
|
||||||
the GPL2 ("Copyleft").
|
|
||||||
Contact information
|
|
||||||
-------------------
|
|
||||||
Kristian Lauszus, TKJ Electronics
|
|
||||||
Web : http://www.tkjelectronics.dk
|
|
||||||
e-mail : kristianl@tkjelectronics.dk
|
|
||||||
*/
|
|
||||||
|
|
||||||
void updatePID(float restAngle, float offset, float turning, float dt) {
|
|
||||||
/* Brake */
|
|
||||||
if (steerStop) {
|
|
||||||
int32_t wheelPosition = getWheelsPosition();
|
|
||||||
int32_t positionError = wheelPosition - targetPosition;
|
|
||||||
if (cfg.backToSpot) {
|
|
||||||
if (abs(positionError) > zoneA) // Inside zone A
|
|
||||||
restAngle -= (float)positionError / positionScaleA;
|
|
||||||
else if (abs(positionError) > zoneB) // Inside zone B
|
|
||||||
restAngle -= (float)positionError / positionScaleB;
|
|
||||||
else if (abs(positionError) > zoneC) // Inside zone C
|
|
||||||
restAngle -= (float)positionError / positionScaleC;
|
|
||||||
else // Inside zone D
|
|
||||||
restAngle -= (float)positionError / positionScaleD;
|
|
||||||
} else {
|
|
||||||
if (abs(positionError) < zoneC)
|
|
||||||
restAngle -= (float)positionError / positionScaleD;
|
|
||||||
else
|
|
||||||
targetPosition = wheelPosition;
|
|
||||||
}
|
|
||||||
restAngle -= (float)wheelVelocity / velocityScaleStop;
|
|
||||||
|
|
||||||
restAngle = constrain(restAngle, cfg.targetAngle - 10, cfg.targetAngle + 10); // Limit rest Angle
|
|
||||||
}
|
|
||||||
/* Drive forward and backward */
|
|
||||||
else {
|
|
||||||
if ((offset > 0 && wheelVelocity < 0) || (offset < 0 && wheelVelocity > 0) || offset == 0) // Scale down offset at high speed - wheel velocity is negative when driving forward and positive when driving backward
|
|
||||||
offset += (float)wheelVelocity / velocityScaleMove; // We will always compensate if the offset is 0, but steerStop is not set
|
|
||||||
restAngle -= offset;
|
|
||||||
}
|
|
||||||
|
|
||||||
restAngle = constrain(restAngle, lastRestAngle - 1, lastRestAngle + 1); // Don't change restAngle with more than 1 degree in each loop
|
|
||||||
lastRestAngle = restAngle;
|
|
||||||
|
|
||||||
/* Update PID values */
|
|
||||||
float error = restAngle - pitch;
|
|
||||||
float pTerm = cfg.P * error;
|
|
||||||
iTerm += cfg.I * 100.0f * error * dt; // Multiplication with Ki is done before integration limit, to make it independent from integration limit value
|
|
||||||
iTerm = constrain(iTerm, -100.0f, 100.0f); // Limit the integrated error - prevents windup
|
|
||||||
float dTerm = (cfg.D / 100.0f) * (error - lastError) / dt;
|
|
||||||
lastError = error;
|
|
||||||
float PIDValue = pTerm + iTerm + dTerm;
|
|
||||||
|
|
||||||
/* Steer robot sideways */
|
|
||||||
if (turning < 0) { // Left
|
|
||||||
turning += abs((float)wheelVelocity / velocityScaleTurning); // Scale down at high speed
|
|
||||||
if (turning > 0)
|
|
||||||
turning = 0;
|
|
||||||
}
|
|
||||||
else if (turning > 0) { // Right
|
|
||||||
turning -= abs((float)wheelVelocity / velocityScaleTurning); // Scale down at high speed
|
|
||||||
if (turning < 0)
|
|
||||||
turning = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
float PIDLeft = PIDValue + turning;
|
|
||||||
float PIDRight = PIDValue - turning;
|
|
||||||
|
|
||||||
PIDLeft *= cfg.leftMotorScaler; // Compensate for difference in some of the motors
|
|
||||||
PIDRight *= cfg.rightMotorScaler;
|
|
||||||
|
|
||||||
/* Set PWM Values */
|
|
||||||
if (PIDLeft >= 0)
|
|
||||||
moveMotor(left, forward, PIDLeft);
|
|
||||||
else
|
|
||||||
moveMotor(left, backward, -PIDLeft);
|
|
||||||
if (PIDRight >= 0)
|
|
||||||
moveMotor(right, forward, PIDRight);
|
|
||||||
else
|
|
||||||
moveMotor(right, backward, -PIDRight);
|
|
||||||
}
|
|
||||||
|
|
||||||
void moveMotor(Command motor, Command direction, float speedRaw) { // Speed is a value in percentage 0-100%
|
|
||||||
if (speedRaw > 100.0f)
|
|
||||||
speedRaw = 100.0f;
|
|
||||||
setPWM(motor, speedRaw * (float)PWMVALUE / 100.0f); // Scale from 0-100 to 0-PWMVALUE
|
|
||||||
if (motor == left) {
|
|
||||||
if (direction == forward) {
|
|
||||||
leftA::Clear();
|
|
||||||
leftB::Set();
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
leftA::Set();
|
|
||||||
leftB::Clear();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
if (direction == forward) {
|
|
||||||
rightA::Set();
|
|
||||||
rightB::Clear();
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
rightA::Clear();
|
|
||||||
rightB::Set();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void stopMotor(Command motor) {
|
|
||||||
setPWM(motor, PWMVALUE); // Set high
|
|
||||||
if (motor == left) {
|
|
||||||
leftA::Set();
|
|
||||||
leftB::Set();
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
rightA::Set();
|
|
||||||
rightB::Set();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void setPWM(Command motor, uint16_t dutyCycle) { // dutyCycle is a value between 0-ICR1
|
|
||||||
if (motor == left)
|
|
||||||
OCR1A = dutyCycle;
|
|
||||||
else
|
|
||||||
OCR1B = dutyCycle;
|
|
||||||
}
|
|
||||||
|
|
||||||
void stopAndReset() {
|
|
||||||
stopMotor(left);
|
|
||||||
stopMotor(right);
|
|
||||||
lastError = 0;
|
|
||||||
iTerm = 0;
|
|
||||||
targetPosition = getWheelsPosition();
|
|
||||||
lastRestAngle = cfg.targetAngle;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* Interrupt routine and encoder read functions */
|
|
||||||
// It uses gray code to detect if any pulses are missed. See: https://www.circuitsathome.com/mcu/reading-rotary-encoder-on-arduino and http://en.wikipedia.org/wiki/Rotary_encoder#Incremental_rotary_encoder.
|
|
||||||
|
|
||||||
#if defined(PIN_CHANGE_INTERRUPT_VECTOR_LEFT) && defined(PIN_CHANGE_INTERRUPT_VECTOR_RIGHT)
|
|
||||||
static const int8_t enc_states[16] = { 0, -1, 1, 0, 1, 0, 0, -1, -1, 0, 0, 1, 0, 1, -1, 0 }; // Encoder lookup table if it interrupts on every edge
|
|
||||||
|
|
||||||
ISR(PIN_CHANGE_INTERRUPT_VECTOR_LEFT) {
|
|
||||||
leftEncoder();
|
|
||||||
#if BALANDUINO_REVISION >= 13
|
|
||||||
}
|
|
||||||
ISR(PIN_CHANGE_INTERRUPT_VECTOR_RIGHT) {
|
|
||||||
#endif
|
|
||||||
rightEncoder();
|
|
||||||
}
|
|
||||||
#elif BALANDUINO_REVISION < 13
|
|
||||||
#warning "Only interrupting on every second edge!"
|
|
||||||
static const int8_t enc_states[16] = { 0, 0, 0, -1, 0, 0, 1, 0, 0, 1, 0, 0, -1, 0, 0, 0 }; // Encoder lookup table if it only interrupts on every second edge - this only works on revision 1.2 and older
|
|
||||||
#endif
|
|
||||||
|
|
||||||
void leftEncoder() {
|
|
||||||
static uint8_t old_AB = 0;
|
|
||||||
old_AB <<= 2; // Remember previous state
|
|
||||||
old_AB |= (leftEncoder2::IsSet() >> (leftEncoder2::Number - 1)) | (leftEncoder1::IsSet() >> leftEncoder1::Number);
|
|
||||||
leftCounter -= enc_states[ old_AB & 0x0F ];
|
|
||||||
}
|
|
||||||
|
|
||||||
void rightEncoder() {
|
|
||||||
static uint8_t old_AB = 0;
|
|
||||||
old_AB <<= 2; // Remember previous state
|
|
||||||
old_AB |= (rightEncoder2::IsSet() >> (rightEncoder2::Number - 1)) | (rightEncoder1::IsSet() >> rightEncoder1::Number);
|
|
||||||
rightCounter += enc_states[ old_AB & 0x0F ];
|
|
||||||
}
|
|
||||||
|
|
||||||
int32_t readLeftEncoder() { // The encoders decrease when motors are traveling forward and increase when traveling backward
|
|
||||||
return leftCounter;
|
|
||||||
}
|
|
||||||
|
|
||||||
int32_t readRightEncoder() {
|
|
||||||
return rightCounter;
|
|
||||||
}
|
|
||||||
|
|
||||||
int32_t getWheelsPosition() {
|
|
||||||
return leftCounter + rightCounter;
|
|
||||||
}
|
|
||||||
Loading…
Reference in New Issue
Block a user