Programming

CPU

We used microcontroller MSP430F2274 to control the speed of two motors motors with respect to data from our sensors (gyroscope, accelerometer) and from an external device (pc, mobile phone) via our bluetooth module.

Structure of program

The structure of the program is described by the following picture.

Click to enlarge:

figure

The controlling of the the trajectory of our robot is not the only application for which we use the Bluetooth module, it was also used for the debugging of the program and for showing the data of the sensors.

Realisation

Our natural approach was to program separate blocks that serve dedicated peripheries at first. The main purpose of this approach was to verify the functionality of the hardware. After that we programmed the angle filter and controller blocks. We decided to program these programs in the C-language instead of assembler. The code for the different blocks is shown in the sections below.


PWM generation:
//////PWM outputs - P4.1-2
#include <msp430.h>

int main(void)
{
  WDTCTL = WDTPW + WDTHOLD; // Stop WDT
  DCOCTL = 0; // Select lowest DCOx and MODx settings /clock settings
  BCSCTL1 = CALBC1_16MHZ; // Set DCO to 16 MHz
  DCOCTL = CALDCO_16MHZ;

  P4DIR |= 0x06; // P4.1 - P4.2 output
  P4SEL |= 0x06; // P4.1 - P4.2 TBx options
  TBCCR0 = 512 - 1; // PWM Period
  TBCCTL1 = OUTMOD_7; // TBCCR1 reset/set
  TBCCR1 = 384; // TBCCR1 PWM duty cycle
  TBCCTL2 = OUTMOD_7;
  TBCCR2 = 128;
  TBCTL = TBSSEL_2 + MC_1; // SMCLK, up mode optionaly---ID3 prescaler 8x
}


SPI communication:
//////test SPI communication with accelerometer (just for axis X)
//////slave select P2.0
//////led P4.4
#include <msp430.h>
#define TESTLED 0x08
#define CSACCEL 0x01
#define SPI_A_A_CTRL1 0x20
#define SPI_A_READ 0x80
void spiAccInit(void);
char data;

int main(void)
{
  WDTCTL = WDTPW + WDTHOLD; // Stop WDT
  DCOCTL = 0; // Select lowest DCOx and MODx settings
  BCSCTL1 = CALBC1_16MHZ; // Set DCO to 16 MHz
  DCOCTL = CALDCO_16MHZ;

  P4DIR |= TESTLED; // P4.4 output (in case P4.5 0x10)
  P3SEL |= 0x0C; // P3.3,2 USCI_B0 option select
  P2DIR |= CSACCEL; // P2.0 output direction

  UCB0CTL0 |= UCMSB + UCMST + UCSYNC; // 3-pin, 8-bit SPI mstr, MSB 1st
  UCB0CTL1 |= UCSSEL_2; // SMCLK
  UCB0BR0 = 0x02; //prescaled 2 times ----10MHz max communication speed for SPI
  UCB0BR1 = 0;
  UCB0CTL1 &= ~UCSWRST; // **Initialize USCI state machine** -----software reset disabled
  spiAccInit();

  while(1)
  {
    P2OUT &= ~CSACCEL; // Enable Accelerometer
    UCB0TXBUF =SPI_A_READ + 0x29; // adress 29 H byte of out_X_H + READING mode
    while (!(IFG2 & UCB0RXIFG)); // USCI_B0 RX buffer ready?
      data = UCB0RXBUF; // data = 00|DATA
    P2OUT |= CSACCEL; // Disable Acceloremoeter
    P4OUT &= ~TESTLED; // P4.4 = 0
    if (data >= 0x7F) // data = AIN > 0.5(REF+ - REF-)?
    P4OUT |= TESTLED; // P4.4 = 1
  }
}

void spiAccInit(void){
  P2OUT &= ~CSACCEL; // Enable Accelerometer
  UCB0TXBUF = SPI_A_A_CTRL1;
  while (!(IFG2 & UCB0TXIFG)); //make sure that first word is away
  UCB0TXBUF = 0x39; // 0b00111001 greater data rate 1000Hz only axis X enabled
  while (!(IFG2 & UCB0TXIFG)); //make sure that first word is away
}


UART communication:
//////P3.4/UCA0TXD
//////19200 - 8N1
//////P3.5/UCA0RXD
#include <msp430.h>

