Showing posts with label app. Show all posts
Showing posts with label app. Show all posts

Monday, August 19, 2019

Tello Drone, Swift and State Machines (Part 2)

Stop Rolling Your Own State Machine Code



FIGURE 1. The Flight Plan iOS App


This sub heading is aimed at me to serve as a reminder to stop reinventing the wheel! A lot of the apps that I write benefit from having a Finite State Machine computational model. In an earlier article I wrote about controlling the Tello drone remotely using Swift. If you have a look at this code you will see that I have implemented a simple state machine to track the state of the drone. You need this because you can't send the drone a command unless WiFi is connected and the drone is in command mode (activated by sending it the "command" string via UDP). Thus our app responds differently depending on what state the drone is in.

I felt justified in writing my own state machine code because the initial application was relatively simple. If I was writing a game then I would always use GKStateMachine, the state machine class provided by Apple as part of game kit, but because this is a utility and I was in the UIKit headspace as opposed to the SceneKit/GameplayKit space I didn't think about it. But there is no reason you can't use GameplayKit classes in your UIKit app and in retrospect that is what I should have done (and just spent a day refactoring my code to do). Insert face palm emoji here!

I have continued to add functionality to my drone control app and as the complexity increased my home grown state machine started to become part of the problem and not the solution. Due to the organic development process (i.e. unstructured), I ended up with two state machines which were not scaling well. More importantly the app was acting weird and ending up in undefined states. Of course I could have fixed this in time, but I realised that Apple have already spent a lot of time putting together a robust state machine class and I should be using that!

FIGURE 2. Drone State Machine Diagram

The collateral benefit of having to refactor my code using GKStateMachine was that it made me sit down and plan out what states I needed and what would cause a transition. In other words I needed to develop a state transition table or diagram (Figure 2). After doing this exercise it became apparent that I didn't need two state machines, I just needed to add two states to the original machine. In addition, being forced to come up with the table made me think about some states and/or transitions that I wasn't handling.

TL;DR - Use GKStateMachine even for simple applications!

To demonstrate how easy it is, I will include the boiler plate code for my drone app.

STEP 1 - Create the state classes


For every state in your FSM you need a class to handle transitions, etc. Typically you will need to override the functions shown. I have included the outline for the disconnected state class below. The other state classes have exactly the same format but with different names.

//
//  DisconnectedState.swift
//  FlightPlan
//
//  Created by David Such on 18/8/19.
//  Copyright © 2019 Kintarla Pty Ltd. All rights reserved.
//

import Foundation
import GameplayKit

class DisconnectedState: GKState {
    unowned let viewController: ViewController
    
    init(viewController: ViewController) {
        self.viewController = viewController
        super.init()
    }
    
    override func didEnter(from previousState: GKState?) {
        viewController.statusLabel.text = "DISC"
        viewController.WiFiImageView.image = UIImage(named: "WiFiDisconnected")
        
        if !UserDefaults.standard.warningShown {
            viewController.showAlert(title: "Not Connected to Tello WiFi", msg: "In order to control the Tello you must be connected to its WiFi network. Turn on the Tello and then go to Settings -> WiFi to connect.")
            UserDefaults.standard.warningShown = true
        }
    }
    
    override func willExit(to nextState: GKState) {
        
    }
    
    override func isValidNextState(_ stateClass: AnyClass) -> Bool {
        return (stateClass == WiFiUpState.self) || (stateClass == PlanningState.self)
    }
    
    override func update(deltaTime seconds: TimeInterval) {
        
    }

}

A couple of points. Firstly, make sure that you import GameplayKit. Second, note the constant definition:

unowned let viewController: ViewController

In my app this is the main view controller which contains the UI and will never be NIL. To prevent a retain cycle we use unowned (and not weak since that view controller can never be NIL).

This constant is used to update the UI based on state changes (alternatively you could use a delegate).

STEP 2 - Define the State Machine


