## Embedded Programming

Wrapping up the BLUEtongue 2.0 Rover’s Drive System series, following articles by Chris about the mechanical re-design of the system and Harry about the high-level software implementation, this article will outline the role of the embedded system in connecting the electrical motors to the high-level software. Primarily, this article will focus on analogue-to-digital converters and their use in the drive system of BLUEtongue 2.0. Some understanding of electrical circuits and microprocessors is assumed in the following explanations.

Analog-to-Digital Converters (ADCs) are a cornerstone of signal processing, and are used in nearly all electrical devices today. The objective of an ADC is to convert an analog voltage signal into a digital representation. Various methods exist in implementing an ADC, each having their own benefits and purpose. To provide a comparison there are two key concepts when analysing ADC methods, that being their speed and their cost.

The speed of an ADC reflects how fast a sample-conversion sequence is performed and is most often measured as ‘how many sample-conversions can be done within a certain time-frame’ (in samples per second). A higher speed is of course useful when high bandwidth is needed. On the other hand, cost describes how expensive it is to implement – as well as improve the resolution – of the ADC and is influenced by the complexity and number of hardware components required in the design.

Typical ADC methods demonstrate that an increase in speed of the solution will cause an increase in the cost. This is indicative of the trade-off between parallel and sequential logic, as parallel logic will be faster but will require more hardware components. For this article, I will give a brief outline of 3 key ADC solutions:

• The first method that will be addressed is the Dual Slope, also known as the Integrating method. This method works by charging up an integrator circuit by the voltage being sampled for a fixed amount of time, then discharging the same circuit at a known reference voltage back to no-charge. By using a counter to track time for the discharge phase, the circuit is able to accurately derive a digital equivalent of the original analogue signal using $Latex formula$. Due to requiring the charging and discharging of a capacitor, this method is one of the slower methods used but also not very costly.
• Next is Successive Approximation (SAR), which, as the name suggests, operates by estimating the digital output by testing each bit in the final representation progressively from the MSB to the LSB. At each step, it sets the current ‘result’ bit to HIGH (bit = ‘1’), performs a Digital-to-Analog conversion (DAC) and checks whether the analog equivalent is greater than the sampled analog voltage, setting the bit back to LOW (bit = ‘0’) if true then moves on to the next MSB. Doing this ensures that the resulting digital value is the closest binary representation that is still less than the sampled voltage. This method’s speed is typically faster than the ramping method, but has a greater cost as a payoff due to the more complex circuitry.
• The last method mentioned is Flash ADC, one of the fastest ADC methods. A flash ADC is a group of parallel comparators which individually check the input voltage against reference voltage for all possible digital outputs and then uses a priority encoder to select the appropriate binary result. The cost of this method is the largest of the three as it requires enough components to perform all these voltage comparisons in parallel.

In addition to the methods described here, there are also interesting ADC solutions such as the Sigma-Delta, but we will leave that for the reader to explore.

### ADCs on BLUEtongue

One of the primary uses of the ADC on BLUEtongue was to implement the swerve drive system. To ensure the system’s functionality, it was important for the real-time wheel headings to be known as accurately as possible. To achieve this, potentiometers (pots) were integrated into the front two shafts of the wheel rotators and fed back to the control board, where the analog read-out of the pot was converted into a digital signal that was then passed through to the on-board computer via USB.

In addition to the swerve drive, ADCs were also used in feedback systems for arm manipulation.

### ADC on the PIC

For BLUEtongue v2.0 the control board consisted of a custom made PCB, housing the dsPIC33EP512MC806 microprocessor (PIC) from Microchip (Read more here). The ADC on the PIC used for the rover is an implementation of the SAR system, with a few additional features.

The PIC provided two independent SAR modules, the first module (ADC0) was able to operate in 12-bit resolution with one channel S&H (Sample and Hold, where the analogue input is captured for the length of the conversion) if desired, whilst both are able to operate at 10-bit resolution with 4 channel S&H.

The resulting conversions were stored in a dedicated 16×16-bit buffer (one buffer for each ADC module exists) allowing for convenient access upon completion. Furthermore, to signify that a conversion sequence has been performed, the PIC is able to generate interrupts or, alternatively, set a ‘done’ bit/flag. The former is useful for time-sensitive, synchronous data whilst the latter (which would be implemented through a form of polling) is less time-critical and better for asynchronous conversions.