int main(void)
{
  WDTCTL = WDTPW + WDTHOLD; // Stop WDT
  DCOCTL = 0; // Select lowest DCOx and MODx settings
  BCSCTL1 = CALBC1_16MHZ; // Set DCO to 16 MHz
  DCOCTL = CALDCO_16MHZ;

  P3SEL = 0x30; // P3.4,5 = USCI_A0 TXD/RXD
  UCA0CTL1 |= UCSSEL_2; // SMCLK
  UCA0BR0 = 65; // 16MHz 19200
  UCA0BR1 = 3; // 16MHz 19200
  UCA0MCTL = UCBRS0; // Modulation UCBRSx = 1
  UCA0CTL1 &= ~UCSWRST; // **Initialize USCI state machine**
  IE2 |= UCA0RXIE; // Enable USCI_A0 RX interrupt

  __bis_SR_register(LPM0_bits + GIE); // Enter LPM0, interrupts enabled }
// Echo back RXed character, confirm TX buffer is ready first
#pragma vector=USCIAB0RX_VECTOR
__interrupt void USCI0RX_ISR(void)
{
  while (!(IFG2 & UCA0TXIFG)); // USCI_A0 TX buffer ready?
  UCA0TXBUF = UCA0RXBUF; // TX -> RXed character
}

Control Algorithms

We implemented a fuzzy control strategy to balance our Wheeled Inverted Pendulum. We use the angle and the angular velocity together with the shape functions and control rules (see figures below) to produce an output.

figure

Complementary filter

The fuzzy control algorithm uses the angle and the angular velocity as input parameters. To get an accurate estimation of the angle and angular velocity of our Wheeled Inverted Pendulum, we need to filter the data from our sensors. Our IMU (inertial measuring unit) contains a gyroscope and an accelerometer. The accelerometer senses the angle (using 2-axis method), while the gyroscope senses the angular velocity. However, the readings from these sensors can contain alot of noise. We try to reduce the noise using the simple complementary filter, shown in the figure below:

figure

An alternative for the complementary filter is famous Kalman filter. We prefered to use this filter, because of it's simplicity and no need for complex mathematical models needed by a Kalman filter. The complementary filter uses the noisy data of both sensors and combines them in a way to provide a better estimation of the angle.

As can be seen in the picture above, the angle output from the accelerometer is filter by a low-pass filter, so only long-term changes have an effect on the filtered angle, thus filtering out short-term fluctuations. This is to filter out the movement acceleration of the robot itself, which also gets picked up by the accelerometer.

The accelerometer data is used directly to estimate the angular velocity. However, it is also used to correct the angle measured by the accelerometer, by integrating the angular velocity. The integrated value is passed trough a high-pass filter, so only short-lasting fluctuationgs have an effect.

Matlab aplication

We also developed a grafical user interface. The first purpose was to better identify our system, to improve the control agoritm. The second purpose of this graphical aplication is that it may work as a simple joystick for controlling the trajectory of our robot.

Click to enlarge:

figure

This GUI is based on the following code. It reads messages of 12 bytes from the serial port where the Bluetooth module is connected, identifies if the message is correct with the use of a header and a tail for the message and plots the values from the message to a chart. The code for the GUI is shown bellow:

Open serial port:
sport=serial('COM9','baudrate',9600);
fopen(sport)


Plotting part:
tail=[1,16,1,16]';
head=[240,15,240,15]';
tic
while sport.BytesAvailable
  message=fread(sport, messagelength);
  if (message(1:4,1)==head(:,1))&(message(17:end,1)==tail(:,1))
    signalvector=message(5:16,1);
  else
    signalvector=zeros(12,1); %consider taking of last sample
    for i=1:messagelength-4
      if(message(i:i+3,1)==head(:,1))
        fread(sport,messagelength+i-1)
        break
      end
      if(message(i:i+3,1)==tail(:,1)) %synchronise
        fread(sport,i+4-1);
        break
      end
    end
  end
  plotsignals=[plotsignals(:,2:end) signalvector];
  timeaxis=[timeaxis(:,2:end) toc];
  plot(timeaxis', [plotsignals; Availablebits]')
  axis([timeaxis(1,1) timeaxis(1,end) 0 3000])
  drawnow
end


Close serial port:
fclose(sport)
delete(sport)

Android application

We also designed a simple java aplication for the control of the trajectory of our robot. This application allows the same controlling functionality as the Matlab graphical user interface. For developing we used standart Android libraries for bluetooth communication android.bluetooth and android.hardware for mobile device accelerometer sensors.

Like in Matlab GUI this aplication sends single control character.

Symbol
Meaning
F
Forward
B
Back
L
left
R
Right
S
Stop

Unlike in matlab code it is not necessery to push buttons for control. Control character is send when value of mobile device accelerometer (pose of mobile phone) overcome given treshold. It is also possible to write symbol manually. It is usable for next developement.

Nevertheless at this moment the application provides very rough control. Future improvement is possible and desired.

Click to enlarge:

figure