From 8b662ea652ad13c78a85a431bc2d29bc534260ab Mon Sep 17 00:00:00 2001 From: Thefeli73 Date: Tue, 13 Apr 2021 20:10:16 +0200 Subject: [PATCH] remove old stuff --- Bluetooth/Bluetooth.ino | 505 ---------------------------------------- MasterFile.ino | 400 ------------------------------- Motors.ino | 184 --------------- 3 files changed, 1089 deletions(-) delete mode 100644 Bluetooth/Bluetooth.ino delete mode 100644 MasterFile.ino delete mode 100644 Motors.ino diff --git a/Bluetooth/Bluetooth.ino b/Bluetooth/Bluetooth.ino deleted file mode 100644 index 4f7fcda..0000000 --- a/Bluetooth/Bluetooth.ino +++ /dev/null @@ -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) diff --git a/MasterFile.ino b/MasterFile.ino deleted file mode 100644 index 55986c8..0000000 --- a/MasterFile.ino +++ /dev/null @@ -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 // Standard Arduino header -#include // Official Arduino Wire library -#include // Official Arduino SPI library - -#ifdef ENABLE_ADK -#include -#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 filter library - see: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/ - -#ifdef ENABLE_XBOX -#include -#endif -#ifdef ENABLE_SPP -#include -#endif -#ifdef ENABLE_PS3 -#include -#endif -#ifdef ENABLE_PS4 -#include -#endif -#ifdef ENABLE_WII -#include -#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 // 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 // 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 -} diff --git a/Motors.ino b/Motors.ino deleted file mode 100644 index cfa864c..0000000 --- a/Motors.ino +++ /dev/null @@ -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; -}