Next, within the view controller referred to in step 1, you need to define your state machine.

//
//  ViewController.swift
//  FlightPlan
//
//  Created by David Such on 3/6/19.
//  Copyright © 2019 Kintarla Pty Ltd. All rights reserved.
//

import UIKit
import GameplayKit

class ViewController: UIViewController {
    
    lazy var stateMachine: GKStateMachine = GKStateMachine(states: [
        DisconnectedState(viewController: self),
        WiFiUpState(viewController: self),
        CommandState(viewController: self),
        PlanningState(viewController: self),
        ManualState(viewController: self),
        AutoPilotState(viewController: self)
        ])

As shown above, this is very straight forward. A lazy stored property is a property whose initial value is not calculated until the first time it is used. You indicate a lazy stored property by writing the lazy modifier before its declaration. We need this so that we can assign a pointer to the class containing our state machine (i.e. viewController which is an instance of ViewController) after it has been initialised.

STEP 3 - Use the State Machine


Now we can use our new state machine to keep track of the drone state and handle transitions between states. The first thing you will want to do is to set the initial state. For our drone this is the disconnected state.

stateMachine.enter(DisconnectedState.self)

You will probably do this in the viewDidLoad method of viewController. Then you can change states when the appropriate event is triggered. For example, the following method is called when the take off button is tapped.

@IBAction func takeOffTapped(_ sender: UIButton) {
        switch stateMachine.currentState {
        case is DisconnectedState:
            showAlert(title: "Not Connected to Tello WiFi", msg: "In order to control the Tello you must be connected to its WiFi network. Turn on the Tello and then go to Settings -> WiFi to connect.")
        case is WiFiUpState:
            showAlert(title: "Awaiting CMD Response", msg: "We haven't received a valid response to our initialisation command. Try sending again from Setup.")
        case is CommandState:
            tello.takeOff()
            stateMachine.enter(ManualState.self)
        case is PlanningState:
            if tello.flightPlan.count == 0 {
                let zoom = scrollView.zoomScale - 0.25
                let pitch = dronePointer.frame.size.height
                
                tello.flightPlan.append(CMD.takeOff)
                dronePointerCenter.y -= pitch
                UIView.animate(withDuration: 0.5, delay: 0, options: .curveEaseInOut, animations: {self.dronePointer.center = self.dronePointerCenter}, completion: nil)
                scrollView.setZoomScale(zoom, animated: true)
            }
        default:
            break
        }

    }

Depending on the current drone state (stateMachine.currentState) we want to perform different actions. To take off manually, we need to be in the command state. In the planning state, we add the take off command to our flight plan, and animate the action on our viewController.

One last tip. In the example above we are using switch for program control to handle the various states. If you want to check the current state against only one state don't use "==". It wont compile. You need to use "is" instead. For example, to check if the current state is Auto Pilot, you would use:

if stateMachine.currentState is AutoPilotState {
            tello.stopAutoPilot()
}

That's it. Next time you need a state machine, don't write your own! Hop on over to GameplayKit and grab GKStateMachine.

Saturday, June 8, 2019

Programming the Tello Drone using Swift (Part 1)

The Tello Drone




In this article we will explore how to write a simple iOS app in Swift to allow control of the Tello.

Tello is a mini drone equipped with a HD camera that is manufactured by Ryze Robotics and includes a flight controller with DJI smarts. It is a great drone to learn to fly on as you can use it indoors and because it is so light (80 grams), crashing is fairly painless if you have the prop guards on. I have crashed mine (a lot) and the worst that has happened is that a propeller came off, which is easy to replace. It is also relatively inexpensive. You can manually control it using either an app (iOS or Android) on your phone, or a combination of the app and a dedicated Bluetooth remote. Either works fine. If you do get the Bluetooth remote be careful of not moving out of Bluetooth range of your phone while you are flying the drone.

Tello Specifications


Tello is Powered by a DJIGlobal flight control system and an Intel processor (Movidius MA2x chipset). The MA2x is based on a SARC LEON processor which has two RISC CPUs to run the RTOS, firmware, and runtime scheduler (Ref: RyzeTelloFirmware). The other specifications are:

  • Weight: Approximately 80 g (Propellers and Battery Included)
  • Dimensions: 98×92.5×41 mm
  • Propeller: 3 inches
  • Built-in Functions: Range Finder, Barometer, LED, Vision System, 2.4 GHz 802.11n Wi-Fi, 720p Live View
  • Port: Micro USB Charging Port
  • Max Flight Distance: 100m
  • Max Speed: 8m/s
  • Max Flight Time: 13min
  • Max Flight Height: 30m

Programming - Firmware Versions


Apart from being a good platform to earn your flying chops, the best thing about the Tello from my perspective is that you can write a script or a program to control the drone remotely. This opens up a lot of possibilities.

Note that there are three different Tello's that you can buy (the Tello, the newer Tello EDU and the Ironman Edition), and they use slightly different API's. So make sure that you use the appropriate version for your drone.

You can work out which firmware you have by connecting your mobile to the Tello WiFi, opening the Tello app, tapping on settings (the gear icon), then tap on the More button, and finally tap on the "..." button to the left of the screen. This should bring up the screen shown below which includes the firmware and app version numbers. My Tello is running firmware version 1.03.33.01. You can download the relevant SDK document for this version.



The Tello EDU uses version 2.0 of the SDK. You can download a PDF of the V2 SDK from here.

Commands that are available in SDK v1.3 but not v2.0 are:

  • height?
  • temp?
  • attitude?
  • baro?
  • acceleration?
  • tof?

Conversely, commands that are available in SDK v2.0 but not v1.3 are:

  • stop (hover)
  • go x y z speed mid (same as go x y z speed but uses the mission pad)
  • curve x1 y1 z1 x2 y2 z2 speed mid (same as curve x1 y1 z1 x2 y2 z2 speed but uses the mission pad)
  • jump x y z speed yaw mid1 mid2 (Fly to coordinates x, y and z of mission pad 1 and recognize coordinates 0, 0 and z of mission pad 2 and rotate to the yaw value)
  • mon
  • moff
  • mdirection
  • ap ssid pass
  • sdk?
  • sn?

The Tello EDU also has a swarm mode if you want to control a bunch of drones.

Programming - Python


There are plenty of examples on how to use Python to control your Tello. For drones running v1.3 have a look at the DroneBlocks code. For the Tello EDU (i.e. v2.0 SDK), Ryze Robotics provide some sample code for you to download and try out.

I uploaded the DroneBlocks code using my Raspberry Pi connected to the Tello WiFi and it worked a treat. Given that there are lots of Python examples, I thought I would put together something in Swift and work up to an app which provides additional functionality not found in the official Tello app.

Programming - Swift (iOS)


We access the Tello API by connecting to the airframe via a WiFi UDP port. Once a connection is in place, the drone is controlled using simple text commands.



The first thing we want to determine is whether our device is connected to the Tello WiFi. There are a couple of Swift functions which can assist with establishing this. The Tello SSID name contains the string "TELLO" (see image above), so this is what we will use to determine wether we are connected to the correct WiFi network.


We can use the code above in our ViewController to ensure that we are hooked up to the Tello, and if not provide an alert. The screenshot below shows this implemented in my proof of concept app.


The code for the ViewController is shown next. It should be fairly self explanatory.



UDP


UDP (User Datagram Protocol) is a communications protocol, similar to Transmission Control Protocol (TCP), but used primarily for establishing low-latency, low-bandwidth and loss-tolerating connections. UDP sends messages, called datagrams, and is considered a best-effort mode of communications. With UDP there is no checking and resending of lost messages (unlike TCP).

Both UDP and TCP run on top of the Internet Protocol (IP) and are sometimes referred to as UDP/IP or TCP/IP.

UDP provides two services not provided by the IP layer. It provides port numbers to help distinguish different user requests and, optionally, a checksum capability to verify that the data arrived intact.

The Tello IP address is 192.168.10.1. The UDP Services available are:

UDP PORT: 8889 - Send command and receive a response.
UDP SERVER: 0.0.0.0 UDP PORT: 8890 - Receive Tello state.
UDP SERVER: 0.0.0.0 UDP PORT: 11111 - Receive Tello video stream.

If you want to send and receive via UDP on iOS then the two main libraries in use appear to be SwiftSocket and GCDAsyncUDPSocket.

Swift Socket looks to be the simpler of the two libraries, so I used that for my initial attempt. I put together a Tello Swift class to do the heavy lifting. It is reproduced below and works as advertised. You will need to put together your own UI but if you hook up the relevant buttons in the View Controller then you shouldn't have any problem reproducing what I have done.

I will add a bit more functionality to the app (e.g. video) and then stick it up on the app store for download.



Wednesday, January 20, 2016

Bluetooth (BLE) Robot Remote Control using an iPhone - Part 2

Introduction


If you want to use an iPhone as your controller then Bluetooth is one of the better communication options for directing your robot / drone / whatever.

In Part 2 we will cover the iPhone app required to control your BLE device. The AVA BLE Remote is available as a free download from iTunes. We will first cover how the app works and then how it was coded. For those who don't want to write their own controller, feel free to use ours. We have included the ability to customise the characters sent in response to a command.

We suggest you read Part 1 to understand the overall design but here is a quick recap.

This app has been designed to work with a Bluno board from DFRobot. The Bluno is a combination of an Arduino and a Bluetooth shield. It uses Bluetooth Low Energy (BLE), which is compatible with iOS 7.0+ devices: iPhone 5+,iPad 3+,iPad Mini,and iPod 5th Gen (note that Bluetooth LE capability is required).




Getting Started: The Connection Screen


When the app starts, it will open the connection tab and display any compatible BLE devices which are within range. To connect to a device just tap on its name. If the connection is successful, the word connected will be displayed and the communication indicator "LEDs" will turn from flashing blue to green. You can start a manual search for BLE devices by tapping the search (magnifying glass) icon in the top right of the connection screen. You can stop the search by tapping the disconnect device button (a cross) in the top left.

If you are already connected to a BLE device, tapping the disconnect device button will disconnect you. You can only attach to one BLE device at a time.



Setting a Task


Tasks are a behavioural robotics concept, but of course you can use them as you wish. You don't need to use tasks at all, if you want you can just use the next tab (Control) to direct your robot. Tasks are useful if you want the robot to do more than one thing. In our case, the robot only responds to remote control commands when the remote task has been set.

The idea is that each task is made up of subset of core behaviours. By combining different behaviours you can create a robot task. For example, the default tasks provided with the app are mapped to the following core behaviours in AVA.

Task ID        Name        Behaviours

