File content as of revision 0:3e435f58182d:

/* Maths behind the Encoder
  12 readings per revolution,   298:1 gear ratio.    3576 pulses per revoltion
  
  Note distances are based of wheel spec and so are approximations 
            Diameter of wheel = 60mm,     circumference = 188.5 mm,       distance per revolution and second is 188.5mm
            Pulses to travel 30cm = 5691
            Pulses to rotate 90 = 2700
*/

#include "mbed.h"
#include <motordriver.h>
#include "QEI.h"
#include <ros.h>
#include <std_msgs/Int32.h>

//Functions
void move(float motorA, float motorB, int distance, int pulse_direction);

//set up serial port
Serial pc(USBTX, USBRX);

// class set up
class action {
    public:
        float motorA;
        float motorB;
        int distance ;
        int pulse_direction;
};
        
// Set motorpins for driving
Motor A(PB_13, PB_15, PC_6, 1); // pwm, fwd, rev, can brake
Motor B(PB_4, PC_7, PA_15, 1); // pwm, fwd, rev, can brake
QEI wheel_A (PB_12, PA_4, NC, 3576, QEI::X4_ENCODING); //counts per rev = 12 * 298:1, use quadrature encoding
QEI wheel_B (PB_5, PB_3, NC, 3576, QEI::X4_ENCODING); //counts per rev = 12 * 298:1, use quadrature encoding

int main(){
    
    action forward{-0.5,-0.5,5691,1}; // class creation ..... edit      action forward{,,This,}       to change distance or rotation length on each
    move(forward.motorA, forward.motorB, forward.distance, forward.pulse_direction); // make the robot move
    
    action reverse{0.5,0.5,5691,-1};
    move(reverse.motorA, reverse.motorB, reverse.distance, reverse.pulse_direction); 
    
    action left{-0.5,0.5,3000,-1};
    move(left.motorA, left.motorB, left.distance, left.pulse_direction);   
    
    action right{0.5,-0.5,3000,1};
    move(right.motorA, right.motorB, right.distance, right.pulse_direction); 

}



void move(float motorA, float motorB, int distance, int pulse_direction){ 
//2 variables to allow for any pulse reading   
float initial_pulse_reading = wheel_B.getPulses();
float current_pulse_reading = wheel_B.getPulses();
// - is forward on our robot
A.speed(motorA);
B.speed(motorB);
// loop for moving forward 30cm

if (pulse_direction > 0){
    while(current_pulse_reading <= (initial_pulse_reading + distance*pulse_direction)){
        current_pulse_reading = wheel_B.getPulses();
    }
}
else{
    while(current_pulse_reading >= (initial_pulse_reading + distance*pulse_direction)){
        current_pulse_reading = wheel_B.getPulses();
    }
}        
A.speed(0);
B.speed(0);
}