For the purpose of the swerve drive, we implemented ADC1 in 12-bit resolution and used the ‘Channel Scan Select’ feature (which allowed the module to sequentially scan multiple ADC pins) to allow the best resolution possible whilst also providing the conversion requirements for the multiple data feedback sources. Furthermore, we used the interrupt method as feedback data for the swerve system constituted an urgent situation.

### Programming the PIC

The following code demonstrates how the ADC was setup on the PIC.

```// ** Code to setup adc for reading potentiometers ** //
// ** Uses the input scan select system to allow reading ** //
// ** of multiple analog inputs within a single module ** //

// Set appropriate pins as inputs (to read from the pots)
TRISBbits.TRISB8 = 1;
TRISBbits.TRISB10 = 1;
TRISBbits.TRISB12 = 1;
TRISBbits.TRISB15 = 1;
TRISEbits.TRISE0 = 1;
TRISEbits.TRISE1 = 1;
TRISEbits.TRISE2 = 1;
TRISEbits.TRISE3 = 1;

// Setup the pins to read analog values
ANSELBbits.ANSB8 = 1;
ANSELBbits.ANSB10 = 1;
ANSELBbits.ANSB12 = 1;
ANSELBbits.ANSB15 = 1;
ANSELEbits.ANSE0 = 1;
ANSELEbits.ANSE1 = 1;
ANSELEbits.ANSE2 = 1;
ANSELEbits.ANSE3 = 1;

// Set the control registers to zero, these contain garbage after a reset
// This also ensures the ADC module is OFF

// clear ADC1 control registers: CON4, CHS0, CHS123 and CHSSH/L

// *** CLOCK SETTINGS *** //
//Changes the ADC module clock period for both conversion ad sampling.
// Tad must be greater than 117.6 ns (electrical specs, pg560), T_CY is 1/70Mhz
// Tad T_CY * (ADCS + 1)
// ADCS (117.6*10^-9)*(70*10^6) - 1
// ADCS 7.232 ~ 8

AD1CON3bits.SAMC = 0x1F; // Sampling for TAD * 14 (as required for 12-bit)

// Auto-sampling, automatically end sampling and begin conversion

// Select the pins that will be cycled through via input scan select
// NOTE: The ADC scans in ascending order of analog number, i.e.
// if connecting an4, 9, 5, 12 the buffer will be filled:
// 4, 5, 9, 12. Ensure any changes enforce this convention!
AD1CON2bits.CSCNA = 1; // Activate channel scan select

// Will need to interrupt after (N-1) sample/conversion sequences.
// Where N = number of signals being read (e.g. an16 an24 = 2 signals = SMPI = 1)
AD1CON2bits.SMPI = 7; //interrupt on sample conversion

//automatically begin sampling whenever last conversion finishes, SAMP bit will be set automatically

// Clear interupt flag, set interrupt priority

// Enable the interupt

}

// ADC interrupt serve routine (ISR). This sets a variable so that the main function
// knows that a conversion has finished and can read from buffer.
void __attribute__((__interrupt__, no_auto_psv)) _AD1Interrupt(void) {
}

```

` `

### Conclusion

Going forward, the Off-World Robotics team will continue to develop and expand its use of signal processing with the aid of ADCs for the drive system, as well as other key systems such as the fine control of the arm. The experience gained from programming on the microprocessor and implementing the ADCs has been very rewarding for me. The knowledge will also prove invaluable to the team as we look to enhance the embedded system for the next iteration of the rover, code-named NUMBAT, with a Controller Area Network ( will appear in a future article!). I hope you have enjoyed this write-up and found the series informative.

To view the entire embedded system repo, click here.

Thank you for reading, to keep up to date with BLUEsat and the Rover, like us on Facebook and stay tuned for more posts on this site. If you are interested in getting involved, you can find more here.

## Bluetounge 2.0: Steering and Joint Control Software

##### Posted on January 4th, 2017 by Harry J.E Day

Welcome back to the second article in our three part series on the BLUEtounge 2.0 Rover’s suspension and drive system. In our last post Chris wrote about the mechanical re-design of the system, and in this post we will look at how we designed the high level software architecture for this system. We will also look at some of the challenges we faced along the way.

