File content as of revision 6:a37a49d2793c:

#include "LSM9DS0.h"
#include "mbed.h"
#include "motordriver.h"

#define pi 3.14159265359

// SDO_XM and SDO_G are both grounded, so our addresses are:
#define LSM9DS0_XM  0x1D // Would be 0x1E if SDO_XM is LOW
#define LSM9DS0_G   0x6B // Would be 0x6A if SDO_G is LOW

Ticker tmr;
LSM9DS0 dof(p9, p10,LSM9DS0_G, LSM9DS0_XM);
Serial pc(USBTX, USBRX); // tx, rx
Motor left(p21, p22, p23, 1); // pwm, fwd, rev, has brake feature
Motor right(p26, p25, p24, 1);
InterruptIn button(p7);

DigitalOut timer_on(LED1);
DigitalOut pb(LED2);
DigitalOut ld3(LED3);
DigitalOut ld4(LED4);

#define R 0.06 //distance from the center of the robot to the wheel (radius)
#define lambda_r 22 // eigenvalue fo the regulator, chosen through tests

float umax=1; //maximum system input (motor speed)

float K=lambda_r*R; //control gain
float T=0.01; //in milliseconds

float w=0; //angular velocity
float r=0; //reference for orientation
float y=0; //current orientation
float u=0; //difference between right and left motor speeds
float u_star=0;

float vr=umax/2; //right motor speed
float vl=umax/2; //left motor speed

int c=0; //counter for measuring time
bool back=0; //180 degrees turn flag
int bumps=0; //bumps counter
int cw=-1; //turn direction flag

//Timer ISR, called with 100Hz frequency
void timer() {
    //sensor input
    dof.readGyro();
    w=(pi/15)*(dof.calcGyro(dof.gz)-dof.gbias[2]); //measure angular velocity
    y=y+T*(w-0.0000001); //integrate to obtain orientation
    
    //input calculation
    u_star=-K*(y-r);
  
    //limiting motor speed
    if (u_star<-umax){
      u=-umax;
    } else if (u_star>umax){
      u=umax;
    } else{
      u=u_star;
    }
    
    //actuator output
    vr=(umax+u)/2.0;
    vl=(umax-u)/2.0;
    left.speed(vr);
    right.speed(vl);
    
    //one second after it bumps, reference changes to -pi or zero
    if (c==100 && back){
        r=r+cw*pi/2;
        c=0;
        back=0;
        cw=-cw; //changes direction of 180 turn when bumps
        pb=0;
    }  
    
    c++;
}

void bump() {
    bumps++; //counts bumps
    tmr.detach(); //turn off timer
    ld4=1;
    
    //avoid abrupt speed changes
    left.speed(0);
    right.speed(0);
    wait(0.5);
    
    //goes backwards for 0.5s
    left.speed(-0.8);
    right.speed(-0.8);
    wait(0.5);
    
    ld4=0;  
    pb=1;
    c=0;
    
    //change reference to plus or minus pi/2
    r=r+cw*pi/2;
    
    //implementation of finding initial position
    if (bumps>2){
        ld3=1;
        back=1;
    }
    
    LPC_TIM3->TC = 0;
    tmr.attach(&timer, T); //turn timer back on
}

int main()
{
    uint16_t status = dof.begin(); //start and setup gyroscope
    dof.calLSM9DS0(dof.gbias, dof.abias); //Calculate bias
    
    wait(3);
    //starts going forward with 0.5 speed in both motors
    left.speed(vl);
    right.speed(vr);
    
    button.rise(&bump); //atach bump external interrupt
    
    //start timer with 0.01 period
    timer_on=1;
    tmr.attach(&timer, T);

    while (true){} //infinite loop
}