marvin:ecp_all
Differences
This shows you the differences between two versions of the page.
Both sides previous revisionPrevious revisionNext revision | Previous revision | ||
marvin:ecp_all [2009/01/28 10:06] – deva | marvin:ecp_all [2009/01/29 11:29] (current) – external edit 127.0.0.1 | ||
---|---|---|---|
Line 4: | Line 4: | ||
</ | </ | ||
- | {{page> | + | ======Introduction====== |
- | {{page> | + | This project is the final part of the course Embedded Systems – Embodied Agents at the University of Aarhus 2008. The outline of the project is to demonstrate a practical implementation based on some of the theory acquired during preliminary lab sessions and lectures during the course. In our case we are building a balancing robot using the LEGO Mindstorm Kit and utilizing the LeJOS framework. The balancing robot was included in the preliminary lab sessions and we wish to make the robot able to think for itself and improve balancing when driving. Therefore we shall use the concept of behaviour models which is also a part of this course. This blog will serve as our “documentation” of the project and when necessary we will introduce a little background regarding sensors, actuators and controllers used in this project. The source code will be included in smaller segments following the development of the project, but the entire source code is available for download. |
- | {{page> | + | |
- | {{page> | + | |
- | {{page> | + | |
- | {{page> | + | |
- | {{page> | + | |
+ | Our original project goal description can be found in its entirety in lab11(([[http:// | ||
+ | |||
+ | |||
+ | =====Structure of this Document===== | ||
+ | This documents is divided into 7 parts. The introduction is the first part and is meant to give a general introduction the project, the LEGO Mindstorm NXT and the software structure as well as the naming conventions for easier reading.\\ | ||
+ | Each lab report should not be read as a chronological evolution of the project, as there will be subjects mentioned that has not been fully implemented in that particular lab session. The lab reports are used to define different subjects of investigation.\\ | ||
+ | The following 5 lap reports describes the process and the results.\\ | ||
+ | * Lap report 1 describes how the robot was build and how the communication with the gyroscope is handled.\\ | ||
+ | * Lap report 2 describes different control architectures, | ||
+ | * Lap report 3 describes the steps involved in making the robot able to drive forward and backward and from there to be able to drive a predefined pattern. In this session we also present a supplementary section about DC servo motors based on the lectures.\\ | ||
+ | * Lap report 4 describes the implementation of behaviour models which include behaviours such as the ability to drive autonomously and avoid obstacles.\\ | ||
+ | * Lap report 5 describes how to make the robot remote controlled utilizing BlueTooth.\\ | ||
+ | Finally there is a discussion/ | ||
+ | |||
+ | |||
+ | =====Motivation===== | ||
+ | This problem of balancing an inverted pendulum is a nice delicate control problem and it has become the “Holy Grail” for many robot hobbyists around the world. The task of balancing a two wheeled robot may sound simple, but in reality many people have tried – and failed. In LAB4(([[http:// | ||
+ | Another great motivation factor has been all the great videos from [[http:// | ||
+ | |||
+ | {{ : | ||
+ | {{ : | ||
+ | |||
+ | =====Literature Review===== | ||
+ | A great number of balancing robot projects exists on the internet and many papers on the subject have been published in IEEE journals. Furthermore commercial solutions are available e.g. the world famous Segway. The two wheel balancing robot has been researched by engineers, mathematicians, | ||
+ | |||
+ | In a paper by Rich Chi Ooi((Balancing a Two-Wheeled Autonomous Robot, Author Rich Chi Ooi, The University of Western Australia | ||
+ | School of Mechanical Engineering, | ||
+ | |||
+ | Now let us turn towards the LEGO projects, which are numerous judging from the Youtube homepage. Two projects deserve special attention. Steven Hassenplug (([[http:// | ||
+ | |||
+ | |||
+ | |||
+ | |||
+ | |||
+ | |||
+ | |||
+ | |||
+ | |||
+ | |||
+ | |||
+ | |||
+ | |||
+ | |||
+ | |||
+ | =====Hardware===== | ||
+ | This section introduces the hardware used in this project. The sensors/ | ||
+ | |||
+ | |||
+ | ====NXT Module==== | ||
+ | {{: | ||
+ | |||
+ | The NXT(([[http:// | ||
+ | |||
+ | ===Motor Ports=== | ||
+ | The NXT has three output ports for attaching motors - Ports A, B and C. | ||
+ | |||
+ | ===Sensor Ports=== | ||
+ | The NXT has four input ports for attaching sensors - Ports 1, 2, 3 and 4. | ||
+ | |||
+ | ===USB Port=== | ||
+ | Connect a USB cable to the USB port and download programs from the computer to the NXT (or upload data from the robot to the computer). It is also possible to use the wireless Bluetooth connection for uploading and downloading. | ||
+ | |||
+ | ===Technical Specifications=== | ||
+ | * 32-bit ARM7 microcontroller | ||
+ | * 256 Kbytes FLASH, 64 Kbytes RAM | ||
+ | * 8-bit AVR microcontroller | ||
+ | * 4 Kbytes FLASH, 512 Byte RAM | ||
+ | * Bluetooth wireless communication (Bluetooth Class II V2.0 compliant) | ||
+ | * USB full speed port (12 Mbit/ | ||
+ | * 4 input ports, 6-wire cable digital platform (One port includes a IEC 61158 Type 4/EN 50 170 compliant expansion port for future use) | ||
+ | * 3 output ports, 6-wire cable digital platform | ||
+ | * 100 x 64 pixel LCD graphical display | ||
+ | * Loudspeaker - 8 kHz sound quality. Sound channel with 8-bit resolution and 2-16 kHz sample rate. | ||
+ | * Power source: 6 AA batteries | ||
+ | |||
+ | ====The Gyroscope==== | ||
+ | {{: | ||
+ | |||
+ | Measure the additional dimension of rotation with the NXT Gyro Sensor. This sensor that lets you accurately detect rotation for your NXT projects. The NXT Gyro Sensor returns the number of degrees per second of rotation as well as indicating the direction of rotation. | ||
+ | |||
+ | |||
+ | |||
+ | ====The Ultrasonic Sensor==== | ||
+ | {{: | ||
+ | |||
+ | The Ultrasonic Sensor enables your robot to see and detect objects. You can also use it to make your robot avoid obstacles, sense and measure distance, and detect movement.The Ultrasonic Sensor measures distance in centimeters and in inches. It is able to measure distances from 0 to 255 centimeters with a precision of +/- 3 cm. | ||
+ | |||
+ | ====The DC Servo Motor==== | ||
+ | {{: | ||
+ | |||
+ | There are three Servo Motors included in the Mindstorm kit. Each motor has a built-in Rotation Sensor. This lets your control your robot’s movements precisely. The Rotation Sensor measures motor rotations in degrees or full rotations (accuracy of +/- one degree). One rotation is equal to 360 degrees, so if you set a motor to turn 180 degrees, its output shaft will make half a turn. The built-in Rotation Sensor in each motor also lets you set different speeds for your motors (by setting different power parameters in the software). The motors have sufficient power in order to make the robot balance. | ||
+ | |||
+ | |||
+ | =====Software===== | ||
+ | The code is purely written in the Java syntax through the LeJOS NXT framework(([[http:// | ||
+ | We use CamelCase notation(([[http:// | ||
+ | All classes are fully documented by the javadoc documentation conventions(([[http:// | ||
+ | |||
+ | ====Class Overview==== | ||
+ | {{ : | ||
+ | The above figure illustrates all the important classes in the project as well as their relations to each other. | ||
+ | A tarball with the source code can be fetched here: {{: | ||
+ | |||
+ | Apart from the classes shown in the class overview, there are the '' | ||
+ | |||
+ | Take note that printing out to the LCD is a very expensive task, and since the art of balancing is very sensitive, the print class cannot be used while actually running. | ||
+ | |||
+ | =====The Name===== | ||
+ | The name //" | ||
+ | |||
+ | |||
+ | ======Lab Report 1 - Gyroscope Communication====== | ||
+ | **Date:** January 2nd 2009\\ | ||
+ | **Duration of activity:** 8-14\\ | ||
+ | **Participants: | ||
+ | |||
+ | =====Goal===== | ||
+ | Make Marvin able to communicate with gyroscope. | ||
+ | |||
+ | =====Plan===== | ||
+ | * Build the robot in LEGO following the instructions on the mathworks website(([[http:// | ||
+ | * Make a test program to test if the gyroscope is working. | ||
+ | * Find or create a class for the gyroscope communication. | ||
+ | |||
+ | =====Theory===== | ||
+ | |||
+ | |||
+ | ====Analogue to Digital Conversion and Limitations==== | ||
+ | The main problem when dealing with sensors is that the real world is continuous and when we want to interpret these data it has to be digitized (discretized) in some way. This is done with an analogue to digital converter (A/ | ||
+ | The gyroscope utilizes the ADC so that it is possible to get readings from the gyroscope. | ||
+ | |||
+ | ====Gyroscope==== | ||
+ | The gyroscope used has been purchased from HiTechnic (([[http:// | ||
+ | |||
+ | {{ : | ||
+ | |||
+ | The gyroscope is produced to work seamless with the LEGO NXT Brick. It connects to the NXT using a standard wire and utilizes the analogue interface. The gyroscope contains a single axis gyroscopic sensor that detects rotation and returns a value representing the number of degrees per second of rotation. The gyroscope sensor measures up to +/- 360 degrees per second of rotation and these readings will be signed if the correct offset is used. | ||
+ | As not all gyroscopes are identical there will be some minor differences in the offset, but the manual that came along with the gyroscope, indicates this to be around 620. This is the offset value has to be subtracted from the readings from the gyroscope in order to get a signed reading. \\ | ||
+ | |||
+ | The sensor can be read 300 times per second (an interval of 3.33 times per second), which correspond to a sample rate of 300 Hz.\\ | ||
+ | |||
+ | The gyroscope operates in relative values, which means that it can not tell where e.g. upright is. One have to " | ||
+ | |||
+ | The axis of measurement is in the vertical plane with the gyroscope sensor positioned with the black end cap facing upwards, as illustrated in thye figure below. | ||
+ | |||
+ | {{ : | ||
+ | |||
+ | The correct placement of the gyroscope can be seen in the [[#Building the robot|building instructions]] | ||
+ | |||
+ | ====Building the Robot==== | ||
+ | ===Preliminary considerations=== | ||
+ | Our primary goal is to make a balancing robot, and therefore it would be nice to eliminate the possibility of failure due to a bad build. This is the reason why we chose to re-use the instructions provided as seen in the building manual(([[http:// | ||
+ | |||
+ | ===The Build=== | ||
+ | The build is pretty straight forward, as the building instructions is well written and also accompanied by photos. One thing to note though, is that the robot is fairly symmetric, so building instructions is only provided for one side of the robot and it is up to the builder to mirror the other side. This is however no problem because of the good photos.\\ | ||
+ | The finished robot can be seen in the picture below. | ||
+ | |||
+ | {{ : | ||
+ | |||
+ | One thing worth noting is that the ultrasonic sensor is shifted slightly to the left. This is because it is important that the gyroscopes fix point is centered on the vertical axis of the robot. Positioning of the gyroscope is well documented in the building instructions. Below is a photo of the gyroscope in place. | ||
+ | |||
+ | {{ : | ||
+ | |||
+ | ====Peripheral Connections==== | ||
+ | ^ Sensor ^ Output ^ Unit ^ Data Type ^ Maximum Sample [1/sec] ^ Port ^ | ||
+ | | Rotary Encoder Left *| angle| deg | int32 | 1000 | B | | ||
+ | | Rotary Encoder Right *| angle| deg | int32 | 1000 | C | | ||
+ | | Ultrasonic Sensor | distance |cm | int32 | 50 (N1)| 2 | | ||
+ | | Gyroscope Sensor |angular speed |deg/sec | uint16 | 300 | 4 | | ||
+ | |||
+ | ^ Actuator ^ Input ^ Unit ^ Data Type ^ Maximum Sample [1/sec] ^ Port ^ | ||
+ | | DC Motor Left * | PWM | % | int8 | 500 | B | | ||
+ | | DC Motor Right * |PWM | % | int8 | 500 | C | | ||
+ | *The rotary encoders and DC Motors (left and right respectively) utilizes the same port. | ||
+ | |||
+ | ====Pros and Cons About the Design==== | ||
+ | As mentioned earlier, this is a well documented and working design. This is of great importance, because we can eliminate this as a factor of error later on. Another issue that have proven important in our experiments is the ability to easily remove the battery for recharging. This may not seem important at this point, but the characteristics of the voltage applied to the motors vary greatly with the charge level of the battery, and therefore the behaviour of the motors varies. It is therefore a nice feature that the battery is easy to remove without tearing the robot apart.\\ | ||
+ | In the group we discussed what effect the wheel diameter will have on the robot. Wheels with a larger diameter will travel a longer than smaller wheels for the same amount of power applied to the motor on which the wheel is mounted. We decided that the resolution of the wheel rotational properties were accurate enough to go with the larger wheels, and these wheels were also used in the design we used in the first place, and therefore was a well-proven design. | ||
+ | |||
+ | A thing worth mentioning is the construction of the wheel mounting point and wheelbase which can be seen on the picture below | ||
+ | ^ Wheel mounting on Marvin^ | ||
+ | |{{: | ||
+ | |||
+ | This construction provides a rigid frame that will prevent structural wobbling. This is important because of the sudden movements forward and backward motions a balancing robot evidently will make. | ||
+ | |||
+ | The wheel mounting point is cleverly made so that there will be no loss of power between the motor and the wheel. | ||
+ | We used the standard wheels throughout testing, but found these to have to much friction when driven on rugged carpet. Although they have a cleverly designed mounting point we found that this was not necessary in our implementation and opted to go with the wheels shown below. | ||
+ | |||
+ | {{ : | ||
+ | |||
+ | =====Implementation===== | ||
+ | |||
+ | ====Numerical Readings from the Gyroscope==== | ||
+ | A gyroscope measures degrees per second or simply angle velocity and therefore we must use numerical integration in order to get the angle. A gyroscope has a bias which is simply an offset and naturally we want to eliminate this offset by calibration methods at the beginning of the program. If we do not calibrate, the integration, | ||
+ | |||
+ | In order to do a discrete integration we simply need to look at a simple recursive difference equation, which in this case will be a running sum. Recall the difference equations for simple first order derivatives and a very simple integration: | ||
+ | Integration: | ||
+ | First difference: < | ||
+ | Each sample in the output signal is a sum of weighted samples from the input. The reason for pointing out the first order derivative as well is that they (of course) are closely related which becomes clear in the illustration below. (([[http:// | ||
+ | |||
+ | {{: | ||
+ | |||
+ | There are many types of algorithms for numerical integrations that are far more advanced and all of them or based on some kind of interpolation technique, but for now we will rely on the simplest form which is the running sum. | ||
+ | |||
+ | ====Programming==== | ||
+ | LeJOS has its own '' | ||
+ | |||
+ | {{ : | ||
+ | |||
+ | This class wraps the gyroscope sensor, and offers readouts as either angle velocities, or as integrated angle velocities over time (angles).\\ | ||
+ | The angle velocities can be read raw from the sensor port, but must be subtracted with an offset defining the stand-still gyroscope. This value is according to the documentation approximately 620, but may vary from gyroscope to gyroscope. We made some experiments and found the value 595.5 to be pretty accurate.\\ | ||
+ | This value is however not accurate enough when run over a large quantity of time, which will make the integration drift, thereby making Marvin unable to keep its balance.\\ | ||
+ | The solution was to use the measured value as an initial value, and then use a weighted average of all measured angles over time, where the last measured angle is weighted very low. We tested the solution with a variety of values, and found the value 0.999999 to be pretty good (one might think the having a static 595.5 might do the same, but experiments show otherwise). | ||
+ | These constants are hard coded into the program with the following names and values: | ||
+ | |||
+ | ^ Variable | ||
+ | | lastOffset | 595.5 | | ||
+ | | a | 0.999999 | | ||
+ | |||
+ | For the integration over time algorithm we use a simple sum multiplied by the delta time since last call. This delta time is acquired by calling the '' | ||
+ | |||
+ | In the following excerpt from the '' | ||
+ | <code java> | ||
+ | public class GyroscopeSensor | ||
+ | { | ||
+ | private SensorPort port; | ||
+ | |||
+ | . | ||
+ | . | ||
+ | . | ||
+ | |||
+ | private double getAngleOffset() | ||
+ | { | ||
+ | double offset = lastOffset * a + (1.0 - a) * (double)port.readValue(); | ||
+ | lastOffset = offset; | ||
+ | return offset; | ||
+ | } | ||
+ | |||
+ | |||
+ | . | ||
+ | . | ||
+ | . | ||
+ | |||
+ | public double getAngle() | ||
+ | { | ||
+ | int now = (int)System.currentTimeMillis(); | ||
+ | int delta_t = now - lastGetAngleTime; | ||
+ | |||
+ | // Make sure we only add to the sum when there has actually | ||
+ | // been a previous call (delta_t == now if its the first call). | ||
+ | if(delta_t != now) { | ||
+ | angle += getAngleVelocity() * ((double)delta_t / 1000.0); | ||
+ | } | ||
+ | |||
+ | lastGetAngleTime = now; | ||
+ | |||
+ | return angle; | ||
+ | } | ||
+ | } | ||
+ | </ | ||
+ | The full source code for this class can be found in the file '' | ||
+ | |||
+ | ======Lab report 2 - PID Balance Control====== | ||
+ | **Date:** January 12nd 2009\\ | ||
+ | **Duration of activity:** 8-12\\ | ||
+ | **Participants: | ||
+ | //Note that we returned to this subject several times during the following lab sessions.// | ||
+ | |||
+ | =====Project Goal===== | ||
+ | Make the robot balance using mathematical modelling or simple PID parameter tuning. | ||
+ | |||
+ | =====Plan===== | ||
+ | * Discuss different control approaches and decide a course of action. | ||
+ | * Introduce the chosen controller | ||
+ | * Evaluate which features to use in the closed loop | ||
+ | * Make implementation of the chosen controller. | ||
+ | * Evaluate different methods of parameter tuning | ||
+ | |||
+ | |||
+ | ===== Theory ===== | ||
+ | ====Our Angle of Approach Towards Control Theory==== | ||
+ | |||
+ | Basically there are three approaches to our control problem, which we will describe briefly in an increasing order of complexity. | ||
+ | The second approach is again to consider the control plant as a black box, but here we wish to estimate the characteristic transfer function of the plant. Recall that if we give an impulse to a filter we are given the filter characteristic by means of the impulse response. The analogy to control theory is to apply a step function and observe the transient response, which is illustrated on the figures in the following section [[# | ||
+ | The third method is to derive a mathematical model of the dynamics of the control plant and this can be somewhat complex. In the case of a balancing robot it is helpful to look at the mathematical modelling of an inverted pendulum and there next to add the physical dimensions of the balancing robot. When working with modelling we end up with non linear equations, which is a problem as our controllers are linear. One common method is to use the state space representation and then to make a linearisation around the steady state point or the equilibrium. If our mathematical model is precise and close to the true physical model we obtain a great theoretical foundation for creating a stable system. This is exactly what Yorihisa Yamamoto(([[http:// | ||
+ | |||
+ | |||
+ | |||
+ | ====Introducing the PID Controller==== | ||
+ | |||
+ | The digital implementation of a PID controller is actually based on very simple filtering techniques similar to what we described in the [[http:// | ||
+ | |||
+ | {{ : | ||
+ | |||
+ | The plant is a term used in control theory to indicate the physical system, which is the robot. The Error signal is fed to the | ||
+ | [[http:// | ||
+ | control and the output is then subtracted from our reference values resulting an a new error signal. We see that the PID part of the loop is simply | ||
+ | |||
+ | |||
+ | < | ||
+ | \frac{{M\left( z \right)}}{{E\left( z \right)}} = D\left( w \right) = K_p + K_I \frac{1}{w} + K_D w | ||
+ | </ | ||
+ | |||
+ | < | ||
+ | M\left( z \right) = \left( {K_p + K_I \frac{1}{w} + K_D w} \right)E\left( z \right) | ||
+ | </ | ||
+ | |||
+ | < | ||
+ | \begin{equation} | ||
+ | \frac{{M\left( z \right)}}{{E\left( z \right)}} = D\left( w \right) = K_p + K_I \frac{1}{w} + K_D w | ||
+ | \end{equation} | ||
+ | </ | ||
+ | |||
+ | < | ||
+ | \begin{equation} | ||
+ | M\left( z \right) = \left( {K_p + K_I \frac{1}{w} + K_D w} \right)E\left( z \right) | ||
+ | \end{equation} | ||
+ | </ | ||
+ | |||
+ | |||
+ | This formulation is in the discrete frequency domain and we need to perform an inverse Z-transformation to go back to the discrete time domain. Instead of performing this explicitly we may just jump directly to the solution, since we already know what is required. We need a discrete time integration and differentiation and we can choose among several techniques. In the following we use the trapezoidal method and the backward difference as mentioned to obtain\\ | ||
+ | |||
+ | < | ||
+ | m_{{\rm{Integral}}} \left[ {\left( {k + 1} \right)T} \right] = m\left( {kT} \right) + \frac{T}{2}\left\{ {e\left[ {\left( {k + 1} \right)T} \right] + e\left( {kT} \right)} \right\} | ||
+ | </ | ||
+ | |||
+ | < | ||
+ | m_{{\rm{Derivative}}} \left[ {\left( {k + 1} \right)T} \right] = \frac{{e\left[ {\left( {k + 1} \right)T} \right] - e\left[ {kT} \right]}}{T} | ||
+ | </ | ||
+ | |||
+ | < | ||
+ | m\left[ {\left( {k + 1} \right)T} \right] = K_p e\left[ {\left( {k + 1} \right)T} \right] + K_I m_{{\rm{Integral}}} \left[ {\left( {k + 1} \right)T} \right] | ||
+ | </ | ||
+ | < | ||
+ | + K_D m_{{\rm{Derivative}}} \left[ {\left( {k + 1} \right)T} \right] | ||
+ | </ | ||
+ | |||
+ | |||
+ | |||
+ | < | ||
+ | \begin{equation} | ||
+ | m_{{\rm{Integral}}} \left[ {\left( {k + 1} \right)T} \right] = m\left( {kT} \right) + \frac{T}{2}\left\{ {e\left[ {\left( {k + 1} \right)T} \right] + e\left( {kT} \right)} \right\} | ||
+ | \end{equation} | ||
+ | </ | ||
+ | |||
+ | < | ||
+ | \begin{equation} | ||
+ | m_{{\rm{Derivative}}} \left[ {\left( {k + 1} \right)T} \right] = \frac{{e\left[ {\left( {k + 1} \right)T} \right] - e\left[ {kT} \right]}}{T} | ||
+ | \end{equation} | ||
+ | </ | ||
+ | |||
+ | < | ||
+ | \begin{equation} | ||
+ | m\left[ {\left( {k + 1} \right)T} \right] = K_p e\left[ {\left( {k + 1} \right)T} \right] + K_I m_{{\rm{Integral}}} \left[ {\left( {k + 1} \right)T} \right] | ||
+ | + K_D m_{{\rm{Derivative}}} \left[ {\left( {k + 1} \right)T} \right] | ||
+ | \end{equation} | ||
+ | </ | ||
+ | |||
+ | |||
+ | |||
+ | |||
+ | |||
+ | This is the output that is fed to the motor. | ||
+ | |||
+ | ^Transient responses of Kp, Kd and Ki^ | ||
+ | |{{: | ||
+ | |||
+ | |||
+ | The above pictures are from (([[http:// | ||
+ | |||
+ | By inspection we summarize the following. An increase in the proportional gain will create a larger overshoot, but reduce the steady state error and rise time. An increase in the integral controller also increase the overshoot, while eliminating the steady state error. If we increase the differential controller, we decrease the overshoot with no affect on the steady state error, but the bandwidth of the system will be reduced. | ||
+ | |||
+ | |||
+ | |||
+ | At wikipedia(([[http:// | ||
+ | <code java> | ||
+ | previous_error = 0 | ||
+ | integral = 0 | ||
+ | start: | ||
+ | error = setpoint - actual_position | ||
+ | integral = integral + error*dt | ||
+ | derivative = (error - previous_error)/ | ||
+ | output = Kp*error + Ki*integral + Kd*derivative | ||
+ | previous_error = error | ||
+ | wait(dt) | ||
+ | goto start | ||
+ | </ | ||
+ | |||
+ | Further readings and inspiration are found at this homepage(([[http:// | ||
+ | |||
+ | ==== Defining the States in the Control Loop ==== | ||
+ | |||
+ | Now that we have established our choice of control loop, we must determine an error function. In a state space representation this corresponds to determining the states, which are defined through differential equations describing the dynamics of the control plant. Therefore we may turn to the model described in the documentation of Rich Chi Ooi((Balancing a Two-Wheeled Autonomous Robot, Author Rich Chi Ooi, The University of Western Australia | ||
+ | School of Mechanical Engineering, | ||
+ | , the angle velocity,< | ||
+ | , (its first derivative), | ||
+ | , and the platform angular velocity,< | ||
+ | , (its first derivative). | ||
+ | |||
+ | {{ : | ||
+ | |||
+ | This makes perfectly sense, since we need to drive the wheels in a direction that keeps the upper body of the robot in equilibrium. We therefore need sensor readings regarding the position of the wheels and the upper body in order to keep the wheels under the robot’s centre of gravity. It is easy to argument for the first order derivatives as well. From a mathematical point of view, the control point is a linearisation of second order non-linear equations (differential equations of the inverted pendulum) and therefore a descent approximation would include the first order terms to indicate the curvature as well. From a practical point of view, the curvature/ | ||
+ | |||
+ | {{ : | ||
+ | |||
+ | |||
+ | |||
+ | ==== Parameters and Stability ==== | ||
+ | The calculation of the right PID control parameters is essential to secure a stable system. This is called loop tuning. If loop tuning is not done according to the task at hand, the control system will become unstable i.e. the output will diverge. Oscillation might occur in this case. The only thing that will prevent divergence or oscillation is saturation or mechanical breakage. This may happen a lot when trying to produce a balancing robot and is generally not a problem due the great structural integrity, however if the controller is to control something else that might not be as rigid as our construction oscillation is always something to avoid in the first place. \\ | ||
+ | Loop tuning could be defined as adjusting the control loop parameters (Proportional gain(P), Integral gain (I) and Derivative gain(D)) to the optimum value for a desired control response. The z-transform for the controller should, in order to be stable obey the rules for a stable system, that is poles of the system should be located inside the unity circle, see diagram below. | ||
+ | |||
+ | {{ : | ||
+ | |||
+ | Poles are represented with a cross and zeros with a circle. The shown diagram has no affiliation with our system. It is just provided as an example. | ||
+ | |||
+ | There are a number of different approaches when tuning PID parameters of which three will be described below. | ||
+ | The methods that can be used range from no knowledge of the system to a full simulation of the system that can be used to calculate stability and set the exact parameters of the PID controller for optimum performance. One thing to note before talking about parameter tuning is the difference between online tuning and offline tuning. Online tuning means tuning when the system is running. That means one adjust parameters while the system is running. Offline tuning means take the control out of the plant, adjust the parameters and then re-engage the system. It is the later method we have used. The robot have been stopped, the parameters have en adjusted in the software, and lastly new firmware have been uploaded. Online tuning could have been used with a Bluetooth link between the robot and a PC from where new parameters could be uploaded on the fly. | ||
+ | |||
+ | === Manual Tuning === | ||
+ | This is a trial and error approach. You do not need to have any idea about the plant. The procedure is as follows: | ||
+ | * Set I and D values to zero | ||
+ | * Increase P until output of loop oscillates | ||
+ | * Set P to half the gain obtained in the previous step. P is now set for a quarter amplitude decay response type | ||
+ | * Increase I until steady state offset is correct. Too much will make the system unstable | ||
+ | * Increase D to get a faster settling time for the system. | ||
+ | |||
+ | These steps can be used in order to tune a system and it is preferable to do it to a system that is online. | ||
+ | |||
+ | The table below show the effects of increasing the parameters and can be used for fine tuning. | ||
+ | ^ Parameter ^ Rise Time ^ Overshoot ^ Settling Time ^ Steady State ^ | ||
+ | ^ P | Decrease | Increase | Small Change | Decrease | | ||
+ | ^ I | Decrease | Increase | Increase | Eliminate | | ||
+ | ^ D | Small Decrease | Decrease | Decrease | None | | ||
+ | |||
+ | === Ziegler-Nichols Method === | ||
+ | Another method developed makes use of what is called a critical gain. This is denoted < | ||
+ | |||
+ | ^ Control Type ^ < | ||
+ | ^ P | < | ||
+ | ^ I | < | ||
+ | ^ D | < | ||
+ | |||
+ | === Tuning Software === | ||
+ | There exist different kinds of tuning software. One of these could be (([[http:// | ||
+ | |||
+ | === Summarize of Approaches === | ||
+ | The three different approaches can be seen from the table below. In that table the advantages an disadvantages can me seen together with what knowledge is required. | ||
+ | ^ Method ^ Advantages ^ Disadvantage ^ Requirements ^ | ||
+ | ^ Manual Tuning | No math required (Online*) | Inaccurate | ||
+ | ^ Ziegler-Nichols | Proven method (Online*)| Trial and error, pretty aggressive tuning | Ability to measure starting parameters such as < | ||
+ | ^ Software Tools | Consistent tuning, Online or offline tuning, Allow for simulation beforehand | Expensive, knowledge about plant and development tools are required | Mathematical model | | ||
+ | * Online can in some cases be a disadvantage, | ||
+ | |||
+ | |||
+ | |||
+ | |||
+ | |||
+ | ===== Implementation ===== | ||
+ | First we present the software implementation, | ||
+ | |||
+ | ==== Software - The Balance Control Class==== | ||
+ | {{ : | ||
+ | This class contains all the PID magic happening that makes Marvin keep its balance. | ||
+ | |||
+ | The balancing module reads the angles and the angle velocities from the gyroscope and motors, and uses these values (weighted) to feed to the PID control calculation which again produces the new power to apply to the motors. | ||
+ | |||
+ | These are the constants used as weights in PID control, and the error calculation: | ||
+ | ^ Symbol ^ Variable ^ Value ^ | ||
+ | | < | ||
+ | | < | ||
+ | | < | ||
+ | | < | ||
+ | | < | ||
+ | | < | ||
+ | | < | ||
+ | |||
+ | < | ||
+ | integral parts of the PID control. We name the gyroscope angle < | ||
+ | and the Motor angle < | ||
+ | velocities) < | ||
+ | |||
+ | The sleep call at the end of the loop, reflects the max sample speed of the gyroscope, which should be is 300 times per second, 3.33msec between calls, but since the code itself takes up some time, the actual sleep value of 3msec seem to work appropriately. | ||
+ | |||
+ | |||
+ | The controls are added, in two places: | ||
+ | * To make Marvin move forward and backwards we add the tilt offset to the gyroscope angle (at the '' | ||
+ | |||
+ | * To make Marvin rotate wee add the left and right motor offsets directly to the right and left motor power (at the '' | ||
+ | |||
+ | The following is an excerpt from the BalanceControl class, showing the inner loop (which is running as a thread), that makes the error calculation, | ||
+ | <code java> | ||
+ | public class BalanceControl extends Thread | ||
+ | { | ||
+ | . | ||
+ | . | ||
+ | . | ||
+ | |||
+ | public void run() | ||
+ | |||
+ | { | ||
+ | MotorControl motors = new MotorControl(Motor.C, | ||
+ | GyroscopeSensor gyro = new GyroscopeSensor(SensorPort.S4); | ||
+ | |||
+ | double int_error = 0.0; | ||
+ | double prev_error = 0.0; | ||
+ | |||
+ | while(true) { | ||
+ | double Psi = gyro.getAngle() + ctrl.tiltAngle(); | ||
+ | double PsiDot = gyro.getAngleVelocity(); | ||
+ | |||
+ | double Phi = motors.getAngle(); | ||
+ | |||
+ | double PhiDot = motors.getAngleVelocity(); | ||
+ | |||
+ | double error = Psi * K_psi + Phi * K_phi + PsiDot * K_psidot + PhiDot * K_phidot; | ||
+ | |||
+ | int_error += error; // Integral Error | ||
+ | |||
+ | double deriv_error = error - prev_error; // Derivative Error | ||
+ | prev_error = error; | ||
+ | |||
+ | double pw = error * Kp + deriv_error * Kd + int_error * Ki; | ||
+ | |||
+ | motors.setPower(pw + ctrl.leftMotorOffset(), | ||
+ | |||
+ | try { sleep(3); } catch(java.lang.InterruptedException e) { } | ||
+ | } | ||
+ | } | ||
+ | } | ||
+ | </ | ||
+ | |||
+ | The full source code for this class can be found in the file '' | ||
+ | |||
+ | |||
+ | |||
+ | |||
+ | |||
+ | ====Hardware Related Issues==== | ||
+ | At first we experienced severe stability problems as Marvin was highly sensitive to the offset of the gyroscope. The offset calibration did not appear to be stationary and this lead to diverging oscillations, | ||
+ | ======Lab report 3 - Driving====== | ||
+ | **Date:** January 14nd 2009\\ | ||
+ | **Duration of activity:** 8-16\\ | ||
+ | **Participants: | ||
+ | |||
+ | =====Project Goal===== | ||
+ | Make robot able to drive a predefined pattern. | ||
+ | |||
+ | =====Plan===== | ||
+ | * Create a Motor Control class that can append extra power to the motors individually to control how the robot is behaving. | ||
+ | |||
+ | =====Theory===== | ||
+ | ====DC Motor Drives==== | ||
+ | {{ : | ||
+ | The Lego Mindstorm kit comes with a set of DC Motors and therefore we shall give a short introduction to the DC motor and the DC motor controller. This will hopefully add nicely to the presentation of the H-bridge and DC servo motors(([[http:// | ||
+ | |||
+ | {{ : | ||
+ | |||
+ | The DC motor is illustrated with a permanent magnet, but in many cases a electromagnet is used instead as it improves the performance of the DC servo motor. In a DC motor the rotor carries the electrical power as opposed to the AC motor, where the stator handles the power. The armature winding needs to be in the rotor of a DC motor to provide a rectification of the voltages and currents, which enables the motor to drive in all four quadrants. As we see on the illustration, | ||
+ | |||
+ | {{ : | ||
+ | |||
+ | ====Power Electronic Converter==== | ||
+ | We see that the four quadrants refers to the four combinations of voltage and current directions. As mentioned the motor is actually transferring energy back to the supply when breaking and this requires special attention when designing a motor controller. In order to control the DC motor a Power Electric Converter (PEC) that satisfies the following conditions is needed. ((Power Electronics, | ||
+ | * The converter must allow both output voltage and current to reverse in order to yield a four-quadrant operation. | ||
+ | * For accurate control of position, the average voltage output of the converter should vary linearly with its control input, independent of the load on the motor. | ||
+ | * The converter output should respond as quickly as possible to its control input, thus allowing the converter to be represented essentially by a constant gain without dead time in the overall servo drive system. | ||
+ | |||
+ | Further demands may be required, but these are essentials and adequate for our purpose. A linear power amplifier would be highly ineffective and therefore a switch mode dc-dc converter is used. A full bridge (H-bridge) converter is in fact capable of operating in all four quadrants and is illustrated on the following diagram. | ||
+ | |||
+ | {{ : | ||
+ | |||
+ | Observe that it contains an energy absorption circuit in order to waste the kinetic energy from the motor load inertia by heating up a resistor. A more advanced solution would be to store the energy back to the battery, which we may assume in the NXT kit. Lastly we will demonstrate the four-quadrant behaviour of the full-bridge dc-dc converter. | ||
+ | |||
+ | {{ : | ||
+ | |||
+ | The top left illustration show forward motion, where two transistors are connected to guide the current forward through the DC motor - notice that the transistors are always connected pairwise. As we decide to change direction the transistors are disconnected in the blanking time, whereafter the other two transistors are connected. The rectifiers are mounted anti parallel and works as a valve for the aforementioned extra energy, that is either eliminated by the resister or fed back to the battery. This is illustrated on the top right, which indicates the forward breaking of the motor. Notice how the voltage and current directions are maintained in the breaking phase, since the motor is incapable of altering its direction instantaneous cf. our explanation regarding kinetic energy. The lower left illustrates the backward motion of the motor and finally lower right shows the backward breaking loop. We therefore see that the H-bridge is in fact capable of four quadrant motion, which is cleverly implemented using transistors and rectifiers. (The diagram is simplified, naturally). | ||
+ | Fortunately the motor control is linear, which is a requirement in our control loop as it is a linear controller. However, the average voltage output does not vary linear with the control input voltage during the blanking time, which occurs when the motor direction changes. In our case this happens all the time to keep the robot in state of equilibrium and we may therefore expect some non linear behaviour from the motors. | ||
+ | |||
+ | ====Motor Encoder/ | ||
+ | When designing the motors, wheels and drive train, it will almost always be important to have some sort of encoder feedback. In the LeJOS framework there are methods to get readings from the tacho counters and these sensor readings have proven to be very useful when designing a balancing robot cf. the research literature in the [[http:// | ||
+ | |||
+ | {{ : | ||
+ | |||
+ | =====Implementation===== | ||
+ | ====Right/ | ||
+ | Now that the robot is balancing it ought to be simple to make it drive around. We expect the controller to maintain its balance even though we apply the necessary offset to the PWM in order to make it move. At first we added a small offset to one wheel and subtracted from the other, which caused the robot to drive in a circle. The robot actually seemed to be more robust when turning and we were able to reach a high speeds when pivoting "on the spot" | ||
+ | |||
+ | [[http:// | ||
+ | {{youtube> | ||
+ | |||
+ | As you may have noticed on the video we did also change the tires. The tires in the original model had a course pattern and this caused it to get stuck on the carpet and so the robot did not behave equally on both carpets and hard surfaces. By changing to a very flat and smooth tire we experienced more similar behaviours on different surfaces and this made the tuning of the parameters in the control loop a bit easier. | ||
+ | |||
+ | ====Forward/ | ||
+ | The idea was then to make the robot move forward by adding a small offset on both wheels, but clearly this was not the right approach. Either the controller does not allow the robot to move, or it oscillates and crashes. The reason for this is actually quite clear if we recall our control loop. \\ | ||
+ | {{ : | ||
+ | |||
+ | If we apply an offset to both wheels we are removing control from the controller and disturbing the calculated error. Either the control loop will overcome this disturbance by some minor oscillations or it will become unstable as we are really adding to the overshoot. This does not happen when we add a small offset to one wheel and subtract from the other since this does not affect the overall error signal. The natural place to control a closed loop control is of course the reference values, which are applied for each state. The reason that we did not use this approach immediately is probably that the reference values have been left unused as we want all the states to be zero in order for the robot to remain in equilibrium. If a small offset is added to the tilt angle (< | ||
+ | Using this approach we are now able to control the robot as expected, thus we are able to make the robot drive a predefined pattern. It is important to mention that the robot still has a tendency to oscillate, which often requires the control loop to " | ||
+ | The goal of this lab session was to make Marvin capable of driving a predefined pattern, but we did not specify anything about the accuracy of its position or which type of pattern. We are currently satisfied with the progress and we find the result satisfactory with respect to the next lab session in which we shall implement the behaviour based control model. | ||
+ | |||
+ | ====Gyroscope Offset Drift Problem==== | ||
+ | Easy as it seems, we did not arrive at the goal without bumps. We had a very annoying problem, where the gyroscope offset was drifting whenever the robot was not standing still in equilibrium (i.e. driving somewhere).\\ | ||
+ | The offset seemed to drift in the direction of the angle (and thereby driving direction), and the larger the angle, the faster the drifting.\\ | ||
+ | We tried several solutions to the problem, in the following they will be outlined, together with the reasons for why they did not work (apart from the last one which in fact worked):\\ | ||
+ | * The first approach was based on the fact that when the robot was in perfect balance (the angle velocities were below a threshold for some amount of time) we could redefine the angle as zero, thus reset the gyroscope angle. This however would make the robot reset its angle at a position //not// vertical whenever driving forward, for some period of time long enough to exceed the threshold buffer length. | ||
+ | * Second we used the fact that when the angle offset had reached a certain level, the robot continuously tried to straighten up, resulting in the wheels spinning up to maximum speed in the opposite direction. Based on this observation, | ||
+ | * Next we tried to calculate the drift size by multiplying the angle with a constant. We put a lot of effort into tuning this constant, but all in vein. If the drift size can be calculated as a function of the angle, it is certainly not a linear one. | ||
+ | * We had read, on an NXT/ | ||
+ | * We later tried to simply add a constant to the calculated angle after each iteration, and tried to adjust it manually, by modifying it, tweaking it, until we thought it as accurate as we could possibly make it. This seemed to work to a certain point. The drift grew smaller, but never seemed to disappear entirely. | ||
+ | * For some time we suspected the gyroscope reading in itself of being too inaccurate, which lead to a fix, where we replaced every reading with the average of a number of readouts, each with a 3ms interval. This seemed to work while the robot was in balance, but had negative effect when it was tilting fast, since this very same method would dampen the large values of the gyroscope, so this was obviously not a satisfying solution either. | ||
+ | * The last attempt was to simply make a running weighted average of all the gyroscope readings, with an initial value measured by hand. This method was based on the assumption, that the robot would be tilting equally much forward and backwards, which seems plausible. This seemed to work, so that was the solution we chose. | ||
+ | |||
+ | All of these considerations lead to the programming of the '' | ||
+ | ====The MotorControl Class==== | ||
+ | {{ : | ||
+ | The MotorControl class handles the motors exclusively. It works as a control interface on top of the actual Motor ports, and can report angle and angle velocity (using the TACHO counter) of both of them, as well as set the power and direction.\\ | ||
+ | |||
+ | The small stabilizing algorithm is located at the top of the '' | ||
+ | The small amount of power added by the sine function, makes the robot wriggle a little when standing absolutely still, which is actually noticeable when observed carefully. | ||
+ | |||
+ | The motor angle and angle velocity methods simply uses the average of the values from the actual motor ports.\\ | ||
+ | The code for these methods can be seen below: | ||
+ | <code java> | ||
+ | class MotorControl | ||
+ | { | ||
+ | private Motor leftMotor; | ||
+ | private Motor rightMotor; | ||
+ | |||
+ | . | ||
+ | . | ||
+ | . | ||
+ | |||
+ | public void setPower(double leftPower, double rightPower) | ||
+ | { | ||
+ | sin_x += sin_speed; | ||
+ | int pwl = (int)( leftPower + Math.sin(sin_x) * sin_amp ); | ||
+ | int pwr = (int)( rightPower - Math.sin(sin_x) * sin_amp ); | ||
+ | |||
+ | leftMotor.setSpeed(pwl); | ||
+ | if(pwl < 0) { | ||
+ | leftMotor.backward(); | ||
+ | } else if(pwl > 0) { | ||
+ | leftMotor.forward(); | ||
+ | } else { | ||
+ | leftMotor.stop(); | ||
+ | } | ||
+ | |||
+ | rightMotor.setSpeed(pwr); | ||
+ | if(pwr < 0) { | ||
+ | rightMotor.backward(); | ||
+ | } else if(pwr > 0) { | ||
+ | rightMotor.forward(); | ||
+ | } else { | ||
+ | rightMotor.stop(); | ||
+ | } | ||
+ | } | ||
+ | |||
+ | public double getAngle() | ||
+ | { | ||
+ | return ((double)leftMotor.getTachoCount() + (double)rightMotor.getTachoCount()) / 2.0; | ||
+ | } | ||
+ | |||
+ | public double getAngleVelocity() | ||
+ | { | ||
+ | return ((double)leftMotor.getActualSpeed() + (double)rightMotor.getActualSpeed()) / 2.0; | ||
+ | } | ||
+ | } | ||
+ | </ | ||
+ | |||
+ | |||
+ | ====Ctrl==== | ||
+ | {{ : | ||
+ | The Ctrl class is used to handle the control parameters of Marvin. | ||
+ | It is used for cross thread communication, | ||
+ | The parameters are left and right motor power offsets, and gyroscope tilt angle offset. | ||
+ | |||
+ | The way we made Marvin drive in a hard coded path was to set these values directly in the Ctrl class, and use a counter to switch values after a predefined number of iterations (simply increment the counter upon each '' | ||
+ | |||
+ | The code for the '' | ||
+ | <code java> | ||
+ | class Ctrl { | ||
+ | private double offsetLeft = 0.0; | ||
+ | |||
+ | . | ||
+ | . | ||
+ | . | ||
+ | |||
+ | public synchronized void setLeftMotorOffset(double offset) | ||
+ | { | ||
+ | this.offsetLeft = offset; | ||
+ | } | ||
+ | |||
+ | . | ||
+ | . | ||
+ | . | ||
+ | |||
+ | public synchronized double leftMotorOffset() | ||
+ | { | ||
+ | return this.offsetLeft; | ||
+ | } | ||
+ | |||
+ | . | ||
+ | . | ||
+ | . | ||
+ | } | ||
+ | </ | ||
+ | |||
+ | The full source code for these classes can be found in the files '' | ||
+ | |||
+ | ======Lab report 4 - Behaviour Control====== | ||
+ | **Date:** January 16nd 2009\\ | ||
+ | **Duration of activity:** 8-16\\ | ||
+ | **Participants: | ||
+ | |||
+ | =====Project Goal===== | ||
+ | Make the robot drive autonomously, | ||
+ | |||
+ | =====Plan===== | ||
+ | * Move balance related code to a thread. | ||
+ | * Make control parameters writeable from outside the motor thread to make it possible to drive around. | ||
+ | * Copy code from previous Behaviour project (([[http:// | ||
+ | * Modify the code to make it fit Marvin. | ||
+ | |||
+ | =====Theory===== | ||
+ | ====The Ultrasonic Sensor==== | ||
+ | Ultrasonic sensors work on a principle similar to radar or sonar by emitting an impulse and interpreting the echoes from radio or sound waves respectively. Ultrasonic sensors generate high frequency sound waves and evaluate the echo which is received back by the sensor. The time interval between the emitted and received signal is then calculated in order to determine a distance to a given object. The ultrasonic sensors are also known as transducers when they both send and receive signals. The Ultrasonic Sensor(([[http:// | ||
+ | |||
+ | {{ : | ||
+ | |||
+ | ====Knowledge Learned from Previous Lessons==== | ||
+ | Prior to using a sensor for a specific purpose it is important to investigate the main use for the sensor. For example in lab session 4 (([[http:// | ||
+ | |||
+ | [[http:// | ||
+ | {{youtube> | ||
+ | |||
+ | =====Implementation===== | ||
+ | ====The Behavior Class==== | ||
+ | {{ : | ||
+ | A platform for a behaviour based system with a simple suppression mechanism. An actual behaviour is defined by extending the '' | ||
+ | It has access to control the Marvin by means of the control methods (forward, backward, left and right). | ||
+ | However, access can be suppressed so that the motor commands will not be performed while the behaviour is suppressed. | ||
+ | |||
+ | ^ Variable | ||
+ | | turnPower | 200 | | ||
+ | | tiltPower | 0.06 | | ||
+ | |||
+ | <code java> | ||
+ | public class Behavior extends Thread | ||
+ | { | ||
+ | private ArrayList behaviors;; | ||
+ | private boolean suppressed; | ||
+ | private Ctrl ctrl; | ||
+ | |||
+ | public Behavior(ArrayList behaviors, Ctrl ctrl) | ||
+ | { | ||
+ | this.ctrl = ctrl; | ||
+ | suppressed = false; | ||
+ | |||
+ | this.setDaemon(true); | ||
+ | |||
+ | // Clone the Behavior list. | ||
+ | this.behaviors = new ArrayList(); | ||
+ | |||
+ | for(int i = 0; i < behaviors.size(); | ||
+ | Behavior b = (Behavior)behaviors.get(i); | ||
+ | this.behaviors.add(b); | ||
+ | } | ||
+ | } | ||
+ | |||
+ | public void suppressLower() | ||
+ | { | ||
+ | for(int i = 0; i < behaviors.size(); | ||
+ | Behavior b = (Behavior)behaviors.get(i); | ||
+ | b.setSuppress(true); | ||
+ | } | ||
+ | } | ||
+ | |||
+ | . | ||
+ | . | ||
+ | . | ||
+ | |||
+ | public synchronized boolean isSuppressed() | ||
+ | { | ||
+ | return suppressed; | ||
+ | } | ||
+ | |||
+ | public synchronized void setSuppress(boolean suppress) | ||
+ | { | ||
+ | suppressed = suppress; | ||
+ | } | ||
+ | |||
+ | public void forward(int period) | ||
+ | { | ||
+ | if(!isSuppressed()) { | ||
+ | ctrl.setLeftMotorOffset(0); | ||
+ | ctrl.setRightMotorOffset(0); | ||
+ | |||
+ | for(int time = 0; time < period; time += 10) { | ||
+ | try { Thread.sleep(10); | ||
+ | ctrl.setTiltAngle(-tiltPower); | ||
+ | } | ||
+ | } | ||
+ | } | ||
+ | |||
+ | public void right(int period) | ||
+ | { | ||
+ | if(!isSuppressed()) { | ||
+ | ctrl.setLeftMotorOffset(turnPower); | ||
+ | ctrl.setRightMotorOffset(-turnPower); | ||
+ | delay(period); | ||
+ | } | ||
+ | } | ||
+ | |||
+ | . | ||
+ | . | ||
+ | . | ||
+ | |||
+ | } | ||
+ | </ | ||
+ | The '' | ||
+ | ====The RandomDrive Class==== | ||
+ | This behaviour drives in one of four random directions for a random number of milliseconds. | ||
+ | All of these numbers are computed using a random function from Java Math library. | ||
+ | |||
+ | ====The AvoidFront Class==== | ||
+ | Implemented using a ultrasonic sensor. The behaviour activates a backward motion and do this for a number of milliseconds to back away from the obstacle. Afterwards Marvin stops and turns right corresponding to an approximately 90 degree turn. The code for the essential parts of the '' | ||
+ | <code java> | ||
+ | public class AvoidFront extends Behavior | ||
+ | { | ||
+ | . | ||
+ | . | ||
+ | . | ||
+ | |||
+ | public void run() | ||
+ | { | ||
+ | UltrasonicSensor us = new UltrasonicSensor(SensorPort.S2); | ||
+ | int tooCloseThreshold = 20; // cm | ||
+ | |||
+ | while (true) { | ||
+ | int distance = us.getDistance(); | ||
+ | while(distance > tooCloseThreshold) { | ||
+ | distance = us.getDistance(); | ||
+ | delay(100); | ||
+ | } | ||
+ | |||
+ | suppressLower(); | ||
+ | | ||
+ | backward(5000); | ||
+ | right(1200); | ||
+ | stop(2000); | ||
+ | |||
+ | unsuppressLower(); | ||
+ | } | ||
+ | } | ||
+ | } | ||
+ | </ | ||
+ | ====The BTController Class==== | ||
+ | The thread handles bluetooth communication in order to control Marvin with a remote control. The arrow keys on the computer is used instead of a remote. This class will be covered in detail in the next lab report. | ||
+ | |||
+ | |||
+ | The full source code for these classes can be found in the files '' | ||
+ | ======Lab report 5 - Bluetooth controlled robot====== | ||
+ | **Date:** January 19nd 2009\\ | ||
+ | **Duration of activity:** 8-16\\ | ||
+ | **Participants: | ||
+ | |||
+ | =====Project Goal===== | ||
+ | Make robot remote controlled from the PC through bluetooth. | ||
+ | |||
+ | =====Plan===== | ||
+ | * Make a bluetooth client code Behaviour on the Marvin, that can receive numbers and act on them. | ||
+ | * Make PC bluetooth application read from keyboard arrows to determine control direction. | ||
+ | |||
+ | =====Theory==== | ||
+ | ====The Protocol==== | ||
+ | {{ : | ||
+ | |||
+ | From wikipedia: //" | ||
+ | LeJOS has built an entire framework for bluetooth communication, | ||
+ | (([[http:// | ||
+ | We do not want to go further into the details of the protocol itself, but the way the device is shared can eventually end up in data loss(([[http:// | ||
+ | |||
+ | On the PC LeJOS has also made a bluetooth interface. This works exactly the same as the one on the NXT, and we therefore simply had to make a connection (listening on the NXT and connecting on the PC) and then creating input/ | ||
+ | |||
+ | LeJOS has put a lot of effort into making the bluetooth communication easy to use, and the implementation were therefore quite straight forward. Observe the resulting code below. | ||
+ | |||
+ | We experienced a lot of slowdown in the balancing thread whenever the bluetooth communication was running, and we had to disable it entirely at numerous occasions to avoid the robot falling over.\\ | ||
+ | We later discovered that the problem could be worked around by disabling some of the other behaviour threads to release more resources, and make Marvin rebalance itself while driving around remote controlled. | ||
+ | |||
+ | Here is a movie of the resulting remote controlled robot, with all other behaviours disabled:\\ | ||
+ | [[http:// | ||
+ | {{youtube> | ||
+ | |||
+ | As it can be seen from the video, Marvin is not exactly reacting real-time to the control changes. This is due to the way the BTControl has been implemented, | ||
+ | |||
+ | =====Implementation===== | ||
+ | ====The BTControl Class==== | ||
+ | {{ : | ||
+ | Bluetooth control behaviour. It reads integers from a bluetooth input stream, interpreting them as keycodes. | ||
+ | It reacts only on the keycodes connected with the arrow keys (up, down, left, right) and tells Marvin to drive accordingly. | ||
+ | |||
+ | ^ Key ^ KeyCode ^ Variable | ||
+ | | left | 37 | directionLeft | ||
+ | | right | 39 | directionRight | ||
+ | | up | 38 | directionForward | ||
+ | | down | 40 | directionBackward | | ||
+ | |||
+ | All exception handlers have been removed to improve on readability: | ||
+ | <code java> | ||
+ | public class BTController extends Behavior | ||
+ | { | ||
+ | . | ||
+ | . | ||
+ | . | ||
+ | |||
+ | public void run() | ||
+ | { | ||
+ | NXTConnection conn = Bluetooth.waitForConnection(); | ||
+ | DataInputStream istream = conn.openDataInputStream(); | ||
+ | |||
+ | while(true) { | ||
+ | if(istream.available() > 0) direction = istream.readInt(); | ||
+ | else delay(200); | ||
+ | |||
+ | if(direction >= directionLeft && direction <= directionBackward) { | ||
+ | suppressLower(); | ||
+ | switch(direction) { | ||
+ | case directionLeft: | ||
+ | left(200); | ||
+ | |||
+ | |||
+ | break; | ||
+ | |||
+ | case directionRight: | ||
+ | right(200); | ||
+ | break; | ||
+ | |||
+ | case directionForward: | ||
+ | forward(200); | ||
+ | break; | ||
+ | |||
+ | case directionBackward: | ||
+ | backward(200); | ||
+ | break; | ||
+ | } | ||
+ | unsuppressLower(); | ||
+ | |||
+ | // Empty input buffer... we only want to use the latest keystroke. | ||
+ | while(istream.available() > 1) istream.readInt(); | ||
+ | } | ||
+ | } | ||
+ | } | ||
+ | } | ||
+ | </ | ||
+ | |||
+ | ====The PCController Class==== | ||
+ | On the PC, we use a window (JFrame) to capture keyboard input and redirect their keycode values to the bluetooth stream. | ||
+ | <code java> | ||
+ | NXTComm nxtComm = NXTCommFactory.createNXTComm(NXTCommFactory.BLUETOOTH); | ||
+ | NXTInfo nxtInfo = new NXTInfo(" | ||
+ | nxtComm.open(nxtInfo); | ||
+ | |||
+ | OutputStream os = nxtComm.getOutputStream(); | ||
+ | final DataOutputStream dos = new DataOutputStream(os); | ||
+ | |||
+ | JFrame frame = ... | ||
+ | |||
+ | frame.addKeyListener(new KeyListener() { | ||
+ | public void keyPressed(KeyEvent e) { | ||
+ | dos.writeInt(e.getKeyCode()); | ||
+ | dos.flush(); | ||
+ | } | ||
+ | public void keyReleased(KeyEvent e) {} | ||
+ | public void keyTyped(KeyEvent e) {} | ||
+ | }); | ||
+ | </ | ||
+ | The full source code for both of these classes can be found in the files '' | ||
+ | |||
+ | ======Improvements====== | ||
+ | Even though we are quite pleased with the performance of Marvin, there are some issues that we would like to improve in the future if the opportunity presents itself.\\ | ||
+ | |||
+ | Marvin is able to balance and drive around randomly while avoiding obstacles. He is also able to be remotely controlled via Bluetooth. However these two behaviours do not operate well together due to resource sharing problems in the threads. We did not find a solution to this problem, but keeping them both at the same time had a high priority in the group, since it is an essential part of the behaviour architecture. The problem might be solved by changing to another framework with a different thread implementation, | ||
+ | |||
+ | We chose to use a PID controller because it had been introduced during the course, but it could be interesting to investigate some other controllers and evaluate their performance as well. The controllers of interest could be an LQR controller or/and a Pole-Placement Controller. These are mere a few amongst many. We are curios as to whether it could be possible to further optimize the PID control parameters for better balancing and robustness, or if it has been pushed to its limits whereas we would have to turn to other controller algorithms to achieve better results. | ||
+ | |||
+ | In order to further optimize the PID control parameters (or any other parameters of interest), it would be nice to have an online tuning method through a PC GUI application, | ||
+ | |||
+ | ======Conclusion====== | ||
+ | In lab 11(([[http:// | ||
+ | During the entirety of the unwinding of the project, we kept the original goals in mind, and tried to keep as close to them as possible. We did not expect, but hoped, to accomplish all of them, however contrary to our expectations, | ||
+ | \\ | ||
+ | Conclusively we consider this project a success. |
marvin/ecp_all.txt · Last modified: 2009/01/29 11:29 by 127.0.0.1