## The System

The BLUEtounge 2.0 Rover has four independently controlled wheels, with the front two wheels also being able to steer. This was a big departure from BLUEtounge 1.0’s skid steer system, which used six wheels, and turned by having the wheels on one side of the rover spin in the opposite direction to those on the other side of the rover. The system was meant as a stepping stone towards a full swerve drive system on either BLUEtounge, or our next rover platform NUMBAT.

Furthermore the BLUEsat Off-World Robotics code base is based around the R.O.S (Robotics Operating System) framework. This framework provides a range of existing software and hardware integrations, and is based around the idea of many separate processes (referred to as nodes), that communicate over TCP based ROS ‘topics’ using data structures called ‘messages’.

That, along with the nature of BLUEsat as a student society placed some interesting requirements on our design:

• The system needed to be able to work with only two wheel modules being able to steer, but as much as possible the code needed to be reusable for a system with four such modules.
• The system needed to avoid being heavily tied to the motors and embedded systems used on BLUEtounge, as many of them would be changing for NUMBAT.
• Due to European Rover Challenge (ERC) requirements, the system needed to support user input, and be able to be controlled by an AI.

As a consequence of the above, and to avoid reinventing the wheel (no pun intended), the system needed to use standard ROS messages and conventions as much as possible. It also needed to be very modular to improve reusability.

## User Input

The user controls the rover’s speed and rotation using an xbox controller. After some investigation, our initial approach was to have one of the analogue sticks control the rover’s direction, whilst the other controlled its speed. This was primarily because we had found that using a single stick to control direction and speed was not very intuitive for the user.

As ROS joystick analogue inputs are treated as a range between -1 and 1 on two axes, the first version of the system simply used the up/down axis of the left stick as the magnitude applied to a unit vector formed by the position of right stick. The code looked a bit like this:

```double magnitude = joy->axes[SPEED_STICK] * SPEED_CAP;
cmdVel.linear.x = joy->axes[DIRECTION_STICK_X] * magnitude;
cmdVel.linear.y = joy->axes[DIRECTION_STICK_Y] * magnitude * -1;
```

(Note: that all code in this article uses the ROS standard of x being forwards <-> backwards, and y being port <-> starboard)

This code produced a geometry_msgs::Twist message that was used by our steering system. However we found that this system had several problems:

• It was very difficult to do fine manoeuvring of the rover, because the range of slow speeds corresponded to too small an area on the joystick. However, since we could only control the power rather than the velocity of the motors, we couldn’t simply reduce the overall power of the rover as this would mean it was unable to traverse steep gradients.
• Physical deadzones on the joysticks meant that driving the rover could be somewhat jerky.
• The code above had a mathematical problem, where the rover’s max speed was higher whilst steering than could be achieved travelling in a straight line.
• Having a two axis direction control was unintuitive for the driver, and hard to control accurately.

In response to this one of our team members (Sean Thompson) developed a new control system that used only one axis for each stick. In this system the left stick was used for power, whilst the right stick was used for (port/starboard) steering.  The system also implemented dead zone and exponential scaling which allowed for better manoeuvring of the rover at low speeds, whilst still being able to utilise the rover’s full power.

Full source code for this implementation can be found here.

## Steering

The steering system for the rover allows the rover to rotate about a point on the line formed between the two rear wheels. In order to achieve this, each wheel must run at a separate speed and the two front wheels must have separate angles. The formulas used to determine these variables are displayed below.

$Latex formula$$Latex formula$

In order to accommodate this a software module was built that converted the velocity vector ($Latex formula$) discussed in the previous section, into the rotational velocities required for each of the wheel modules, and the angles needed for the front two wheels. The system would publish these values as ros messages in a form compatible with the standard ros_command module, enabling easier testing in ROS’s gazebo simulator and hopefully good compatibility with other ROS systems we might need to use in the future.

The following code was used to implement these equations:

```        const double turnAngle = atan2(velMsg->linear.y,velMsg->linear.x);
const double rotationRadius = HALF_ROVER_WIDTH_X/sin(turnAngle);

// we calculate the point about which the rover will rotate
// relative to the centre of our base_link transform (0,0 is the centre of the rover)

geometry_msgs::Vector3 rotationCentre;
// the x axis is in line with the rear wheels of the rover, as shown in the above diagram
rotationCentre.x = -HALF_ROVER_WIDTH_X;
// and the y position can be calculated by applying Pythagoras to the rotational radius of the rover (r_turn) and
// half the length of the rover
// omega_rover is then calculated by the magnitude of our velocity vector over the rotational radius
const double angularVelocity = fabs(sqrt(pow(velMsg->linear.x, 2) + pow(velMsg->linear.y, 2))) / rotationRadius;

//calculate the radiuses of each wheel about the rotation center
//NOTE: if necessary this could be optimised
double closeBackR = fabs(rotationCentre.y - ROVER_CENTRE_2_WHEEL_Y);
double farBackR = fabs(rotationCentre.y + ROVER_CENTRE_2_WHEEL_Y);
double closeFrontR = sqrt(pow(closeBackR,2) + pow(FRONT_W_2_BACK_W_X,2));
double farFrontR = sqrt(pow(farBackR,2) + pow(FRONT_W_2_BACK_W_X,2));

//V = wr
double closeBackV = closeBackR * angularVelocity;
double farBackV = farBackR * angularVelocity;
double closeFrontV = closeFrontR * angularVelocity;
double farFrontV = farFrontR * angularVelocity;

//work out the front wheel angles
double closeFrontAng = DEG90-atan2(closeBackR,FRONT_W_2_BACK_W_X);
double farFrontAng = DEG90-atan2(farBackR,FRONT_W_2_BACK_W_X);

//if we are in reverse, we just want to go round the same circle in the opposite direction
if(velMsg->linear.x < 0) {
//flip all the motorVs
closeFrontV *=-1.0;
farFrontV *=-1.0;
farBackV *=-1.0;
closeBackV *=-1.0;
}

//finally we flip the values if we want the rotational centre to be on the other side of the rover
if(0 <= turnAngle && turnAngle <= M_PI) {
output.frontLeftMotorV = closeFrontV;
output.backLeftMotorV = closeBackV;
output.frontRightMotorV = farFrontV;
output.backRightMotorV = farBackV;
output.frontLeftAng = closeFrontAng;
output.frontRightAng = farFrontAng;
ROS_INFO("right");
} else {
output.frontRightMotorV = -closeFrontV;
output.backRightMotorV = -closeBackV;
output.frontLeftMotorV = -farFrontV;
output.backLeftMotorV = -farBackV;
output.frontLeftAng = -farFrontAng;
output.frontRightAng = -closeFrontAng;
ROS_INFO("left");
}
```

Separating steering from the control of individual joints also had another important advantage, in that it significantly improved the testability and ease of calibration of the rover’s systems. Steering code could be tested to some extent in the gazebo simulator using existing plugins, whilst control of individual joints could be tested without the additional layer of abstraction provided by the steering system. It also allowed the joints to be calibrated in software (more on this in our next article).

## Joint Control System

In BLUEtounge 1.0, our joint control system consisted of many lines of duplicated code in the main loop of our serial driver node. This code took incoming joystick messages and converted them directly into pwm values to be sent through our embedded systems to the motors. This code was developed rapidly and was quite difficult to maintain, but with the addition of the feedback loops needed to develop our swerve drive, the need to provide valid transforms for 3d and automation purposes, and our desire to write code that could be easily moved to NUMBAT – a new solution was needed.

We took an object oriented approach to solving this problem. First a common JointController class was defined, this would be an abstract class that handled subscribing to the joints control topic, calling the joints update functions and providing a standard interface for use by our hardware driver (BoardControl in the diagram below) and transform publisher (part of JointsMonitor).  This class would be inherited by classes for each type of joint, where the control loop for that joint type could be implemented (For example the drive motors control algorithm was implemented in JointVelocityController, whilst the swerve motors where implemented in JointSpeedBasedPositionController).

In addition a JointMonitor class was implemented, this class stored a list of joints and published debugging and transform information at set increments. This was a significant improvement in readability from our previous ROS_INFO based system as it allowed us to quickly monitor the joints we wanted. The main grunt of this class was done in the endCycle function, which was called after the commands had been sent to the embedded system. It looked like this:

```// the function takes in the time the data was last updated by the embedded system
// we treat this as the end of the cycle
void JointsMonitor::endCycle(ros::Time endTime) {
cycleEnd = endTime;
owr_messages::board statusMsg;
ros::Time estimateTime = endTime;
int i,j;
// currentStateMessage is a transform message, we publish of all the joints
currentStateMessage.velocity.resize(joints.size());
currentStateMessage.position.resize(joints.size());
currentStateMessage.effort.resize(joints.size());
currentStateMessage.name.resize(joints.size());

// we look through each joint and estimate its transform for a few intervals in the future
// this improves our accuracy as our embedded system didn't update fast enough
for(i =0; i < numEstimates; i++, estimateTime+=updateInterval) {
j =0;
for(std::vector<JointController*>::iterator it = joints.begin(); it != joints.end(); ++it, j++) {
jointInfo info = (*it)->extrapolateStatus(cycleStart, estimateTime);
publish_joint(info.jointName, info.position, info.velocity, info.effort, j);

}
statesPub.publish(currentStateMessage);
}
// we also publish debugging information for each joint
// this tells the operator where we think the joint is
// how fast we think it is moving what PWM value we want it to be at.
for(std::vector<JointController*>::iterator it = joints.begin(); it != joints.end(); ++it, j++) {
jointInfo info = (*it)-&amp;amp;gt;extrapolateStatus(cycleStart, endTime);
owr_messages::pwm pwmMsg;
pwmMsg.joint = info.jointName;
pwmMsg.pwm = info.pwm;
pwmMsg.currentVel = info.velocity;
pwmMsg.currentPos = info.position;
pwmMsg.targetPos = info.targetPos;
statusMsg.joints.push_back(pwmMsg);

}
debugPub.publish(statusMsg);

}
```

Overall this system proved to be extremely useful, it allowed us to easily adjust code for all motors of a given type and reuse code when new components where added. In addition the standardised interface allowed us to quickly debug problems (of which there where many), and easily add new functionality. One instance where this came in handy was with our lidar gimbal, the initial code to control this joint was designed to be used by our autonomous navigation system, but we discovered for some tasks it was extremely useful to mount a camera on top and use the gimbal to control the angle of the camera. Due to the existing standard interface it was easy to add code to our joystick system to enable this, and we didn’t need to make any major changes to our main loop which would have been risky that close to the competition.

## Conclusion

Whilst time consuming to implement and somewhat complex this system enabled us to have a much more manageable code base. This was achieved by splitting the code into separate ROS nodes that supported standard interfaces, and using an OO model for implementing our joint control. As a result it is likely that this system will be used on our next rover (NUMBAT), even though the underlying hardware and the way we communicate with our embedded systems will be changing significantly.

Next in this series you will hear from Simon Ireland on the embedded systems we needed to develop to get position feedback for a number of these joints, and some of the problems we faced.

Code in this article was developed for BLUEsat UNSW with contributions from Harry J.E Day, Simon Ireland and Sean Thompson, based on the BLUEtounge 1.0 steering and control code by Steph McArthur, Harry J.E Day, and Sam Scheding. Additional assistance in review and algorithm design was provided by Chris Squire, Chris Miller, Yiwei Han, Helena Kertesz, and Sebastian Holzapfel. Full source code for the BLUEtounge 2.0 rover as deployed at the European Rover Challenge 2016, as well as a full list of contributors, can be found on github.

## BLUEtongue 2.0: Rover Suspension Re-Design

##### Posted on December 21st, 2016 by Chris Miller

This is the first part in a small three-part series about the re-design of the rover suspension. We’ll touch on aspects across several parts of the team, but for now I’ll introduce you to the mechanical aspects.

However, before I talk about this re-design, I feel it necessary to explain why such substantial change was needed. When we first began the design of BLUEtongue back in 2013 the team opted for a Rocker-Bogie style of suspension due to it its many benefits in traction and stability when operating in rocky environments.