     0               Status        BID_STOP
     1               Remote     BID_MANUAL
     2               Patrol        BID_AVOID, BID_ESCAPE, BID_POWER
     3               Follow IR BID_PIR_ATTRACT, BID_AVOID, BID_ESCAPE, BID_POWER
     4               Avoid IR   BID_PIR_REPEL, BID_AVOID, BID_ESCAPE, BID_POWER

By combining simple core behaviours you can get emergent complex task following behaviours which degrade gracefully when the robot faces unexpected situations. In our robot, each of the behaviours are set a priority. This priority decides which behaviour takes precedence. If the priorities are the same then the code will deal with them in the order presented (which gives an implied priority). You can refresh your knowledge on behavioural robotics by reading our earlier post on different approaches to robot AI.

Behaviour                        Priority                      Inputs                               Outputs 

BID_STOP                            1                            cliff, collision sensors       left & right motor stop
BID_MANUAL                    2                            BLE remote                       left & right motor controls
BID_ESCAPE                       3                            distance sensors, heading  left & right motor controls
BID_AVOID                         4                            distance sensors                 left & right motor controls
BID_POWER                        5                            batt voltage, homing          left & right motor controls
BID_PIR_ATTRACT           6                            PIR                                     left & right motor controls
BID_PIR_REPEL                 6                            PIR                                     left & right motor controls

The Task tab in the iPhone app allows you to send a tasking message to your Bluno. It is the responsibility of the robot code to assign current behaviours based on the assigned task.

Tapping a task will transmit the tasking ID (e.g. 1) via Bluetooth. Tap the add button (+) to add your own tasks. Tap Edit to delete or rearrange the tasks. Tapping "Default Tasks" will reload the task list that comes with the app.




Controlling your BLE Device


The Control tab acts as a virtual gamepad for your BLE device. The currently selected task is displayed at the top of the screen. Tapping the function keys (F1 to F4) will send the designated character via Bluetooth to your Bluno. Similarly, tapping the directional dPad or stop button will send the character assigned to those buttons. You can change the characters which are transmitted from the key mapping option in the console tab. The default mappings are:

F1    w
F2    x
F3    y
F4    z

Up Arrow         f
Down Arrow    b
Left Arrow       l
Right Arrow     r
Stop                  s

The speed slider will send the selected speed encapsulated with less than and greater than brackets.

Messages received back from the Bluno are displayed at the bottom of the screen.




The Console


The console tab allows you to send any character strings that you wish to the Bluno. Just type the message in the text field and tap Send. Any received messages will be displayed below.

The console tab also contains the system log which records various events, such as device discovery, connection, data transmission and disconnection. You can email the system log by tapping the mail button in the top right of the screen.

Tap the settings (gear icon) button if you wish to change the keys mapped to the Control buttons.




iOS Code


The iOS code is pretty straight forward and made much easier by the Bluno frameworks provided by DFRobot. You will need to include the following classes in your code.

- BLEDevice
- BLEUtility
- DFBlunoDevice
- DFBlunoManager

The download link for the entire Xcode project is provided below, but here are the highlights. Most of the heavy lifting is done by the DFBlunoManager class. This is a shared instance (singleton), so you just access it in your classes using:

 blunoManager = [DFBlunoManager sharedInstance];

The first step is scanning for and connecting with available Bluetooth LE (BLE) devices. Scanning is as simple as:

[blunoManager scan];

The results of the scan are handled by the DFBlunoDelegate, so you will need to have one of your classes conform to this protocol. We used the TabBarViewController as this is nice and central. Within our TabBarViewController, there are a couple of delegate methods which get notified when a scan is performed.

#pragma mark- DFBlunoDelegate

- (void)bleDidUpdateState: (BOOL)bleSupported
{
    NSString *logString;
    NSString *timeStamp = [formatter stringFromDate: [NSDate date]];
    
    if (bleSupported) {
        logString = [NSString stringWithFormat: @"%@: Scanning for BLE devices\n", timeStamp];
        [blunoManager scan];
    } else {
        logString = [NSString stringWithFormat: @"%@: BLE not supported\n", timeStamp];
    }
    
    [self log: logString];
}

- (void)didDiscoverDevice: (DFBlunoDevice *)device
{
    NSString *timeStamp = [formatter stringFromDate: [NSDate date]];
    NSString *logString = [NSString stringWithFormat: @"%@: New device discovered - %@\n", timeStamp, device.identifier];
    
    [self log: logString];
    
    BOOL bRepeat = NO;
    
    for (DFBlunoDevice *bleDevice in appDelegate.deviceArray) {
        if ([bleDevice isEqual: device]) {
            bRepeat = YES;
            break;
        }
    }
    
    if (!bRepeat) {
        [appDelegate.deviceArray addObject: device];
        
        NSString *logString = [NSString stringWithFormat: @"%@: Device added to BLE list\n", timeStamp];
        
        [self log: logString];
    }
    
    [connectionViewController.tbDevices reloadData];
}

We use these to update the table tbDevices in the ConnectionViewController. Once you have found an eligible device, you connect to it using:

[blunoManager connectToDevice: device];

In particular, since we use a table view to list the devices in range we allow the user to connect to a specific device by tapping on the row in the table that corresponds to that device. The device data is stored in deviceArray (which is a property of the application delegate). We also have a property in the application delegate which points to the currently active device (blunoDevice). The relevant code from the ConnectionViewController is:

#pragma mark- Table View Delegate

- (void)tableView:(UITableView *)tableView didSelectRowAtIndexPath:(NSIndexPath *)indexPath
{
    DFBlunoDevice *device = [appDelegate.deviceArray objectAtIndex:indexPath.row];
    
    if (appDelegate.blunoDevice == nil) {
        appDelegate.blunoDevice = device;
        [blunoManager connectToDevice: appDelegate.blunoDevice];
    } else if ([device isEqual: appDelegate.blunoDevice]) {
        if (!appDelegate.blunoDevice.bReadyToWrite) {
            [blunoManager connectToDevice: appDelegate.blunoDevice];
        }
    } else {
        if (appDelegate.blunoDevice.bReadyToWrite) {
            [blunoManager disconnectToDevice: appDelegate.blunoDevice];
            appDelegate.blunoDevice = nil;
        }
        
        [blunoManager connectToDevice: device];
    }
    
    [self.activityIndicator stopAnimating];
    self.activityIndicator.hidden = YES;
    [tableView deselectRowAtIndexPath: indexPath animated: YES];
}

Once you are connected to the remote device, sending data to it is performed by the following method (found in the application delegate).

#pragma mark - Bluno Communications

- (void)sendString: (NSString *)msg {
    if (self.blunoDevice.bReadyToWrite) {
        NSData *data = [msg dataUsingEncoding: NSUTF8StringEncoding];
        
        [blunoManager writeDataToDevice: data Device: self.blunoDevice];
    }
}

The DFBlunoDelegate will let you know what happens via two methods.

- (void)didWriteData: (DFBlunoDevice*)device
{
    //  NSLog(@"%s", __func__);
    
    NSString *timeStamp = [formatter stringFromDate: [NSDate date]];
    NSString *logString = [NSString stringWithFormat: @"%@: Data written\n", timeStamp];
    
    appDelegate.state = Transmitting;
    [self log: logString];
}

- (void)didReceiveData: (NSData *)data Device: (DFBlunoDevice *)device
{
    //  NSLog(@"%s", __func__);
    
    NSString *timeStamp = [formatter stringFromDate: [NSDate date]];
    NSString *receivedTextString = [[NSString alloc] initWithData: data encoding: NSUTF8StringEncoding];
    
    //  NSLog(@"ASCII Rx: %d",[receivedTextString characterAtIndex: 0]);
    
    if (receivedTextString && receivedTextString.length > 0 && ![receivedTextString isEqualToString: @"\n"]) {
        NSString *logString = [NSString stringWithFormat: @"%@: Rx - %@\n", timeStamp, receivedTextString];
        manualViewController.txtReceivedMsg.text = receivedTextString;
        consoleViewController.txtReceivedMsg.text = receivedTextString;
        [self log: logString];
    }
    
    appDelegate.state = Receiving;
    consoleViewController.txtSendMsg.text = @"";
}


In our app we use these to update the log file in the console and print out any data received from the remote device.

Finally, to disconnect a device, just use:

[blunoManager disconnectToDevice: appDelegate.blunoDevice];

You will need to replace appDelegate.blunoDevice with whatever you have called your Bluno Device object.

The complete source code is available at the Reefwing Code Repository.

Bluno Code


The Bluno code was covered in Part 1 of this series. Sample code to allow you to test the app is available from the Reefwing Gist Repository (https://gist.github.com/reefwing/eab05c12b615070732c3).