Resulting from the complexity and cost attached to steerable wheels (such as swerve drives), we utilised skid steering like that you’ll find on a tank or bobcat. Unfortunately, to a significant extent, we misunderstood the physical nature of the suspension we were in the process of designing and the ramifications our choice to peruse skid steering would have. Upon initial testing, the inherent problems in the system began to make themselves known. First, the suspension was too tall and insufficiently rigid for a skid steering design. Due to this, attempts to turn the rover resulted in either the flexure of the structure or would cause the bogie to “kick”, rendering the rover immobile. You may see older photos of the rover with what we’ve called “bracing bars”.

These bars lock the bogie to the rocker, permitting limited steering capability and allowing the rover to limp around. Secondly, the rocker was too long and couldn’t fit in conventional luggage. As we’d planned from the start to flat pack the rover into our personal luggage for transit to and from the contest, we had to search long and hard to find a suitable enclosure. Thirdly, the construction order. As many undergraduates will quickly realise when they build things for the first time, build order is a very important thing to consider. Whilst in a Computer Aided Design (CAD) environment, assembly really is as easy as a few clicks. Need to mount a motor in tight spot? Sure! Try to do this in physical space where motors can’t fly through walls? Not so easy. Due to this, our assembly process was very convoluted, requiring gearboxes to be adjoined to the motors within other structures, and removed for disassembly, etc (It was a nightmare!). All in all, our first suspension iteration was an utter nightmare. Hindsight really is 20/20.

So, now that we’re on the same page as to the why, I want to introduce you to the what. Post our first presence at the European Rover Challenge in 2015, we realised the suspension was one of the key limiting factors of the BLUEtongue rover platform. With the knowledge that a fundamental redesign was needed, we got to work over the next few months. The final design is a parallel swing arm type suspension with a full rotation swerve drive. The new system was designed with a heavy focus on steering, dynamic response, assembly and transport.

As seen in the video attached below, steering is achieved through the actuation of a radially free, but axially constrained, shaft. Due to the low loads experienced and limited rotation speeds, this arrangement is achieved with radial bearings and circlips. The design originally called for the use of a swivelling hub (really just a small scale Lazy Susan) for the axial restoring force. However, during initial testing, these were negated to allow for power cabling to pass through the shaft centres. Here it quickly became evident these hubs were unnecessary. Luckily so as well, as this topside location was later used to mount analogue potentiometers for feedback once it was established that the intended locating method of relative encoders and magnetically activated homing was insufficient (Stay tuned for our next two articles for more on this). In order to drive the shaft, a DC motor with gearhead was mounted parallel, and an addition reduction gear step used to mechanically link the two. Additional problems arose from this arrangement where the torque loading during operation consistently began to “strip” the lock screw of the brass pinion gear, leading to un-actuated free rotation of the shaft. A problem easily solved through the use of thicker walled Carbon Steel (1045 for anyone interested) replacement pinion gears.

Coupled with the problem of rover steering is the dynamic response. Due to time pressures, we were unable to properly characterise the design to validate our solution. As a result, we opted to take a leaf from the hobbyist’s books and use shock absorbers designed for large scale RC cars. Whilst a little smaller than ideal, the readily available variety of damping fluids and compression springs allowed for on the fly adaptation and variability. This allowed us to tailor the dynamics of the system to those desirable for the rover. This design will serve as a starting point to aid in verification of analytical and numerical modelling, laying the foundation for our upcoming NUMBAT rover. I’ve included some slow motion video for you to enjoy, it’ll give you a good idea of how the suspension operates under an impulse loading. Watch this space for future posts about this kind of thing, we’ll be revisiting this later (eventually…).

I’m not going to dive too deep into the remaining points on assembly and transport as they deal more with how you design something opposed to what you’re designing. Our main objective here was to decouple mounting arrangements such that subassemblies can be shipped separately and then joined with minimal effort. If you take a look at the suspension, it can be boiled down into three main parts. The suspension subassembly, the rotation subassembly and the wheel subassembly. When mated, these for a completed suspension and drive assembly that can then easily be joined to the rover chassis. All-in-all we only need to insert or remove a total of nine screws to join or remove each suspension unit. A major improvement over the Rocker-Bogie, which would require a complete disassembly of both the wheel and suspension structures. (Lessons learned)

Thank you for reading, like us on Facebook or stay tuned here for more articles, and feel free to get involved with the project if this grabbed your interest. You can find more about joining here.